If those are servos working in parallel to operate the arm, you may want to check that they are not somewhat fighting each other and are operating exactly together.
Actually it is the big old digital servos HS-5745mg that make noise!. Xan warned me about them
More progress. It looks the problem I am having is partially the test program. I don’t think the pulsouts were working properly. Probably a timing issue trying to get the pulses for all 6 servos (+1 later when I added the IR scanner one, plus the channels for the motors.
So I hacked up the program some to first use mservo and now hservo. Also played around with arrays and the like. I also was having problems not knowing if all of the servos were plugged in properly, so I added some code to move the selected servo back and forth. Here I found I had a few plugged in the wrong order.
Once I converted to something other than pulsout I don’t appear to have power issues. Still working on zeroing servos, may need to readjust the horns some and my display output still needs to be set to the right scale. But with this code at least all of the servos move:
[code];------------------------------------
; Rover with arm Servo verify
;
;------------------------------------
; How to use
;
; Press A to decrease the servo
; offset by 5us
;
; Press C to increase the servo
; offset by 5us
;
; Press B to change which servo is
; being manipulated, and to send
; data back to the terminal
;
; The servos are changed in this order:
;
; Base --> Shoulder --> Elbow
; ^ |
; | |
; | v
; Send <- Wrist <- Gripper <-Wrist
; Data Rotate
;------------------------------------
enablehservo
;System variables
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
NUMSERVOS con 9
ServoTable bytetable BasePin,ShoulderPin, ElbowPin,WristPin, GripperPin, WristRotatePin, IRScanPin, RMotorPin,LMotorPin
swServoVal var sword
i var byte
aServoVals var sword(NUMSERVOS)
Base var aServoVals(0)
Shoulder var aServoVals(1)
Elbow var aServoVals(2)
Wrist var aServoVals(3)
Gripper var aServoVals(4)
Rotate var aServoVals(5)
IRScan var aServoVals(6)
RMotor var aServoVals(7)
LMotor var aServoVals(8)
Base = 0
Shoulder= 0
Elbow = 0
Wrist = 0
Gripper = 0
Rotate = 0
IRScan = 0
RMotor = 0
LMotor = 0
currentServo var byte
currentServo = 0
;button init
buttonA var bit
buttonB var bit
buttonC var bit
prevA var bit
prevB var bit
prevC var bit
input p12
input p13
input p14
sound 9,[50\3800, 50\4200, 40\4100]
gosub MoveServos
gosub ShowWhichServo
gosub MoveServos
main
prevA = buttonA
prevB = buttonB
prevC = buttonC
buttonA = in12
buttonB = in13
buttonC = in14
if (buttonA = 0) AND (prevA = 1) then
sound 9,[50\3800]
if aServoVals(CurrentServo) > -500 then
aServoVals(CurrentServo) = aServoVals(CurrentServo) - 25
gosub MoveServos
else
sound 9,[150\3500]
endif
elseif (buttonB = 0) AND (prevB = 1)
sound 9,[50\(3600 + (currentServo * 100))]
currentServo = currentServo + 1
if(currentServo = NUMSERVOS) then
sound 9,[75\3200, 50\3300]
goto output_data
endif
gosub ShowWhichServo
gosub MoveServos
elseif (buttonC = 0) AND (prevC = 1)
sound 9,[50\4400]
if aServoVals(CurrentServo) < 500 then
aServoVals(CurrentServo) = aServoVals(CurrentServo) + 25
gosub MoveServos
else
sound 9,[150\3500]
endif
endif
goto main
MoveServos:
hservo [BasePin\Base, ShoulderPin\Shoulder, Elbowpin\Elbow, WristPin\Wrist, Gripperpin\gripper, WristRotatePin\rotate, IRScanPin\IRScan, RmotorPin\RMotor, LMotorPin\LMotor]
return
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
output_data
serout s_out,i38400,";[SERVO OFFSETS]",13]
serout s_out,i38400,"Base_Offset con ", sdec Base,13]
serout s_out,i38400,"Shoulder_Offset con ", sdec Shoulder,13]
serout s_out,i38400,"Elbow_Offset con ", sdec Elbow,13]
serout s_out,i38400,"Wrist_Offset con ", sdec Wrist,13]
serout s_out,i38400,"Gripper_Offset con ", sdec Gripper,13]
serout s_out,i38400,"WristRotate_Offset con ", sdec Rotate,13]
serout s_out,i38400,"IRScan_Offset con ", sdec IRScan,13]
serout s_out,i38400,";[MOTOR NEUTRAL OFFSET]",13]
serout s_out,i38400,"RightChNeutral con ", dec RMotor, 13]
serout s_out,i38400,"LeftChNeutral con ", dec LMotor, 13]
waitloop
gosub MoveServos[Base,Shoulder,Wrist,Elbow,Gripper,Rotate,RMotor,LMotor]
buttonB = in13
if (buttonB = 0) AND (prevB = 1) then
currentServo = 0
sound 9,[200\3800]
goto main
endif
prevB = buttonB
goto waitloop
[/code]
My… Program! 
Haha, just kidding. Yours definitely looks much nicer than mine… I’m not sure what I was thinking when I wrote all those ifs… Must have been an off-day. I should know better than that… 
I believe a rewrite is in order for the “official” program, since you’ve got some good ideas with Hservo and wiggling the selected servo…
LOL 
Hi Xan,
Ben playing around with my Rover with your code. I still have a couple of fixes to do with my rover. The wires to one side of motors came free from the connections to the Sabertooth, when I put them back I must have reversed the A and B wires as now the wheels go in the wrong direction. I of course can fix by reopening up the case and swapping the wires or could do a simple code fix. I will probably do it the right way.
Also still need to get my zero point correct and verify my arm dimensions versus the ones in the code… But I do like to live dangerous. Since we both are using the AL5D I thought it would be not too bad. However I know that you are using the light weight wrist rotate which added 1.125 inches of reach and I am using the heavy weight one which added .5 inch so I know I need to subtract .625 inches (converted to mm) from one of the measurements is that the ulna?
So I loaded your code to see what would happen. The wheel motors are working well , except for what I mentioned above .
So I started the arm. I like the park position. When processing the Start button, you set the wrist positions to some fixed location and angles. Do this depend on which configuration of Arm you have?
I also quickly found out that my IRScanner setup with the sensor mounting pointing up from the servo to give it a full range, may not be the best configuration for this as the arm can run into it (with a reasonable amount of force). Can turn it over and have it pointing down, but that reduces the range of the servo to maybe 40 degrees. Could also use a C bracket and mount it on the surface, but I think that will aso get in the way of the arm.
I need to play around more with the arm. I notice that when I move it, it will go smooth and then it will violently yank back the opposit direction. I am wondering if I am running into an overflow somewhere. I will start debuging later. Will probably need to dump variables with different coordinats and different angles to see what happens. Also once I am comfortable with this, I may start experimenting with converting some floating point functions into fixed point. Anyway I am having fun! Now I just need to find out where that screw came flying from 
Kurt
Hi Kurt,
Happy to hear your rover is making progress!
You need to substract the .625 inches from the gripper length.
I hope you’re working with the version (1.2) I’ve send you by mail. It’s got a couple of constants defining the init postion:
;[WRIST INIT POSITION]
InitWristPosX con 55
InitWristPosY con 100
InitGripperAngle1 con -550
This will be different for each arm. The values above will be fine for the AL5D with light gripper. I don’t know how it works out with the heavy gripper though.
I’m not fully sure what IR scanner you’re talking about. The one in the front? The arm is large enough to reach over the scanner…
Yepp, this is caused by a (or more) overflow. The software is tested with the smaller AL5B and not fully tested with the D version.
Be carefull the screws don’t hurt yah 
Xan
That is what I thought. My guess is that in the processing of the Start button, I need to change the code from the constants to your defines.
Here is a picture of mine. See how the servo at the bottom of the arm likes to rest on the sensor…
http://i416.photobucket.com/albums/pp245/Kurts_Robots/DSC03240.jpg
Ok sounds like some debug time. Should be fun!
Starting to do a little debugging and I think you may be overflowing a long variable.
I added the following code in after the call to the ArmIK:
if (WristPosX <> WristPosX_Last) or (WristPosY <> WristPosY_Last) or (GlobalGripperAngle1 <> GlobalGripperAngle1_Last) then
WristposX_Last = WristPosX
WristposY_Last = WristPosY
GlobalGripperAngle1_Last = GlobalGripperAngle1
serout s_out,i9600,[sdec WristPosX," ", sdec WristposY, " ", sdec GlobalGripperAngle1,"=",sdec BaseAngle1, " ", sdec ShoulderAngle1, " ", sdec WristAngle1,13]
endif
I then ran it slowly until I got the arm jerk:
Here is some of the debug output:
265 625 0=0 247 198
265 628 0=0 246 227
265 632 0=0 244 261
265 634 0=0 244 286
265 637 0=0 243 316
265 642 0=0 241 382
265 648 0=0 -780 662
It Jerked on the last line. If you look at the start of that function you have:
;IKSW - Length between Shoulder and wrist
IKSW2 = SQR((((IKWristPosY-BaseLength)*(IKWristPosY-BaseLength))+(IKWristPosX*IKWristPosX))*c4DEC)
Which with the last values you end up with it solving out like:
sqrt((600600+265265)*10000), which if you solve this I think you are trying to take the sqrt of: 4,302,250,000 and I think the largest number a long can take is something like: 4,294,967,295
Will play more later.
Kurt
Still playing here. I could probably fix that one line I mentioned by decreasing the accuracy a little bit. If I remember correctly, I think that:
sqrt(x*Y) = sqrt(x)*sqrt(y).
So I could fix that line by maybe C4DEC by 4 before the multiply and then mutlplying the result by 2. Yes you lose a slight bit of accuracy here. But then a few lines down you have:
Temp1 = (((HumerusLength*HumerusLength) + (UlnaLength*UlnaLength))*c4DEC - (IKSW2*IKSW2))Which squares the number again which will overflow… There are a few other lines like that as well. Can probably hack these lines in a similar way…
I also noticed that when the arm really jerked that IKSolutionError was TRUE. One change I was about to do is to maybe not call the Movement function if there is an error.
However then the arm and wheels stopped working). I assumed it was the battery was discharged (which it was). So I recharged the battery. But still not working properly. Checked battery voltage: 13.67V. Checked the voltage coming out of HD battery regulator: 6.04v, but still no motion and anemic beep. Not sure yet if regulator going bad. Will try plugging in external battery. Also maybe violent arm jerk dislodged something…
Kurt
I still am in the process of debugging the power supply issue. I am not sure yet if it is the two batteries having problems or voltage regulator. With no servos plugged into BB2 I have 6v, with 1 or 2 servos running it drops down to 3v or so. I need to pull top off of rover and check voltage going into regulator both under load and not under load to see if the voltage is still there…
But before that, I stole the power supply from the brat (6v 1600mah) and connected it to the BB2/servos and it runs the arm ok on that…
So back on the software side, I have tried some changes, which has made the arm make less violent movements. Some of them may not be needed due to later changes.
The first was I tried to handle the overflows of the 32 bits I mentioned in the previous post. I did this by only multiplying by 10,000/4 and keeping track of this. This rippled in this function, which now looks like:
[code]ArmIK [IKWristPosX, IKWristPosY, IKGripperAngle1]
;IKSW - Length between Shoulder and wrist
IKSW2 = SQR((((IKWristPosY-BaseLength)*(IKWristPosY-BaseLength))+(IKWristPosX*IKWristPosX))*(c4DEC/4)) ; KJE - value 1/2 what it should be now
;IKA1 - Angle between SW line and the ground in rad
GOSUB GetArcTan2 (IKWristPosY-BaseLength), IKWristPosX], IKA14
;IKA2 - Angle of the line S>W with respect to the humerus in radians
Temp1 = (((HumerusLength*HumerusLength) - (UlnaLength*UlnaLength))*(c4DEC/4) + (IKSW2*IKSW2)) ; KJE - Value is 1/4 of what it should be.
Temp2 = ((2*HumerusLength)*c2DEC * IKSW2) ; KJE - value is 1/2
GOSUB GetArcCos (Temp1 / (Temp2/c4DEC)) *2 ], IKA24 ; KJE - corrected for 1/4value / 1/2 value
;Elbow Angle
Temp1 = (((HumerusLength*HumerusLength) + (UlnaLength*UlnaLength))*(c4DEC/4) - (IKSW2*IKSW2)) ; KJE 1/4
Temp2 = (2*HumerusLength*Ulnalength)
GOSUB GetArcCos (Temp1 / Temp2) * 4] ; kje fixed for 1/4
IKElbowAngle1 = -(1800-AngleRad4*180/3141)
;Shoulder Angle
IKShoulderAngle1 = (IKA14 + IKA24) * 180 / 3141
;Wrist Angle
IKWristAngle1 = IKGripperAngle1-IKElbowAngle1-IKShoulderAngle1
;Set the Solution quality
IF((IKSW2*2) < (UlnaLength+HumerusLength-30)*c2DEC) THEN ; kje fixed for 1/2
IKSolution = TRUE
ELSE
IF((IKSW2*2) < (UlnaLength+HumerusLength)*c2DEC) THEN ; kje fixed for 1/2
IKSolutionWarning = TRUE
ELSE
IKSolutionError = TRUE
ENDIF
ENDIF
return[/code]
Then I changed the main loop to not move the arm if there was not any valid solution. This helped some of the jerking around. The code changes looked like:
[code] IF IKSolutionError = FALSE then ; KJE - Added test to not do movement if there is an error
IKSolutionErrorBeep = FALSE ; reset if we have output any warning beeps
GOSUB Movement-ToFloat(BaseAngle1)/10.0, |
-ToFloat(ShoulderAngle1)/10.0, |
-ToFloat(ElbowAngle1)/10.0, |
-ToFloat(WristAngle1)/10.0, |
-ToFloat(Gripper1)/10.0, |
-ToFloat(WristRotateAngle1)/10.0, 100.0]
ELSEIF IKSolutionErrorBeep = FALSE
IKSolutionErrorBeep = TRUE ; KJE added beep if the arm tries to move to an error location.
Sound P9,[60\4000,60\4000]
ENDIF
[/code]
Note: when the error happens I cause a beep and remember it. I defined this varaible as a bit and init it to false outside of main.
I then found that when I got to the error position, if I kept moving the sticks in the wrong direction, it would at times get a valid solution, that again wrapped around and jerk. So I changed the PS2 Input function, that if the last calculation caused an error, it did not allow the WristPosX or WristPosy to continue in the same direction (ie if it was positive don’t let it go more positive. My Ifs could be simplified, but…
[code] IF ABS(Dualshock(5) - 128) > DeadZone THEN ;Left Stick L/R
; kje - If the Arm is in an error condition, don’t continue in the direction of error.
if not ((IKSolutionError = TRUE) and ( ((WristPosX > 0) and (DualShock(5) < 128)) or ((WristPosX < 0) and (DualShock(5) > 128)))) then
WristPosX = WristPosX -(Dualshock(5) - 128)/12 ;X Position
endif
ENDIF
IF ABS(Dualshock(6) - 128) > DeadZone THEN ;Left Stick U/D
; kje - If the Arm is in an error condition, don't continue in the direction of error.
if not ((IKSolutionError = TRUE) and ( ((WristPosY > 0) and (DualShock(6) < 128)) or ((WristPosY < 0) and (DualShock(6) > 128)))) then
WristPosY = WristPosY -(Dualshock(6) - 128)/12 ;Y Position
endif
ENDIF
[/code]
That is all for now. I need to play around with my initial positions and them maybe I will convert the movement functions to integer based.
Kurt
Sounds like you’re making good progress. The math for arm IK is not terribly difficult to calculate, but implementation in the real world, not as easy as one might think. We really appreciate ya Kurt!
FYI - took apart and reconfigured the power wires, such that now I can turn on the BB2 and servos (6v from HD regulator) independant from the 12v going to Sabertooth. Also while I was at it I added in the M type plug for recharging the batteries, including having it break the ground wire circuit coming from the battery when the external power plug is connected.
I have all of the wires connected again and everything appears to be working again. Not sure what was wrong here, but before, but it appears to working again. Now to get back in one piece with all of the wires back inside the chasis and back to having some fun.
Earlier I had completed the conversion of the the movement code to integer math. Once I get it all back in one piece I will start playing so more with the performance…
Kurt
Hi Kurt,
I’ve tested you’re modifications concerning the overflow problems. They work just perfect! I also made some modifications to the input to make sure the arm can’t be moved any furter in the wrong direction when stretched out. They both are big improvements if you ask me 
In the IK sub
;Set the Solution quality
IKSolutionWarning = ((IKSW2*2) > ((UlnaLength+HumerusLength-40)*c2DEC)) ; kje fixed for 1/2
IKSolutionError = ((IKSW2*2) > ((UlnaLength+HumerusLength)*c2DEC)) ; kje fixed for 1/2
Resetting in the main sub isn’t necessary anymore.
In the PSInput sub
[code] IF ABS(Dualshock(5) - 128) > DeadZone THEN ;Left Stick L/R
IF IKSolutionWarning THEN ;Allow only movement to the base
WristPosX = WristPosX -((Dualshock(5) - 128)/12) MIN 0 ;X Position
ELSE
WristPosX = WristPosX -(Dualshock(5) - 128)/12 ;X Position
ENDIF
ENDIF
IF ABS(Dualshock(6) - 128) > DeadZone THEN ;Left Stick U/D
IF IKSolutionWarning THEN
IF WristPosY > BaseLength THEN ;Allow only downward movement
WristPosY = WristPosY -((Dualshock(6) - 128)/12) MIN 0
ELSE ;Allow only upward movement
WristPosY = WristPosY -((Dualshock(6) - 128)/12) MAX 0
ENDIF
ELSE
WristPosY = WristPosY -(Dualshock(6) - 128)/12 ;Y Position
ENDIF
ENDIF[/code]
It would be a good thing if we could also limit movements if a servo hits his limits. But I think that’s hard to do… But maybe I’ll come up with something.
Xan
Now that I think I caught up on some other coding I thought I would start playing with the rover and arm. My first step will be to rezero the servos and motors. At idle I have one motor on one side that is moving so will need to adjust. I was thinking of hacking up my version of code and maybe integrate in the zero code like I did with my brat. In this case I am not using a TV remote but a ps2 remote so maybe some button combo on the PS2 that enters the mode, can use a joystick or such on the PS2 to adjust the current servo and another button to select which servo…
May store the offsets in the EEPROM such that we don’t have to recompile.
Another thing I am playing with, is my arm has a tendancy to hang up on the front of the rover, especially if I put a servo with a sensor on the front.
I think this show up in the pictures:
http://i416.photobucket.com/albums/pp245/Kurts_Robots/DSC03265.jpg and the closeup http://i416.photobucket.com/albums/pp245/Kurts_Robots/DSC03266.jpg
I am thinking of slighly modifying the arm, by rotating the forarm (tubing) by 180 degrees to put the gripper servo above the gripper insead of below it. Does this sound reasonable? Other suggestions?
Kurt
Nice hat Kurte!
Interesting 'bot!
Why not just make the tubular arm section a little longer?
Is that low-profile servo for the gripper? OK, yes, you could probably flip it over as well.
Alan KM6VV
Thanks Alan,
Good idea about using a longer tube. I may try that next as I have a few from my old CHR-3 legs which is probably 3/4 to 1 inch longer. The servo is a standard HS-422 and as I mentioned I may try turning it over.
I am having fun with it. It is more or less the standard 4wd with the AL5D arm attached on top. I did modifiy the top some (cut it into two pieces) and mounted the arm, power plus and switches and an RS232 port in the front piece. I have installed the BB2 with the PRO inside the case and hopefully nothing will have to be external.
I have it such that I can both program the bot as well as recharge the batteries from the plugs on the outside of the rover.
As for the hat it goes with my other larger rover.
http://i416.photobucket.com/albums/pp245/Kurts_Robots/Tractor-with-rover-robot.jpg
Kurt
Now that’s a REAL toy!
I’m working on a remote 4WD camera platform as well.
Alan KM6VV