Met kinematics
Dependencies: MODSERIAL QEI mbed-dsp mbed
Fork of Motorcode by
Revision 10:d78f5d556cf7, committed 2018-10-31
- Comitter:
- Jopper0575
- Date:
- Wed Oct 31 08:12:31 2018 +0000
- Parent:
- 9:466dff9ae128
- Commit message:
- Foward & Backward kinematics
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Fri Oct 05 12:51:06 2018 +0000 +++ b/main.cpp Wed Oct 31 08:12:31 2018 +0000 @@ -29,6 +29,7 @@ const float sample_time = 0.05; //s const float sample_frequency = 200; //hz + double countstoangle(int counts){ double angle; int counts_abs = abs(counts); @@ -42,6 +43,50 @@ } } + +// KINEMATICS +// Variables +const int L1 = 300; // Length arm 1 in mm +const int L2 = 482; // Length arm 2 in mm + +// Gets current postition of end effector | FOWARD KINEMATICS +double getX1pos(double q1){ // X-pos of joint + return x1_c = L1*cosd(q1); +} +double getY1pos(double q1){ // Y-pos of joint + return y1_c = L1*sind(q1); +} +double getX2pos(double q1, double q2){ // X-pos of end-effector + double x1_c = getX1pos(q1) + return x2_c = x1_c + L2*cosd(q1+q2); +} +double getY2pos(double q1, double q2){ // Y-pos of end-effector + double y1_c = getY1pos(q1) + return y2_c = y1_c + L2*sind(q1+q2); +} + +// Calculate new position | BACKWARD KINEMATICS +double getNewq2(double x2_d, double y2_d){ // requires desired x and y position of end-effector + double frac = ( pow(x2_d,2) + pow(y2_d,2) - pow(L1,2) - pow(L2,2) ) / ( 2 * L1 * L2 ); // define kinematic cosine fraction for easier calculation + double q2_d = atan2d( -sqrt(1 - pow(frac,2)) , frac ); // new desired q2 joint angle + return q2_d; +} +double getNewq1(double x2_d, double y2_d){ // requires desired x and y position of end-effector + double q2_d = getNewq2(x2_d, y2_d); // first calculate q2 + double k1 = L1 + L2*cosd(q2_d); // kinematic relation 1 + double k2 = L2*sind(q2_d); // kinematic relation 2 + double q1_d = atan2d( y2_d, x2_d ) - atan2d( k2, k1); // new desired q1 joint angle + return q1_d; +} +double getNewq3(double x2_d, double y2_d){ // Cup holder + double q3_d = getNewq2(x2_d, y2_d) + getNewq1(x2_d, y2_d); // q2_d+q1_d +} + + + + + + int main() { ledr = 1;