looks pretty beefy… so my question is… if I put a rubber glove on it to protect it from mother nature does it have enough strength and lifting torque to pull the pine needles and gook out of the gutters on my house (that are like 3 stories off the ground)? 
It sure is strong enough to close or open the curtains…
Yepp tried that once… 
LOL A remote control mobile curtain closer! 8)
Yep the arm does have some strength. I am currently writing the code to locate an object within the sensor range and then position the arm to it…
With the current code using the PS2 to control the arm, when you press the start button it turns the arm on or off and snaps it to some initial position. I know that it has some power here as it snapped my servo horn with the sensor right off of the servo as you can see in this picture:
http://i416.photobucket.com/albums/pp245/Kurts_Robots/DSC03275.jpg.
The bottom of the arm even with the longer brackets and tubing can still easily catch up on the top of the sensor, so I may need to try a few different things. The easiest may be to turn the hand over and have the servo on top instead of on the bottom. The other may be to try to have a smarter go to init position code that the first step is made to make sure it clears the sensor. May play with this some. But first for a new servo horn…
Kurt
WOW! You really have to be more careful with your bots! 
Just kidding. A good thing it’s just the horn. I had the two collapsing in to each other a couple of times while testing. But nothing broke down over here.
This is one of the reasons I’ve mounted the sensor more down. The other reason was that it can find smaller objects.
Xan
I know, I know, I need to be more careful 
I definately need to change the sensor mount. I believe the arm on your rover is maybe .5 inches longer with the light weight wrist rotate where mine has the heavy duty version. The arm and sensor touch each other reasonably often and with the InitArm speed of 600 it can do it with a bang.
Last night I was looking at maybe changing the InitArm into a 2 position sequence where the first move gets the arm out of the danger zone, before it parks it. Maybe it does it conditionally (first time as we don’t know where the arm is when we begin and then later if we think we are positioned anywhere near where the two could bang.
I will take your suggestion about seeing if I can move the sensor down. It is currently mounted in a multi-purpose sensor housing which sticks up. I don’t want to turn it over as then the housing will hit the servo. May try some other way to attach servo to housing.
Kurt
Hi Kurt,
I think that making a 2 step movement will be a good solution. I’m doing the same thing when turning off the phoenix in V2.0.
- Place it slowly back on the ground
- Free all servos
I used a C bracket with a Gripper attachment plate. It’s not needed with the wrist rotate upgrade so I had one spare.
Can’t find the separate plate but here is a picture from the assembly tutorial, although it’s mounted in a 90 deg angle.
Some large sensor housing-brackets would be great for this in the future. What do you think Jim?
Xan
I now have it hooked up with a similar setup, I used a large C bracket with a piece of lexan that was looks similar to the mount. Still want to play with it as it still can interfere with the arm… Thought of trying a short C bracket but not enough movement.
But in the mean time I have been playing with a new IR scanner function, that will scan until it finds an object with a max distance. It will then try to determine the two edges and aim at the center. Now assuming an object is found also probably plus some other checks like the width of the object is within some prescribed range I would like to automatically move the arm to that location and them maybe try to pick it up…
But now I first need to figure out my Math and the appropriate cooridinate system. First I will try to explain it without a diagram, to see if I can explain it well enough and take a stab at the equations.
First assumption of coordinate system. X=distance fore and aft from rover. Y is the UP and down of the arm and Z = direction 90 degrees off of where tires can roll…
We have a couple of fixed deltas
dxArmToSensorServo: ~8cm
dyArmToSensorServo:~8cm
LSensor arm: ~5cm
So if we do a ping at some angle (A) and it returns some value (D) in cm, the actual distance to the servo is (D+5).
The X and Z location of the object from the servo is:
Xs= cos(A)(D+LSensorArm)
Zs= sin(A)(D+LSensorArm)
Then we need to convert this to arm coordiates.
Xa=Xs+dxArmToSensorServo
Za=Zs
Ya= ? (will probably choose a value - maybe sensor Y position)
What I need to figure out next is how to position the arm here. At first I thought I would just need to pass these coordinates into the ArmIK functions. but I noticed that Z is not really used/generated here.
What I am suspecting is that the calculations are more or less 2D and that is the coordiate system is not based on the Rover, but on the direction the arm is rotatated at the time. Does this make sense?
If so does that imply my real XA is:
XA = sqrt(XaXa + ZaZa)
YA stays the same.
And now I would need to calculate the desired base angle. Probably need to use ArcCos and ArcTan of these values? I sure am rusty at this!
Kurt
Hi Kurt,
Sorry I didn’t answer this post yet. I’m a bit busy with family stuff at the moment. I’ll help you out with the math as soon as I’ve got the time.
BTW: could you post a picture of your setup? I would like to see how far the long bracket sticks out.
Xan
Hi Xan,
I hope things go Ok with the family stuff.
No hurry, I think I understand most of it, but will know more when I finish putting in the first pass of the code and see everything I thought was wrong
Here is poor picture showing how far the sensor sticks out. The rover is turned off so the arm is simply resting.
http://i416.photobucket.com/albums/pp245/Kurts_Robots/DSC03277.jpg
Hi Xan,
A slight update:
I wrote a scan function that scans back and forth with limits of +60 and -60 degrees until the IR scanner finds an object that is within some distance. This is currently a maximum of 25cm from the front of the sensor. I keep scanning in the current direction to find an edge, go back half way between where I found it first and the edge and scan back in the opposit direction. I then take the mid point. This is now working reasonably OK. There are times when the final ping does not give me the distance I expect, but I will track that down. During this scanning I have the option of dumping some data to the S_OUT (currently using the square button to turn on debug output or not). Some of the debug information that I have currently during the scan looks like:
IR in cm: 13 Angle:340
IR in cm: 14 Angle:344
IR in cm: 13 Angle:352
IR in cm: 15 Angle:356
IR in cm: 16 Angle:388
LocObj findedge state:4 Left:240 Right: 388 dir: 1 Obj Angle: 340 D: 13
IR in cm: 22 Angle:364
LocObj findedge state:0 Left:364 Right: 388 dir: 0 Obj Angle: 340 D: 13[/code]
Note angles are tenths of a degree.
After both edges are found, I take the distance from the sensor add on the length of the sensor arm to get the distance from the sensor servo. I then convert this distance and the sensor angle into X,Y,Z coordinates and I add the delta X from the sensor servo to the center of arm base. So far I have not done anything with the Y delta...
Some debug output from the above trace:
[code]
LocObj: IR in cm: 16 Angle:376 DServo mm: 220 BAP angle:27 OX: 248 OY:80 OZ:134
[/code]
In the above I dumped some more data then was necessary including the value I needed to pass the the BAP sin/cos function (0-255). The Ox, Oy, Oz are the coordinates in 3D space to the object.
Next I think the ARM code on the Rover is more or less 2D once you rotate the base to a desired angle. So I have the code to convert the 3D above to a base angle, as well as the distance from the arm base to the object. I think this in the Arm code would be near the WristX in the calculations. Here is a debug output of that calculation:
[code]
IKArm3d X: 248 Y: 80 Z:134->dist=281 Angle=283
[/code]
Again X,Ys are in mm and angle is in tenths of a degree.
So far that is as far as I have gotten. Next is I want to position the arm to this location. I need to reorganize the code a little more to allow me to set the arm position to these coordinates without interference from the PS2. But I first need to figure out what the current arm code is expecting for coordinates: In particular this code:
[code] ;Inverse Kinematic calculations
GOSUB ArmIK [WristPosX, WristPosY, GlobalGripperAngle1]
ShoulderAngle1 = -IKShoulderAngle1+900
ElbowAngle1 = IKElbowAngle1+900
WristAngle1 = -IKWristAngle1
GOSUB CheckLimitsThat is all for now. I hope some of this makes some sense.
Kurt
I am still playing around with this. I found my arm servos needed some serious zeroing, so I started playing with the servo zero program. I have my BAP on the BB2 inside of the rover so pushing the buttons was not very convienent. So I wrote a version of the program that used the PS2 controller instead. Also as I may integrate the check zero stuff into the main program I did not want to recompile each time I tweeked one of the offsets. So I use the EEPROM on the BAP to store the offsets. Also it is somewhat interesting to try to figure out what zero is for the IRsensor servo, so I added doing some ADIns when that servo is the one being adjusted. Currently I have the wheel motors not being set, will turn that back on later… In case anyone wants to see this version of the Calculate offsets program:
[code];Project Lynxmotion Rover including Robot Arm
;Description: Rover with 6DOF Robot Arm, controlled by a PS2 remote
;Secondary program to generate the zero points for the rover arm and motors
;using the PS2 to control the settings…
;PS2 CONTROLS:
; - Start Turn on/off Rover / Arm
; - select Switch between Rover and Arm control
;
; ROVER CONTROL
; - Right Stick U/D ;increment/decrement offset
; - Right Stick L/R ;change which servo we are adjusting
;
;
;====================================================================
;
;--------------------------------------------------------------------
;[CONSTANDS]
TRUE con 1
FALSE con 0
;--------------------------------------------------------------------
;[PS2 Controller]
PS2DAT con P12 ;PS2 Controller DAT (Brown)
PS2CMD con P13 ;PS2 controller CMD (Orange)
PS2SEL con P14 ;PS2 Controller SEL (Blue)
PS2CLK con P15 ;PS2 Controller CLK (White)
PadMode con $79
;--------------------------------------------------------------------
;[PIN NUMBERS]
BasePin con P0 ;Base Connection
ShoulderPin con P1 ;Shoulder Connection
ElbowPin con P2 ;Elbow Connection
WristPin con P3 ;Wrist Connection
GripperPin con P4 ;Gripper Connection
WristRotatePin con P5 ;Wrist Rotation Connection (Optional)
IRScanPin con P6 ;IR Scanner pin (optional)
RMotorPin con P10 ;Sabertooth CH1
LMotorPin con P11 ;Sabertooth CH2
RightChannelPin con P10 ;Sabertooth Motor driver Right channel
LeftChannelPin con P11 ;Sabertooth Motor driver Left channel
GripperFSRPin con P16 ;Force Sensing Resistor for the Gripper
IRSensorPin con P18 ;What pin is the IR Scanner on.
NUMSERVOS con 7
ServoTable bytetable BasePin,ShoulderPin, ElbowPin,WristPin, GripperPin, WristRotatePin, IRScanPin, RMotorPin,LMotorPin
swServoVal var sword
i var byte
aServoOffsets var sword(NUMSERVOS)
Base con 0
Shoulder con 1
Elbow con 2
Wrist con 3
Gripper con 4
Rotate con 5
IRScan con 6
RMotor con 7
LMotor con 8
CurrentServo var byte
scanrange var word ’ A/D result variable
scanrangeLast var word ; previous one.
;[Ps2 Controller]
DualShock var Byte(7)
DSLast var Byte(7)
DSChanged var byte
LastButton var Byte(2)
DS2Mode var Byte
PS2Index var byte
;--------------------------------------------------------------------
;[REMOTE]
DeadZone con 10 ;The deadzone for the analog input from the remote
;====================================================================
;Interrupt init
ENABLEHSERVO ;Enable Interrupts for HServo
enable
low RightChannelPin
low LeftChannelPin
;====================================================================
;[INIT]
;PS2 controller
sound P9,[100\5000, 100\4500, 100\4000]
; see how well my pointers work
gosub ReadServoOffsets
CurrentServo = 0
;aServoOffsets = 0,0,0,0,0,0,0,0,0
gosub MoveServos
gosub ShowWhichServo
high PS2CLK
gosub Ps2Init
LastButton(0) = 255
LastButton(1) = 255
pause 100
;====================================================================
;[MAIN]
main:
;Read remote input
GOSUB Ps2Input
pause 25
goto main
;--------------------------------------------------------------------
;[PS2INIT] Initialization for the PS2 remote
Ps2Init:
low PS2SEL
shiftout PS2CMD,PS2CLK,FASTLSBPRE,$1\8]
shiftin PS2DAT,PS2CLK,FASTLSBPOST,[DS2Mode\8]
high PS2SEL
pause 1
if DS2Mode <> PadMode THEN
low PS2SEL
shiftout PS2CMD,PS2CLK,FASTLSBPRE,$1\8,$43\8,$0\8,$1\8,$0\8] ;CONFIG_MODE_ENTER
high PS2SEL
pause 1
low PS2SEL
shiftout PS2CMD,PS2CLK,FASTLSBPRE,$01\8,$44\8,$00\8,$01\8,$03\8,$00\8,$00\8,$00\8,$00\8] ;SET_MODE_AND_LOCK
high PS2SEL
pause 1
low PS2SEL
shiftout PS2CMD,PS2CLK,FASTLSBPRE,$01\8,$4F\8,$00\8,$FF\8,$FF\8,$03\8,$00\8,$00\8,$00\8] ;SET_DS2_NATIVE_MODE
high PS2SEL
pause 1
low PS2SEL
shiftout PS2CMD,PS2CLK,FASTLSBPRE,$01\8,$43\8,$00\8,$00\8,$5A\8,$5A\8,$5A\8,$5A\8,$5A\8] ;CONFIG_MODE_EXIT_DS2_NATIVE
high PS2SEL
pause 1
low PS2SEL
shiftout PS2CMD,PS2CLK,FASTLSBPRE,$01\8,$43\8,$00\8,$00\8,$00\8,$00\8,$00\8,$00\8,$00\8] ;CONFIG_MODE_EXIT
high PS2SEL
pause 100
sound P9,[100\4000, 100\4500, 100\5000]
goto Ps2Init ;Check if the remote is initialized correctly
ENDIF
return
;--------------------------------------------------------------------
;[PS2Input] reads the input data from the PS2 controller and processes the
;data to the parameters.
Ps2Input:
low PS2SEL
shiftout PS2CMD,PS2CLK,FASTLSBPRE,$1\8]
shiftin PS2DAT,PS2CLK,FASTLSBPOST,[DS2Mode\8]
high PS2SEL
pause 1
low PS2SEL
shiftout ps2CMD,PS2CLK,FASTLSBPRE,$1\8,$42\8]
shiftin ps2DAT,ps2CLK,FASTLSBPOST,[DualShock(0)\8, DualShock(1)\8, DualShock(2)\8, DualShock(3)\8, |
DualShock(4)\8, DualShock(5)\8, DualShock(6)\8]
high PS2SEL
pause 1
if DS2Mode <> PadMode then
low PS2SEL
shiftout PS2CMD,PS2CLK,FASTLSBPRE,$1\8,$43\8,$0\8,$1\8,$0\8] ;CONFIG_MODE_ENTER
high PS2SEL
pause 1
low PS2SEL
shiftout PS2CMD,PS2CLK,FASTLSBPRE,$01\8,$44\8,$00\8,$01\8,$03\8,$00\8,$00\8,$00\8,$00\8] ;SET_MODE_AND_LOCK
high PS2SEL
pause 1
low PS2SEL
shiftout PS2CMD,PS2CLK,FASTLSBPRE,$01\8,$4F\8,$00\8,$FF\8,$FF\8,$03\8,$00\8,$00\8,$00\8] ;SET_DS2_NATIVE_MODE
high PS2SEL
pause 1
low PS2SEL
shiftout PS2CMD,PS2CLK,FASTLSBPRE,$01\8,$43\8,$00\8,$00\8,$5A\8,$5A\8,$5A\8,$5A\8,$5A\8] ;CONFIG_MODE_EXIT_DS2_NATIVE
high PS2SEL
pause 1
low PS2SEL
shiftout PS2CMD,PS2CLK,FASTLSBPRE,$01\8,$43\8,$00\8,$00\8,$00\8,$00\8,$00\8,$00\8,$00\8] ;CONFIG_MODE_EXIT
high PS2SEL
pause 288
goto ps2input
endif
#ifdef DEBUGOUTPUT
DSChanged = FALSE
for i = 0 to 6
if DualShock(i) <> DSLast(i) then
DSLast(i) = DualShock(i)
DSChanged = TRUE
endif
next
if DSChanged then
serout s_out, i9600, "DS: ", hex DualShock(0), " ", hex Dualshock(1), " ", hex DualSHock(2), " ", hex DualShock(3), " ", hex DualSHock(4), |
" ", hex DualShock(5), " ", hex DualShock(6), 13]
endif
#endif
;output the data
IF (DualShock(1).bit0 = 0) and LastButton(0).bit0 THEN ;Select Button test
gosub output_data
ENDIF
IF (DualShock(1).bit4 = 0) and LastButton(0).bit4 THEN ;D-Up Button test
CurrentServo = CurrentServo + 1
if (currentServo = NUMSERVOS) then
CurrentServo = 0
endif
gosub ShowWhichServo
ENDIF
IF (DualShock(1).bit6 = 0) and LastButton(0).bit6 THEN ;D-Down Button test
if CurrentServo = 0 then
CurrentServo = NUMSERVOS-1
else
CurrentServo = CurrentServo - 1
endif
gosub ShowWhichServo
ENDIF
IF ABS(Dualshock(4) - 128) > DeadZone THEN ;Right Stick U/D
if DualShock(4) > 128 then
if (aServoOffsets(CurrentServo) < 3000) then
aServoOffsets(CurrentServo) = aServoOffsets(CurrentServo) + (DualShock(4)-128)/4
gosub MoveServos
else
sound 9,[150\3500]
endif
else
if (aServoOffsets(CurrentServo) > -3000) then
aServoOffsets(CurrentServo) = aServoOffsets(CurrentServo) + (DualShock(4)-128)/4
gosub MoveServos
else
sound 9,[150\3500]
endif
endif
ENDIF
LastButton(0) = DualShock(1)
LastButton(1) = DualShock(2)
return
;==============================================================================
; subroutine: MoveServos
; Calls Hservo to move all of the servos to their current zero location
;==============================================================================
MoveServos:
hservo [BasePin\aServoOffsets(Base), ShoulderPin\aServoOffsets(Shoulder), Elbowpin\aServoOffsets(Elbow), WristPin\aServoOffsets(Wrist), |
Gripperpin\aServoOffsets(gripper), WristRotatePin\aServoOffsets(rotate), IRScanPin\aServoOffsets(IRScan)]
;RmotorPin\aServoOffsets(RMotor), LMotorPin\aServoOffsets(LMotor)|
; ]
hservowait[BasePin,ShoulderPin, ElbowPin, WristPin, GripperPin, WristRotatePin, IRScanPin]
; Hack for IRScan if we have something placed at a center point, then we can do an IR in to see what gets us the best results
if CurrentServo = IRScan then
; first read in the IRvalue.
adin IRSensorPin, scanrange
if scanrange <> scanrangeLast then
serout s_out, i9600, "IR Sensor: ", dec scanrange, 13]
scanrangeLast = scanrange
endif
endif
return
;==============================================================================
; Subroutine: ShowWhichServo
; Helps to let the user know which servo they are adjusting
;==============================================================================
ShowWhichServo:
hservo [ServoTable(CurrentServo)-5000\128]
hservowait[ServoTable(CurrentServo)]
hservo [ServoTable(CurrentServo)\5000\128]
hservowait[ServoTable(CurrentServo)]
hservo [ServoTable(CurrentServo)\0\128]
hservowait[ServoTable(CurrentServo)]
return
;==============================================================================
; Subroutine: Output_data
; Outputs to the user the current zero point offsets for each of the servos
;==============================================================================
output_data
serout s_out,i9600,";[SERVO OFFSETS]",13]
serout s_out,i9600,"Base_Offset con ", sdec aServoOffsets(Base),13]
serout s_out,i9600,"Shoulder_Offset con ", sdec aServoOffsets(Shoulder),13]
serout s_out,i9600,"Elbow_Offset con ", sdec aServoOffsets(Elbow),13]
serout s_out,i9600,"Wrist_Offset con ", sdec aServoOffsets(Wrist),13]
serout s_out,i9600,"Gripper_Offset con ", sdec aServoOffsets(Gripper),13]
serout s_out,i9600,"WristRotate_Offset con ", sdec aServoOffsets(Rotate),13]
serout s_out,i9600,"IRScan_Offset con ", sdec aServoOffsets(IRScan),13]
serout s_out,i9600,";[MOTOR NEUTRAL OFFSET]",13]
serout s_out,i9600,"RightChNeutral con ", dec aServoOffsets(RMotor), 13]
serout s_out,i9600,"LeftChNeutral con ", dec aServoOffsets(LMotor), 13]
gosub WriteServoOffsets
return
;==============================================================================
; Subroutine: ReadServoOffsets
; Will read in the zero points that wer last saved for the different servos
; that are part of this robot.
;
;==============================================================================
pT var pointer ; try using a pointer variable
cSOffsets var byte ; number of
bCSIn var byte
bCSCalc var byte ; calculated checksum
b var byte ;
ReadServoOffsets:
readdm 0, [cSOffsets, bCSIn]
serout s_out, i9600, “RSO: cnt:”, dec cSOffsets, " CS in:", hex bcsIn];
if (cSOffsets = 0) or (cSOffsets > NUMSERVOS) then RSO_Error ; offset data is bad so go to error location
; OK now lets read in the array of data
readdm 2, [str aServoOffsets\csOffsets*2]
;... calculate checksum...
; for fun lets do in assembly language.
mov.l #ASERVOOFFSETS, er1 ; get the address of our array into er1
mov.b @CSOFFSETS, r0h ; get the read in count of entries into our counter
shal.b r0h ; multiply by 2 for the number of bytes (2 bytes per entry)
xor.b r0l, r0l ; zero out checksum
RSO_LOOP:
mov.b @er1+, r2l ; get the next byte and post increment the pointer
add.b r2l, r0l ; add it to our checksum
dec.b r0h ; decrement our count
bne RSO_LOOP:8 ; did not go to zero go to process next byte
mov.b r0l,@BCSCALC:24 ; save away our calculated checksum
for i = 0 to numservos-1
serout s_out, i9600, " ", sdec aServoOffsets(i),":", hex aServoOffsets(i)]
next
serout s_out, i9600, " CS Calc:", hex bCSCalc]
; OK we completed the loop.
if bCSCalc = bCSIn then RSO_RETURN
RSO_ERROR:
aServoOffsets = 0,0,0,0,0,0,0
RSO_RETURN:
serout s_out, i9600, [13]
return
;==============================================================================
; Subroutine: WriteServoOffsets
; Will write out the current servo offsets to the EEPROM on the BAP29
;
;==============================================================================
WriteServoOffsets:
; OK First calculate the checksum
; for fun lets do in assembly language.
mov.l #ASERVOOFFSETS, er1 ; get the address of our array into er1
mov.b #NUMSERVOS*2, r0h ; count of bytes to do
xor.b r0l, r0l ; zero out checksum
WSO_LOOP:
mov.b @er1+, r2l ; get the next byte and post increment the pointer
add.b r2l, r0l ; add it to our checksum
dec.b r0h ; decrement our count
bne WSO_LOOP:8 ; did not go to zero go to process next byte
mov.b r0l,@BCSCALC:24 ; save away our calculated checksum
; go back to basic to write out our values and return
writedm 0, [NUMSERVOS, bCSCalc, str aServoOffsets\NUMSERVOS*2]
return[/code]
To help me figure out some of the alignments and positioning I marked up a box with lines at 15 degree increments out from the center and marked these lines with some distances like 20cm… Here is the arm positioned to one that is 15 degrees off center.
http://i416.photobucket.com/albums/pp245/Kurts_Robots/DSC03286.jpg
Note: I changed my IR sensor to be vertical instead of horizontal on the IR servo arm. Also I changed my IR sensing code to not really look for the edges as this was unreliable as the IR sensor would start to detect the object before it gets there due to the width of the beam. So now I look for the maximum value (minimum distance). The code is getting closer. Here is a picture showing where the arm was moved to after the IR sensor detected the object.
http://i416.photobucket.com/albums/pp245/Kurts_Robots/DSC03287.jpg.
I also updated the InitArm code that if the current Y is less than the InitArmY it does the init in 2 steps. The first position has the Y much higher. This appears to help my arm not to hang up on the sensor or rover body if you by chance to an init while the arm is down low…
That is all for now
Kurt
Hi Kurt,
You had some good fun with the rover for sure.
Sorry for not being to active at the moment…
Interesting to see how you write the offsets to the EEPROM. I was thinking of doing the same thing with the DIY Remote calibrating part. But that’s another story
How does the arm move at the moment? You should have it move in 2 steps as well. First movement to get it in front of (or above) the gripper. And then move so the gripper can grab it. Did you include that as well or only in the “go homeâ€
Hi Kurt,
I forgot to ask… Are you using Inverse kinematics for the base to at the moment? I’ve got it somewhere. Let me know if you need it…
Xan
Hi Xan,
Yep I am having fun . I don’t make as much progress here as I should as I keep jumping around doing other things, but at least it is starting to work!
Yes at the bottom of the program in the last post had the two functions that wrote out and read back in my offset table. I have tried a couple of variations of this over time. For example the first one in my tv brat did a loop for each offset and did a readdm or writedm for each entry with: lowbyte and highbyte of each entry.
I also tried using pointer variables and loop through the number of bytes incrementing my pointer. Then I decided to try the STR modifier, which in this case simply says read or write the number of bytes I say, which worked out great.
I used inline assembly language to compute a checksum to make sure everything was read and written OK and in the rover read in version I also checked to make sure the offsets were within a valid range else I fell back to hard coded values. The assembly code could easily be replaced with something like:
bCSCalc = 0
for i = 0 to COFFSETS-1
bCSCalc = bCSCalc + LOWBYTE(aServoOffsets(i)) + HIGHBYTE(aServoOffsets(i))
nextHi Kurt! Yup, that’s what we had to do to get the BRAT to accurately find the water bottles in the BRAT seek and destroy project. It works better for this sort of application.
Hi Kurt,
Sorry for my late response (again). I’m at work at the moment so I don’t have the current code with me at the moment. It’s kinda hard to see what you’ve changed in the main loop without having the original code.
I’ll check my different versions on the IK when I got home. Changing the 2D IK to 3D IK is something about adding a additional line to the IK sub. It wasn’t that much. I do think that we should have both 2D and 3D in there since 2D is much easier to control with the remote. If we want to change between 2D and 3D we should add FK as well since we want to know the angle of the base when switching. I’ll check this as soon as possible to update you with the new IK. I think this will help to send the arm easier to the object you’re trying to grab.
Changing the angle of the IR sensor seems like a good idea. It will also increase the range since I’m using one of the normal brackets instead of the large ones.
Keep up the good work!
Talk to you soon.
Xan
Hi Xan,
Thanks, I have not done as much lately on this as I would like. Been busy doing stuff outside plus trying to help out a few online… Look forward to seeing your versions of the IK.
Yesterday I started to reorganize some of the code, to make it easier for me to experiment with some other changes. Things like trying to split up the PS2INPUT function into a few functions. I would like the PS2 function to simply get the input and maybe make a few state changes, but then reserve the rest of the work for functions specific to mode. (I prefer functions that all of the code fits on one page…)
The main function may also take care of the PS2 joystick deadband processing and return the X,Y for both joysticks. I am in the process of trying to decide if I should normalize this. For two of them in rover mode you return value-128 and for two others you return -(value-128). Actually maybe -127, but… The other thing that slightly throws me is the coordinate system of the two joysticks. X is up/down and Y is left/right, but may not change that…
I am then going to play with the R3 button in ARM mode to switch from the current 2D IK mode into controls like I am on a Backhoe with Cat controls. Left joystick does the rotate and dipper angle, Right does the boom and bucket curl. Note: John Deere are different, left does rotate/boom and right does dipper/curl, but mine was changed to Cat controls as it drove or excavator friend nuts whenever he used our machine.
Kurt
Hi Kurt,
Backhoe controls! There is something interesting about that. I never got past running a front-end loader, or a scraper. I always wanted to drive a backhoe!
A full backhoe model would make an excellent build!
Alan KM6VV
I agree, having functions that fit the page are much easier to read. That’s why I’ve already had split the init part for the sub. (or did was that only in the phoenix V2…? LOL)
I’m not quite sure I understand exactly where you want to do this stuff but I’m happy to see the results.
Are you talking about controlling the arm? I always have to think what joystick to move to get the resulted movement. If you have another way I’m in for it. Now the arm is relatively easy to control when you stand on the left from it. I thought about hitting the R3 or L3 button to swap directions to make it easy to control it from the other side.
I’ve never had to change to drive one of those so I’ve no idea how those controls work. But I’m pretty sure they though longer about what to put on what joy so I’m happy to try it out.
I’ve been digging in to my archive yesterday to dig up my 3D version. It is still using floating point calculations but it should be easy to change it to fixed point. Another thing you might notice is that the logical endpoint of the arm is the middle of the gripper and not the wrist as it is now. This was pretty hard to control but might make it easier when the gripper needs to go to a specific position. Just check it out and you’ll know what I’m talking about.
[code]ArmIK [IKGripperPosX, IKGripperPosY, IKGripperPosZ, IKGripperAngle]
;Length between the Base and Gripper
IKGripperPosYZ = IKGripperPosY ;TOINT(FSQRT(TOFLOAT((IKGripperPosY*IKGripperPosY)+(IKGripperPosZ*IKGripperPosZ))))
;Gripper offset
GOSUB GetSinCos [TOFLOAT(GripperAngle)]
IKWristX = TOINT(TOFLOAT(IKGripperPosX)-CosA*TOFLOAT(GripperLength))
IKWristY = TOINT(TOFLOAT(IKGripperPosYZ)-(SinA*TOFLOAT(GripperLength)))-BaseLength
;IKSW - Length between Shoulder and wrist
IKSW = FSQRT(TOFLOAT((IKWristY*IKWristY)+(IKWristX*IKWristX)))
;IKA1 - Angle between SW line and the ground in rad
GOSUB GetBoogTan [IKWristY, IKWristX]
IKA1 = BoogTan
;IKA2 - ?
IKA2 = FACOS((TOFLOAT((HumerusLength*HumerusLength) - (UlnaLength*UlnaLength)) + (IKSW*IKSW)) / (TOFLOAT(2*HumerusLength) * IKSW))
;Elbow Angle
;IKSW = FACOS(0.61183);((TOFLOAT((HumerusLength*HumerusLength) + (UlnaLength*UlnaLength)) - (IKSW*IKSW)) / TOFLOAT(2*HumerusLength*Ulnalength))
IKElbowAngle = (180-TOINT((FACOS((TOFLOAT((HumerusLength*HumerusLength) + (UlnaLength*UlnaLength)) - (IKSW*IKSW)) / TOFLOAT(2*HumerusLength*Ulnalength))*180.0) / 3.141592))*-1
;Shoulder Angle
IKShoulderAngle = TOINT(((IKA1 + IKA2) * 180.0) / 3.141592)
;Base Angle
GOSUB GetBoogTan [IKGripperPosZ, IKGripperPosY]
;IKBaseAngle = TOINT((BoogTan*180.0) / 3.141592)
;Wrist Angle
IKWristAngle = IKGripperAngle-IKElbowAngle-IKShoulderAngle
;Set the Solution quality
IF(IKSW < TOFLOAT(UlnaLength+HumerusLength-30)) THEN
IKSolution = TRUE
ELSE
IF(IKSW < TOFLOAT(UlnaLength+HumerusLength)) THEN
IKSolutionWarning = TRUE
ELSE
IKSolutionError = TRUE
ENDIF
ENDIF
return[/code]
Let me know if it helped!
Xan