Putting robotics at your service™

Free shipping on orders over $200

How Robot Arm react to PlayStation 2 Wireless Control

Print view Share :
Previous topicNext topic

Page 3 of 3 [ 44 posts ]

1, 2, 3
User avatar
Guru ( offline )
Posts: 4913
Posted: 2012-02-08 13:02 
As I mentioned it would need to be updated in the ArmIK function:
Code:
ArmIK [IKWristPosX, IKWristPosY, IKGripperAngle1]
   
   ;IKSW - Length between Shoulder and wrist
#ifdef USESQR
   IKSW2 = SQR((((IKWristPosY-BaseLength)*(IKWristPosY-BaseLength))+(IKWristPosX*IKWristPosX))*(c4DEC/4))   
#else
   IKSW2 = TOINT(FSQRT(TOFLOAT((((IKWristPosY-BaseLength)*(IKWristPosY-BaseLength))+(IKWristPosX*IKWristPosX))*(c4DEC/4))))
#endif   
   ;IKA1 - Angle between SW line and the ground in rad
   GOSUB GetAtan2 [(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))         
   Temp2 = ((2*HumerusLength)*c2DEC * IKSW2)                                                
   GOSUB GetArcCos [(Temp1 / (Temp2/c4DEC)) *2 ], IKA24                                       

   ;Elbow Angle
   Temp1 = (((HumerusLength*HumerusLength) + (UlnaLength*UlnaLength))*(c4DEC/4) - (IKSW2*IKSW2))   
   Temp2 = (2*HumerusLength*Ulnalength)
   GOSUB GetArcCos [(Temp1 / Temp2) * 4]
   IKElbowAngle1 = -(1800-AngleRad4*180/3141)

   ;Shoulder Angle
   IKShoulderAngle1 = (IKA14 + IKA24) * 180 / 3141

   ;Wrist Angle
   IKWristAngle1 = IKGripperAngle1-IKElbowAngle1-IKShoulderAngle1
      
   ;Set the Solution quality   
  IKSolutionWarning = ((IKSW2*2) > ((UlnaLength+HumerusLength-40)*c2DEC))
  IKSolutionError = ((IKSW2*2) > ((UlnaLength+HumerusLength)*c2DEC))
   
return


But the Angle checks are not done on these actual angles as you can see in the main function:
Code:
  ;Inverse Kinematic calculations
  GOSUB ArmIK [WristPosX, WristPosY, GlobalGripperAngle1] 
  ShoulderAngle1 = -IKShoulderAngle1+900
  ElbowAngle1 = IKElbowAngle1+900
  WristAngle1 = -IKWristAngle1

  GOSUB CheckLimits


Actually if I were to play a little more with this code, I would probably move all of this secondary code into the ArmIK function as it is replicated everywhere that it ArmIK is called.

Kurt


Rookie ( offline )
Posts: 25
Posted: 2012-02-08 14:33 
Kurt,

Thank you for your prompt reply. We will look into it.

Another strange problem occur. We use HS-645GM for our wrist. It was shaky when the servo start to work. I thought the servo is bad. I went to a hobby shop to buy a new one today, guess what, all their HS-645MG inventory are shaky with Bot Board II Atom Pro 28. I test with 225. It works fine. Are you aware of this?


User avatar
Guru ( offline )
Posts: 9257
Posted: 2012-02-08 14:35 
There is nothing wrong with the 645 servos. Post a video and images of your wiring so we can see what you are doing wrong.

_________________
Jim Frye, the Robot Guy
http://www.lynxmotion.com
I've always tried to do my best...


Rookie ( offline )
Posts: 25
Posted: 2012-02-08 15:16 
Ok, we will video it tonight and submit the video tonight.

Thanks


User avatar
Guru ( offline )
Posts: 9257
Posted: 2012-02-08 15:26 
The video will help us to determine the possible problems, but the images of the board and it's power supply wiring is also required to help us determine the cause of the problem.

_________________
Jim Frye, the Robot Guy
http://www.lynxmotion.com
I've always tried to do my best...


User avatar
Guru ( offline )
Posts: 4913
Posted: 2012-02-08 21:20 
ktsu10 wrote:
Kurt,

Thank you for your prompt reply. We will look into it.

Another strange problem occur. We use HS-645GM for our wrist. It was shaky when the servo start to work. I thought the servo is bad. I went to a hobby shop to buy a new one today, guess what, all their HS-645MG inventory are shaky with Bot Board II Atom Pro 28. I test with 225. It works fine. Are you aware of this?

I have never had any problems with 645mg servos I have used. Usually when I hear something like this, I assume some sort of power issue. Something like supplying more than 6V to the servo or not supplying enough current (to small of a battery or wall wort, or bad wiring, like using a AA battery holder with bad contacts and small wires).

As for the wrist not going beyond 45 degrees. I extracted a lot of the arm code into a test program where I simply entered an X, Y and wrist angle and then printed out all of the calculated values. With my arm an AL5D, it should be able to reach out to about 334 units in X direction with Y at 69... with the lengths of 147 and 187. Sure enough the elbow came in right around 900, but was limited to 450. I changed my test program like:
Code:
Elbow_MAX      con 850

And it was reflected in the outputs, so I think simply changing this should work... However, depending on the age of your Arm, there may be an issue. That is my AL5D is built with two large scale digital servos (HS-5745MG), which Lynxmotion does not carry anymore. I am guessing that unless these servos are reprogrammed they probably have a maximum range of 140 degrees, which I am seeing. I believe the newer Arms are now using non digital servos (HS-755HB, and HS-805BB), which do have a full range of 180 degrees. Maybe someday I may have to update my arm to the current version. Digitals are nice but I hate the noise and the lack of ability to turn the servo off. So far these are the only 2 digital servos I have (except for the Robonova still sitting in a box somewhere :lol:).

So the question is, which version do you have?

Kurt


Rookie ( offline )
Posts: 25
Posted: 2012-02-09 17:21 
Kurt,

Thank you for your info. Will give them a try.


Rookie ( offline )
Posts: 25
Posted: 2012-02-19 00:25 
Hi,

Everything seems to be working fine now thank you so much for your help. One more question, if we wanted to increase the speed at which the gripper opens and closes how could we only speed up the gripper and none of the other servos? Also the gripper seems to automatically close and only opens if we keep holding L1. Do you know how to increase the speed of the gripper?


User avatar
Guru ( offline )
Posts: 4913
Posted: 2012-02-19 11:40 
I am glad it is working OK for you now!

Speed of gripper... Look at where L1 and L2 are processed. Currently in My code it looks something like:
Code:
   IF (DualShock(2).bit2 = 0) AND (GripperPressure>GripperHoldPressure) THEN   ;L1 Button test
       TargetGripper1 = (TargetGripper1-abGOModifier(ArmSpeedSelect)) min Gripper_MIN ;Gripper Close
   ENDIF
   IF DualShock(2).bit2 AND (LastButton(1).bit2 = 0) THEN   ;L1 Button release test
       TargetGripper1 = 0   ; put to center position...
   ENDIF

Note: The second IF in this code was the stuff I hacked in earlier that I believe should put the gripper back to middle position when you release the L1 button. You may want this or not... If not simply remove that second part that says button release test...

But in this code you see that the Gripper changes by the amount: abGOModifier(ArmSpeedSelect)) each time the code goes through this while the button is still pressed. The values of abGoModifier is defined in a byte table:
Code:
abGOModifier        bytetable 20, 40, 60, 80    ; Gripper Opening/Closing Speed (L1/L2)

To change the speeds in this code you can simply change these values to something larger...

Also later in the code there is a block of code to make sure the gripper does not go too fast. It was mainly in there such that for example you hit the button to center the gripper and it is a long distance from center it will still only move at some maximum speed. In the original version this max speed was come defined constant that I believe had a value of 40. If you are using that version of the code simply increase that value. My current version says the maximum speed is the value from the above byte table. That code looks like:
Code:
     ;Limit gripper max speed
     IF (ABS(Gripper1-TargetGripper1) > abGOModifier(ArmSpeedSelect)) THEN
       IF ((Gripper1-TargetGripper1) >= 0) THEN
         IF (GripperPressure>GripperHoldPressure) THEN
           Gripper1 = Gripper1 - abGOModifier(ArmSpeedSelect)
         ENDIF
        ELSE
         Gripper1 = Gripper1 + abGOModifier(ArmSpeedSelect)        
        ENDIF
     ELSE
        Gripper1 = TargetGripper1
     ENDIF

Hint: look in the code for the comment "Limit gripper max speed" to see what your code is currently doing.

Good luck
Kurt


Rookie ( offline )
Posts: 25
Posted: 2012-02-23 00:57 
Hi Lynx,

In my program I do not have this line.

abGOModifier bytetable 20, 40, 60, 80 ; Gripper Opening/Closing Speed (L1/L2)

My version is ps2arm1.bas

If I change my code to use abGOModifier, I need to put it somewhere. I put it in here. Please let me know if that is wrong.

;--------------------------------------------------------------------
;[TABLES]
GetACos bytetable 255,253,252,251,250,248,247,246,244,243,242,241,239,238,237,235,234,233,232,230,229,228,226,|
225,224,223,221,220,219,217,216,215,213,212,211,209,208,207,206,204,203,201,200,199,197,196,|
195,193,192,191,189,188,186,185,184,182,181,179,178,177,175,174,172,171,169,168,166,165,163,|
162,160,159,157,156,154,153,151,149,148,146,145,143,141,140,138,136,135,133,131,129,128,126,|
124,122,120,118,116,115,113,111,109,106,104,102,100,98,96,93,91,89,86,83,81,78,75,73,70,66,63,|
60,56,52,48,44,38,33,26,15,0

abGOModifier bytetable 20, 40, 60, 80 ; Gripper Opening/Closing Speed (L1/L2)
;--------------------------------------------------------------------


Rookie ( offline )
Posts: 25
Posted: 2012-02-24 00:56 
I also changed my code to try to accomodate the abGOModifier(ArmSpeedSelect) code.
the first section of code shown here is what I originally had and the second is what I changed it to, but when I try to compile the changed code I get an error. Do you know why?

This is the original code:
Code:
   IF (DualShock(2).bit2 = 0) AND (GripperPressure>GripperHoldPressure) THEN   ;L1 Button test
       Gripper1 = (Gripper1-80) min Gripper_MIN ;Gripper Close
   ENDIF
   
   IF (DualShock(2).bit0 = 0)THEN ;L2 Button test
       TargetGripper1 = (TargetGripper1+40) max Gripper_MAX ;Gripper Open
   ENDIF      
   
   IF (DualShock(2).bit3 = 0) THEN   ;R1 Button test
       TargetWristAngle1 = (TargetWristAngle1-40) min WristRotate_MIN ;Wrist Rotate CW
    ENDIF

     IF (DualShock(2).bit1 = 0) THEN ;R2 Button test
       TargetWristAngle1 = (TargetWristAngle1+40) max WristRotate_MAX ;Wrist Rotate CCW
   ENDIF   

   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


This is the changed code:
Code:
IF (DualShock(2).bit2 = 0) AND (GripperPressure>GripperHoldPressure) THEN   ;L1 Button test
       TargetGripper1 = (TargetGripper1-abGOModifier(ArmSpeedSelect)) min Gripper_MIN ;Gripper Close
   ENDIF

   IF (DualShock(2).bit0 = 0)THEN ;L2 Button test
       TargetGripper1 = (TargetGripper1+abGOModifier(ArmSpeedSelect)) max Gripper_MAX ;Gripper Open
   ENDIF      
   
   IF (DualShock(2).bit3 = 0) THEN   ;R1 Button test
       TargetWristAngle1 = (TargetWristAngle1-40) min WristRotate_MIN ;Wrist Rotate CW
    ENDIF

     IF (DualShock(2).bit1 = 0) THEN ;R2 Button test
       TargetWristAngle1 = (TargetWristAngle1+40) max WristRotate_MAX ;Wrist Rotate CCW
   ENDIF   

   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


User avatar
Guru ( offline )
Posts: 4913
Posted: 2012-02-24 10:53 
Error on which line?

Also did you change the code later on that checked to make sure the gripper was not going too fast?

Kurt


Rookie ( offline )
Posts: 25
Posted: 2012-02-25 14:22 
The program doesn't say which line the error is on.. also if the program didn't compile how would we check if the gripper was moving too fast?

and one more problem is that we bought a continuous servo for the base, but it doesn't stop when the remote control is released and it moves too fast. Do you know how we can make the continuous servo slow down and stop when the joystick is released?


User avatar
Guru ( offline )
Posts: 4913
Posted: 2012-02-25 14:52 
Can't help much with error, when I don't have your sources, or know what the error message is and which line it is complaining about...

As for moving rotating the wrist faster or closing the gripper faster, there has been a lot of discussions and code excerpts up on the thread: viewtopic.php?f=6&t=7923&start=45

As for continuous servos. They work differently. That is if the servo pulse is 1500us (more or less, may need to adjust what the zero point is per servo). The servo will stop rotating. If the value is less than this, it will rotate one direction, and if it is greater than this it will rotate the other direction. The farther away from the center point the faster it will rotate... To do this you will need to rework some of the code.
That is you need to look where the value of BaseAngle1 is used. Currently it is modified to be some angle, instead you need to to change this to be something like some delta speed from 1500 or simply a value in us.

You need to update where this value gets set...
Code:
   IF ABS(Dualshock(3) - 128) > DeadZone THEN ;Right Stick L/R
        BaseAngle1 = BaseAngle1 -(Dualshock(3) - 128)/abBSpeedDivisor(ArmSpeedSelect) ;Base Rotation
   ENDIF

This part may be as simple as setting the BaseAngle1 to some value centered around 1500 that is proportional to how far the joystick is moved... Could be something like:
Code:
   IF ABS(Dualshock(3) - 128) > DeadZone THEN ;Right Stick L/R
        BaseAngle1 = 1500  -(Dualshock(3) - 128) * 3 ;Base Rotation
    ELSe
        BaseAngle1 = 1500
   ENDIF

This would give you a range of us of something like: 1120-1880 (rounded...) which may be too much or too little. You can always experiment with different multipliers or combinations of multipliers and divisors...
Note: if you go this route you may want to rename the variable to something like BaseSpeed or the like. Also need to check other places where this value is set and probably update. Example: if old code set it to 0 you may need to set it to 1500. Also with this change you will need to change the movement function and change how the variable gets used... (Note: it will be a different variable name in the function...) But if you go the above route you will need to change the movement function to simply use the value passed in instead of what it is doing now and trying to convert degrees into us...

Good Luck
Kurt


1, 2, 3

All times are UTC - 5 hours [ DST ]. It is currently 2014-10-24 12:06
Feedback Form
Feedback Form