Lynxmotion AL5D robotic arm
I’m proud owner of Lynxmotion AL5D robotic arm. The parts kit is of very high quality, and as a result, the arm is very strong and versatile. I wanted my arm to be portable and independent of big computers and all currently available controllers lack flexibility that I needed, therefore I started building my own controller around Arduino platform. This article shows first preliminary result of this work – inverse kinematics code which would be used to position the arm.
In robotics, inverse kinematics is a method to position a tip of some linked stricture in 3D space by calculating joint angles from tip X, Y, and Z coordinates. Much information about the subject exists on the web, for example, this introductory article explains the subject using simple trigonometry.
To move the arm, six servos need to be controlled ( five for the arm without wrist rotate ). Given that large amount of processing time would be spent calculating servo angles, I decided not to drive servos directly from Arduino pins and made simple servo shield using Renbotics schematic and library code. I built only half of the circuit using single 4017 counter – this gives me seven servo control channels, which is plenty.
In addition to the article linked above, I’d like to mention two other resources, which helped me tremendously during code development. First is Micromega Application Note 44, which shows inverse kinematics equations for similar arm. They also have nice video of working arm. It should be noted that gripper of AL5D arm has much simpler geometry, therefore second order polynomial calculations are not necessary. The second one is this Lynxmotion project page with Excel spreadsheet. Many formulas from the spreadsheet were used in my code; I also used the spreadsheet during debugging after modifying arm dimensions.
Below is first working draft of inverse kinematics code. It can be used as-is or transformed into a library. As presented, it shall be used with caution – no boundary check is performed so it is quite easy to inadvertently send the arm flying into your forehead or the control board. The code uses single-precision floating point math, which seems to be adequate for the task.
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 | #include <servoShield.h> /* Servo control for AL5D arm */ /* Arm dimensions( mm ) */ #define BASE_HGT 67.31 //base hight 2.65" #define HUMERUS 146.05 //shoulder-to-elbow "bone" 5.75" #define ULNA 187.325 //elbow-to-wrist "bone" 7.375" #define GRIPPER 100.00 //gripper (incl.heavy duty wrist rotate mechanism) length 3.94" #define ftl(x) ((x)>=0?(long)((x)+0.5):(long)((x)-0.5)) //float to long conversion /* Servo names/numbers */ /* Base servo HS-485HB */ #define BAS_SERVO 0 /* Shoulder Servo HS-5745-MG */ #define SHL_SERVO 1 /* Elbow Servo HS-5745-MG */ #define ELB_SERVO 2 /* Wrist servo HS-645MG */ #define WRI_SERVO 3 /* Wrist rotate servo HS-485HB */ #define WRO_SERVO 4 /* Gripper servo HS-422 */ #define GRI_SERVO 5 /* pre-calculations */ float hum_sq = HUMERUS*HUMERUS; float uln_sq = ULNA*ULNA; ServoShield servos; //ServoShield object void setup() { servos.setbounds( BAS_SERVO, 900, 2100 ); servos.setbounds( SHL_SERVO, 1000, 2100 ); servos.setbounds( ELB_SERVO, 900, 2100 ); servos.setbounds( WRI_SERVO, 600, 2400 ); servos.setbounds( WRO_SERVO, 600, 2400 ); servos.setbounds( GRI_SERVO, 600, 2400 ); /**/ servos.start(); //Start the servo shield servo_park(); Serial.begin( 115200 ); Serial.println("Start"); delay( 500 ); } void loop() { //zero_x(); //line(); circle(); } /* arm positioning routine utilizing inverse kinematics */ /* z is height, y is distance from base center out, x is side to side. y,z can only be positive */ //void set_arm( uint16_t x, uint16_t y, uint16_t z, uint16_t grip_angle ) void set_arm( float x, float y, float z, float grip_angle_d ) { float grip_angle_r = radians( grip_angle_d ); //grip angle in radians for use in calculations /* Base angle and radial distance from x,y coordinates */ float bas_angle_r = atan2( x, y ); float rdist = sqrt(( x * x ) + ( y * y )); /* rdist is y coordinate for the arm */ y = rdist; /* Grip offsets calculated based on grip angle */ float grip_off_z = ( sin( grip_angle_r )) * GRIPPER; float grip_off_y = ( cos( grip_angle_r )) * GRIPPER; /* Wrist position */ float wrist_z = ( z - grip_off_z ) - BASE_HGT; float wrist_y = y - grip_off_y; /* Shoulder to wrist distance ( AKA sw ) */ float s_w = ( wrist_z * wrist_z ) + ( wrist_y * wrist_y ); float s_w_sqrt = sqrt( s_w ); /* s_w angle to ground */ //float a1 = atan2( wrist_y, wrist_z ); float a1 = atan2( wrist_z, wrist_y ); /* s_w angle to humerus */ float a2 = acos((( hum_sq - uln_sq ) + s_w ) / ( 2 * HUMERUS * s_w_sqrt )); /* shoulder angle */ float shl_angle_r = a1 + a2; float shl_angle_d = degrees( shl_angle_r ); /* elbow angle */ float elb_angle_r = acos(( hum_sq + uln_sq - s_w ) / ( 2 * HUMERUS * ULNA )); float elb_angle_d = degrees( elb_angle_r ); float elb_angle_dn = -( 180.0 - elb_angle_d ); /* wrist angle */ float wri_angle_d = ( grip_angle_d - elb_angle_dn ) - shl_angle_d; /* Servo pulses */ float bas_servopulse = 1500.0 - (( degrees( bas_angle_r )) * 11.11 ); float shl_servopulse = 1500.0 + (( shl_angle_d - 90.0 ) * 6.6 ); float elb_servopulse = 1500.0 - (( elb_angle_d - 90.0 ) * 6.6 ); float wri_servopulse = 1500 + ( wri_angle_d * 11.1 ); /* Set servos */ servos.setposition( BAS_SERVO, ftl( bas_servopulse )); servos.setposition( WRI_SERVO, ftl( wri_servopulse )); servos.setposition( SHL_SERVO, ftl( shl_servopulse )); servos.setposition( ELB_SERVO, ftl( elb_servopulse )); } /* move servos to parking position */ void servo_park() { servos.setposition( BAS_SERVO, 1715 ); servos.setposition( SHL_SERVO, 2100 ); servos.setposition( ELB_SERVO, 2100 ); servos.setposition( WRI_SERVO, 1800 ); servos.setposition( WRO_SERVO, 600 ); servos.setposition( GRI_SERVO, 900 ); return; } void zero_x() { for( double yaxis = 150.0; yaxis < 356.0; yaxis += 1 ) { set_arm( 0, yaxis, 127.0, 0 ); delay( 10 ); } for( double yaxis = 356.0; yaxis > 150.0; yaxis -= 1 ) { set_arm( 0, yaxis, 127.0, 0 ); delay( 10 ); } } /* moves arm in a straight line */ void line() { for( double xaxis = -100.0; xaxis < 100.0; xaxis += 0.5 ) { set_arm( xaxis, 250, 100, 0 ); delay( 10 ); } for( float xaxis = 100.0; xaxis > -100.0; xaxis -= 0.5 ) { set_arm( xaxis, 250, 100, 0 ); delay( 10 ); } } void circle() { #define RADIUS 80.0 //float angle = 0; float zaxis,yaxis; for( float angle = 0.0; angle < 360.0; angle += 1.0 ) { yaxis = RADIUS * sin( radians( angle )) + 200; zaxis = RADIUS * cos( radians( angle )) + 200; set_arm( 0, yaxis, zaxis, 0 ); delay( 1 ); } } |
Here is a short description of the code. The coordinate space follows one in Micromega AN44. set_arm() takes 3 coordinates and grip angle and sets servo angles for base, elbow, shoulder and wrist. Arm dimensions are defined in millimeters at the beginning of the sketch. Three short functions are written to demonstrate the functionality. zero_x() moves the arm in a straight line along the y-axis, line() moves the arm side to side and circle() moves the arm in a circle in y-z plane. In all 3 functions, wrist angle is set to 0 degrees, it can be changed to some other value to make motion more interesting.
Resources mentioned before the code listing are very helpful in understanding the calculations and problem solving approach. If they are not enough, please don’t hesitate to ask questions. I will be posting my progress as more code is developed. Stay tuned.
Oleg.
HI can you tell me if i want to use this for my custom arm which may have less no of servos, and different measurement of arm segment.
Thanks for advance
“Bone” sizes are defined as #define’s at the beginning of the sketch so you should have no problems here. As far as extra servos, this code doesn’t support wrist rotate anyway and if you don’t have wrist servo just take away code which adjusts for gripper angle.
how you decide servo bounds ? is it for that specific servo model ? or you decide on physical aspect of arm?
Servo bounds are for the specific joint. For example, shoulder should not be allowed to move back full swing.
what is co-ordinates of tip when it is in park position. It will help me change the code according to my arm
Park position is set in servo pulse widths. I set it even before attaching the arm to the Arduino using servo tester.
Excuse me Sir, Can I ask the abstract and the authors of this project? I will used it for some purpose as a Review on Related Literature since we are also making this kind of Robotic Arm. Please? thx.
The author’s name is Oleg Mazurov.
Love that arm, but do you have any video’s of it running? Would love to see it use the wrist and all at the same time.
The video is in the works – give me couple more days 🙂
Here -> http://www.youtube.com/watch?v=nz_tgDD8FNw
Very cool!
I think this is the only DIY robot arm using inverse kinematics I’ve seen on the web.
If I ever get around to finishing my own, I might be able to use some of this code.
Thanks!
thanks for quick replay, can u please explain your half shield and which value of that big cap you are using. I am confused of your wrist part. in code what is wrist angle? is it up down motion ? it seems from code that the change in length due to wrist movement, am i right ?
Does arm take shortest way from one point to another ? does code take care of case like this
I move from park to point A so if i give co-ordinates of point B can it go there or it always has to be from park point ?
How you decide the bounds for arm, is it just by checking the values of different extreme position? I want to say that how can i find it for my arm? was it given by lynxmotion? Please elaborate.
Sorry for lots of question but i think it will help me and a lot of people.
You ask too many questions in a single post – it’s hard to find time to write an answer to all of them at once, so I won’t 🙂
There is a link to excel spreadsheet with formulas hosted on lynxmotion.com; many things would become clear after studying it. The cap value is not crtitical – just use one which seems big enough.
Bounds are not decided. It’s a matter of simple geometrical calculations; I will implement it eventually. One bound is a sphere with length of the arm as radius and base joint as center.
As the other post rightly mentioned, this is the first time someone tamed the beast IK and caged inside Arduino.
Thanks for sharing.
HI I am thiking of making an automated drilling machine. how can i implement it.
hi
you could send me the code of the completed project??
got some functions that are not implemented in this topic
eg
servos.setposition (BAS_SERVO, ftl (bas_servopulse));
that ftl (bas_servopulse) does what??
ftl is float to long conversion. It is defined on line 11 of the code above
Hi,
you could direct me to where I can find the direct kinematics modeling for this robotic arm??
I really liked your code and wanted to verify the direct kinematics
set input parameters the angles and getting in return the position of arm
Look at the Lynxmotion project page (link in the article) – direct cinematic formulas are in Excel spreadsheets.
Hi,
i have an old lynxmotion arm i think its 5 :S but i dont have the servo shield. How can i modify your code to do invers kinematics with my arm?
The code doesn’t depend on particular servo shield, they all accept either angle or pulse width. You still need to drive servos somehow.
Ok, i understand thet, but i have a problem understanding the code. I attached my servos to PWM 2,3,4,5, i am not thet good in programing to change your code to drive servos from those locations. Where in the code are named locations of servos?
Thx for fast reply
They are all over the place; take a look at lines 13-25 for names and go from there.
Hi,i notice you are using six servos from the #define statements. Is it possible for me to use one controller to control 5 encoders( with will control H-bridges)simultaneously. I understand that i will need to use 5 interrupts, from your experience is there such a micro-controller out that will be capable of doing this, or close to this?
Sorry for being out of the topic but your assistance will be much appreciated.
I wrote an article recently about using pin change interrupt to read encoder -> http://www.circuitsathome.com/mcu/rotary-encoder-interrupt-service-routine-for-avr-micros . With this technique, reading 5 encoders is not a big task even for small micros like Atmega328 since you can configure almost any pin to be pin change interrupt source and there is a separate vector for each port.
(which will control 5 H-bridges)*
Hey
I am really new to arduino so i have a kind of a stupid question.What does the function void set_arm( float x, float y, float z, float grip_angle_d ) do? I mean if we give values of x , y, z and grip angle to the function, What will be the output of the function?
I am asking it because the funtions void zero_x(), void circle and void line are independent of the function void set_arm. So from where will the end result generate? (which will turn the motor in the specific angle). Hope you understand my question
functions generating motion coordinates calculate those coordinates and then call set_arm(), see, for example, circle() line 151. set_arm() output is pulses for servos, it does all inverse kinematics before that, i.e, calculates joint angles form coordinates.
Hey oleg
I have checked the link of the excel sheet you shared but it is for 2D arm. It only takes the values of x and y. Can you please share the algo or perhaps the equations that you used to find out the IK of the arm. Here is another excel sheet from lynxmotion and they have mentioned the equations they used. http://www.lynxmotion.com/images/html/proj058.htm
Thnx
Take a look at Micromega application note linked in the article – it explains necessary calculations.
Its quite confusing But i really liked your code. One question that i had in mind is where do we the values of x, y and z in your code?
set_arm(12.3,45.3,23.4,45)
is this the right method of inserting the x,y,z coordinates in the function so that the arm moves to the required x,y,z coordinates? if it is the right method then what is the need for void circle, void line and void zero_x ?
set_arm() takes x,y,and z coordinates. circle() generates coordinates so arm moves in a circle. You can also enter coordinates directly, if you’d like.
There are no pins defined in this code. I mean which four pins should be used to connect arduino to the servo shield.
Thank you so much for giving us your time. Really appreciate it.
Hi,
When I compare http://www.micromegacorp.com/downloads/documentation/AN044-Robotic%20Arm.pdf with your work, I can see the connections – and for the most part I understand how most of the figures are derived.
However the PDF appeared to rush and skip detailed explanations of how the shoulder and wrist angles are determined (my maths isnt that strong).
Do you have time to break it down for me?
Regards
Michael
Joints are all the same. A joint and two “bones” plus distance between far ends of two bones form a triangle. An arm is treated as a set of triangles in x,z plane, rotating base adds y plane.
I’m using 6 servos directly to the Arduino port (2-8). To control them servo.h use the library, providing for each servant, an angle of 0-180 °.
What changes should I do in the code since I’m using this library and not servoshield.h?
IF i’m using the servo.h library, what i need to change in lines 93-96?
Did you notice an error in the Application Note 44 article? I believe their calculations for r’ and z’ mix up the use of cosine and sine. I assume you corrected this in your software, which I’ll be reviewing shortly. I have all of the same components and will be testing your code in a few days. Your explanation is very clear and helpful. Thanks for putting this together.
One last question, why did you not use the SSC-32 servo controller that comes with the LynxMotion arm? It seems to me this would be a lot easier than making your own servo controller.
I don’t remember. There are several ways to calculate angles and I was looking at many code samples to find one which can be done in the least amount of machine cycles. My calculations are based on formulas from one of the spreadsheets posted on Lynxmotion site.
I think you can buy Lynxmotion kit without SSC-32. Servo controllers are simple circuits – I had mine lying around and just used it.
With a few modifications, the code you posted works fine with the SSC-32 servo controller. I’ll need to go through the calculations in a little more detail to see how you implemented them. The SSC-32 doesn’t come with a nice servo library like the one you used, but it isn’t that hard to write out the commands I need. It’s a blast to see your arm finally doing what you command it to do! Now if I could just have it make breakfast for me in the morning!
Hi,
I am trying to use your code in my project. I really like it, you have commented everything and it’s readable. I changed the length of the “bones” and the servo limits, what I don’t understand is this part in your code:
/* Servo pulses */
float bas_servopulse = 1500.0 – (( degrees( bas_angle_r )) * 11.11 );
float shl_servopulse = 1500.0 + (( shl_angle_d – 90.0 ) * 6.6 );
float elb_servopulse = 1500.0 – (( elb_angle_d – 90.0 ) * 6.6 );
float wri_servopulse = 1500 + ( wri_angle_d * 11.1 );
Can you explain more, please ? I don’t get what are these values.
Thanks a lot,
Chris.
That’s transformations from joint angles to servo pulse lengths. 1500 is 1.5ms which corresponds to middle position of a servo. 11.1, 6.6, etc., are constants, like “degrees per millisecond” ratios. The rest are actual angles of various joints and direction of rotation.
sir,
how can i build this and where i can purchase the projects matter ??
hii good job. I want to list a materials this project. I will make it finaly project my university. ı please to answer
i need to ask what will be the total cost of this project.please count every component because i’m a student
My arm seems to be acting erratic running the inverse kinematics. For instance when I run just the line() function the elbow and shoulder move back and forth slightly. Also, sending custom set_arm( 100, 250, 100, X ) commands where I only change the grip angle the whole arm moves to different positions.
I’ve got another script that controls the arm using your usb host shield and ps3 remote without any erratic behavior.
Any idea what I should look at?
Arm coordinates are set to the tip of the gripper so if you change grip angle the whole arm needs to move to keep the tip in the same place – so this is correct behaviour. The biggest discrepancy between builds is angles (degrees per ms) of servos used by builders. Check the code which works to see if it uses different angles.
I’m sorry, I want to ask why there is an error when I convert basepulse=(int) bas_servopulse; ?…and print it to serial..can’t display the value…
Can you tell me the difference between grip angle and wrist angle?
Hi Oleg,
great work! Really enjoyed this as I have just set out to do something similar myself. The fun in a project like this is coming up with a solution yourself but now I know where to get some hints in case I get stuck 😉
Thanks for sharing!
hi oleg.
Love the code, so much so that i am porting it into visual basic and inputting the x, y and z values from an xbox kinect. making a manipulator of my own arm. my question is, what are the limits of of the incoming variables x, y, z @ eg 0-100 for y, +50 to -50 for x etc, any help is much appreaciated.
Limits are specific to a particular arm, you need to check them before feeding them into IK.
Hi Oleg,
I really liked your code, I want to make a DIY robot arm as well.
Thank you for sharing!
Hey, Fantastic code. I HAVE SLIGHTLY MODIFIED THE CODE based on servo.h. Can you tell me how to send x,y,z co-ordinates via serial communication via arduino to make the gripper reach the desired position?
call set_arm() with gripper tip coordinates as parameters
Hey, can you please explain me line 69 and 70 ie. grip angle offsets. Finally, if i want to send x,y,z coordinates via serial communication through arduino, is this code correct ?
Void loop()
{
If(serial.available()> 0)
{
X = serial.read();
Y = serial.read();
Z = serial.read();
set_arm ( X,Y,Z, 0);
}
}
Will this code work? Any corrections would be highly welcome.
The 3 coordinates are of the gripper’s tip. If you change the gripper’s angle you need to account for the offset.
And what about the code stated above for serial commumnication? Is it correct?
I don’t think so – it will read the first entered symbol and then attempt to read an empty buffer twice.
So, can you kindly tell me the code to send x,y,z co-ordinates serially please?
if you had add a picture with the words and points where is each angle, it could be easier to understand.
App.note linked in the article has plenty of pictures.
Hello Oleg,
I’m working on inverse kinematics with an AL5D and really appreciated your post, it’s super helpful!
I have a small question, similar to the posting from 30. Nov 2011. You are using the constants 11.11, 6.6 and 11.1 as multipliers for the pulse width. How/where did you find these values? I assume they are servo specific, but I can’t figure it out, so I would be really glad if you could give me a hint!
Jack
it’s been a long time since I wrote this so I don’t remember exactly why I did it this way. The idea is to obtain a time interval for a certain servo angle, the constants are “milliseconds per degree” values for 90deg and 180deg servos.
Thanks for your reply Oleg!!
From the servo specs (Hitec servos) I would have guessed 1 degree is 10 microseconds. I will play around with the value and see what happens!
Hi,
I have a robotic arm but unfortunately it uses regular dc motors instead of servos. I was wondering if it is possible to adapt your code to send motor commands instead of servo commands?
Thanks,
Mark
Hi Mark,
If you can command your arm to set certain joint angles then you can use this code.
Hello Oleg, you talk on Russian?
I do.
скажите а в каком програмном обеспечении Вы писали управляющую программу?
Программа написана на С++ для Ардуино IDE.
Hi,
I want to have a robotic arm where i give a particular coordinates as a command and the ram moves to that location . Does this code achieve this??. Basically i also want to know how do you extract the information of your coordinates?.
скажите пожалуйста Олег, а возможно ли таким манипулятором выполнять более сложные операции чем движение по кругу или радиусу? Цель моих вопросов уточнить возможность использования манипулятора как фрезерный станок 3д! у меня усть контроллер степ-драйв с шаговыми двигателями, ну и плюс станок фрезерный с чпу. Возможно я не очень понятно излагаю свои мысли, просто не хочу много информации сразу выкладывать что бы не отнимать много у Вас времени!
Можно двигать произвольно, типа так -Ю http://www.youtube.com/watch?v=nz_tgDD8FNw . Фрезерный станок из этой руки не получится – слишком слабая.
hello oleg. i have some questions about basic things because the tool is differ. i’m using processing and arduino. and i’m not using arduino shield but using uno board connected 6 servo motor. anyway, on the function named set_arm, what is the value of gripper? is this motor installed robots’ wrist? or motor controlling robots’ finger?
and because this code using servoshield code, it uses pulse value(ex. 600-2400). is this able to convert 0-180, this means i want to send value of angle to robot-arm directly and confortable.
It’s gripper angle. Wrist is gripper rotation.
is it important use middle sheild that us um_fpu64??
which program you use to enter the servo library??
iwant better circuit for servo sheild?
Hello,
I am building a robotic arm with renbotic shield like yours. however, whenever i try to run your code (as sample code), it return to me some errors missing function.
D:\arduino-1.5.6-r2\sketchbook\libraries\USB_Host_Shield_20/Max3421e.h:18:2: error: #error “Never include max3421e.h directly; include Usb.h instead”
robotic_arm.ino: In function ‘void setup()’:
robotic_arm.ino:72: error: ‘class MAX3421E’ has no member named ‘powerOn’
robotic_arm.ino: In function ‘void mouse_init()’:
robotic_arm.ino:108: error: ‘class USB’ has no member named ‘setDevTableEntry’
robotic_arm.ino:108: error: ‘class USB’ has no member named ‘getDevTableEntry’
robotic_arm.ino: In function ‘byte mouse_poll()’:
robotic_arm.ino:128: error: ‘class USB’ has no member named ‘getReport’r
strangely, I check those H file and cpp file to be sure and there are functions with those name. do you know what could go wrong?
thanks
This is strange since code on this page doesn’t use USB host library. Where did you get the code you’re trying to compile?
I am getting the same error several times trying to compile PTPDevInfo, using code from GitHub
Hello Oleg!
This is amazing work! And very useful to me 🙂 I have one question, I am going through Note 44 and your code, you have atan2(x,y) instead of atan2(y,x) (to match the atan2(y/x) calculation). Can you please tell me why you have it the other way round? 🙂
Many thanks!
Charlotte
I don’t remember why I did that. Are you getting correct results from the code? Try to flip parameters and see what happens, just keep your distance from the arm since it can hit you.
Hey!
Thanks for responding! I’m not sure if it is correct or not haha. My robot was built by scratch and I am using the Pololu Mini Maestro so trying to make it work with your code – I’m getting close
Many thanks again
Charlotte
Hey!
Me again 🙂 What do the values you’ve chosen do to the angles?
float bas_servopulse = 5000.0 – (( qRadiansToDegrees(bas_angle_r )) * 11.11 );
float shl_servopulse = 5000.0 + (( shl_angle_d – 90.0 ) * 6.6 );
float elb_servopulse = 5000.0 – (( elb_angle_d – 90.0 ) * 6.6 );
float wri_servopulse = 5000 + ( wri_angle_d * 11.1 );
Please ignore qRadiansToDegreess, I am just using Qt Creator 🙂
Many thanks!
Charlotte
you mean 11.11, 6.6? This are coefficients for different servos.
So do they just make the movement big enough for the servos? So they will move to the right angle? How did you find these values?
Many thanks!
Servos all accept 1-2ms pulse but different servos turn different amount of degress (per some unit time). IIRC these two numbers are for 90deg and 180deg servos.
Олег, здравствуйте!
Не могли бы вы дать свой контакт… что бы задать пару вопросов. заказал себе миниатюрную руку и ардуино уно. И не получается использовать ваш скетч. Естественно я поменял код… сделал с использованием обычной библиотеки серво.
Пишите сюда.
Рука ведет себя непонятным образом. Уже пересобрал так что бы при утановки 90 градусов на сервоприводах, манипулятор вставал буквой Г со сгибом в локте. Купил транспортир и в код добовил что бы информация в порт писалась – используя эти инструменты пытался понять в чем дело…
Ваши начальные значения соответствуют тем что у робота Lynx6 Robotic Arm? Положение руки при 90 градусах на сервоприводах, увеличение угла тоже вниз щуп двигает?
grip_angle_d – это как я понимаю угол относительно плоскости xy в которой рука поворачивается?
у меня вот такой китаец и ардуино уно.
http://robotbase.en.alibaba.com/product/1162909747-217010484/AS_6_DOF_aluminium_robotic_arm_educational_robot_with_Electric_Controller_Robot_contest_recommendation.html
Код который пользую для отладки
#include
#include
/* Arm dimensions( mm ) */
#define BASE_HGT 82.00 //высота базы 2.65″
#define HUMERUS 92.00 //высота от плеча до локтя 5.75″
#define ULNA 92.00 //длинна от локтя до запястия 7.375″
#define GRIPPER 125.00 //длинна кисти (захвата) 3.94″
#define ftl(x) ((x)>=0?(long)((x)+0.5):(long)((x)-0.5)) //переводим float в long
/* pre-calculations */
float hum_sq = HUMERUS*HUMERUS;
float uln_sq = ULNA*ULNA;
Servo W1;
Servo W2;
Servo W3;
Servo W4;
Servo W5;
Servo W6;
void setup() {
Serial.begin(115200);
W1.attach(3); //вращает базу
W2.attach(4); //плече
W3.attach(5); //локоть
W4.attach(6); //запястие
W5.attach(7); //поворот кисти (щупа)
W6.attach(8); //кисть (щуп)
}
String inString;
void loop()
{
while (Serial.available() > 0) {
int inChar = Serial.read();
if (inChar == ‘!’) {
Serial.println(inString);
int SR1 = inString.substring(0,3).toInt();
delay(5);
int SR2 = inString.substring(3,6).toInt();
delay(5);
int SR3 = inString.substring(6,9).toInt();
delay(5);
Serial.println(SR1);
Serial.println(SR2);
Serial.println(SR3);
//
set_arm(SR1,SR2,SR3,0);
inString = “”;
break;
}
inString += (char)inChar;
}
}
void set_arm( float x, float y, float z, float grip_angle_d )
{
float grip_angle_r = radians( grip_angle_d ); //переводим угол в радианы
/* Угол базы и радиальное расстаяние от x,y координат */
float bas_angle_r = atan2( x, y );
float rdist = sqrt(( x * x ) + ( y * y ));
/* rdist это вроде как радиус проекции руки если смотреть сверху */
y = rdist;
/* Поворот кисти(щупа) в зависимости от угла захвата */
float grip_off_z = ( sin( grip_angle_r )) * GRIPPER;
float grip_off_y = ( cos( grip_angle_r )) * GRIPPER;
/* Позиция запястия */
float wrist_z = ( z – grip_off_z ) – BASE_HGT;
float wrist_y = y – grip_off_y;
/* расстояние от плеча до запястия */
float s_w = ( wrist_z * wrist_z ) + ( wrist_y * wrist_y );
float s_w_sqrt = sqrt( s_w );
/* s_w угол относительно земли (или оси x) */
//float a1 = atan2( wrist_y, wrist_z );
float a1 = atan2( wrist_z, wrist_y );
/* s_w угол к плечевой кости */
float a2 = acos((( hum_sq – uln_sq ) + s_w ) / ( 2 * HUMERUS * s_w_sqrt ));
/* угол плеча */
float shl_angle_r = a1 + a2;
float shl_angle_d = degrees( shl_angle_r );
/* угол локтя */
float elb_angle_r = acos(( hum_sq + uln_sq – s_w ) / ( 2 * HUMERUS * ULNA ));
float elb_angle_d = degrees( elb_angle_r );
float elb_angle_dn = -( 180.0 – elb_angle_d );
/* угол запястья */
float wri_angle_d = ( grip_angle_d – elb_angle_dn ) – shl_angle_d;
/* Servo pulses */
float bas_servopulse = 1500.0 – (( degrees( bas_angle_r )) * 11.1 );
float d1 = 90 – degrees( bas_angle_r );
Serial.println(degrees( bas_angle_r));
Serial.println(d1);
float shl_servopulse = 1500.0 + (( shl_angle_d – 90.0 ) * 6.6 );
float d2 = 90 + ( shl_angle_d – 90.0 );
Serial.println(shl_angle_d);
Serial.println(d2);
float elb_servopulse = 1500.0 – (( elb_angle_d – 90.0 ) * 6.6 );
float d3 = 90 – ( elb_angle_d – 90.0 );
Serial.println(elb_angle_d);
Serial.println(d3);
float wri_servopulse = 1500 – ( wri_angle_d * 11.1 );
float d4 = 90 + wri_angle_d;
Serial.println(wri_angle_d);
Serial.println(d4);
int val1 = ftl( bas_servopulse );
//val1 = map(val1, 0, 1023, 0, 179);
W1.writeMicroseconds(val1);
Serial.println(val1);
delay(100);
int val2 = ftl( shl_servopulse );
//val3 = map(val3, 0, 1023, 0, 179);
W2.writeMicroseconds(val2);
Serial.println(val2);
delay(100);
int val3 = ftl( elb_servopulse );
//val6 = map(val6, 0, 1023, 0, 179);
W3.writeMicroseconds(val3);
Serial.println(val3);
delay(100);
int val4 = ftl( wri_servopulse );
//val2 = map(val2, 0, 1023, 0, 179);
W4.writeMicroseconds(val4);
Serial.println(val4);
delay(100);
}
Можно сервы по одной отлаживать. Начать лучше с той, которая в основании.
Ось координат проходит через ось в оосновании, чем от нее дальше, тем координата больше. Подробностей не помню.
Hi i am trying to use an IMU sensor with this code and make the robotic arm mimic my arm movements. please can you tell me a few things:
1. what is the use of servos.setbounds()
2. will this work if I dont use a servo shield and how can I modify this code to use it without a shield.
3. how can I use the values of the IMU sensor.
4. Plus I just want the movement for the elbow, shoulder and base as I have other plans to use the griper and the wrist
Hi,
Your project is really cool. I found this at right time. Thanks for sharing this contents. I’m having 2 doubts..
1) Can u tell me the x,y,z axis in a 3d orientation as per ur program (like Breadth, Width, Height).
2) Why z & y is not taking negative axis and x is taking both.
I’ll find useful if you replied……. Thank u so much for the contents..:-)
Hi Oleg,
I ve got following problem with your code:
1) while moving on the X direction only, the gripper goes not linearly (stright line) but it moves on a circle.
2) while moving on the Y direction only, farer the gripper goes, less values are incremented
Do you have any idea, what it could be?
Kind regards
Brankoo
PS: great job
Hi Oleg,
thanks for this piece of code. I have adapted it to a different robot arm (simulated so far) and it seems to work well. I’ve put some more description on how it works at my web site:
http://rg-asc.ziti.uni-heidelberg.de/sites/default/files/docs/esr/robot.pdf
Andreas
… forgot the link to where the robot operation can be viewed:
http://rg-asc.ziti.uni-heidelberg.de/en/esr#robot
Andreas
Hi,
Can you provide details about which Arduino you used? and also the renbotics schematics etc as that link is broken now/account suspended! not your fault i know but it would be good if you could provide full information.
Thanks
Chris