Biorobotics 7 / Mbed 2 deprecated State_Machine

State machine

Dependencies:   mbed Adafruit_GFX BioroboticsMotorControl MODSERIAL BioroboticsEMGFilter

forward_kinematics.h@50:5d2176b93a65, 2018-11-05 (annotated)

Committer:
MAHCSnijders
Date:
Mon Nov 05 10:14:31 2018 +0000
Revision:
50:5d2176b93a65
Parent:
22:720a410c4980 
With new comments

Who changed what in which revision?

UserRevisionLine numberNew contents of line
brass_phoenix 20:af1a6cd7469b 1 #pragma once
brass_phoenix 20:af1a6cd7469b 2
brass_phoenix 20:af1a6cd7469b 3 #include "mbed.h"
brass_phoenix 20:af1a6cd7469b 4 #include "constants.h"
brass_phoenix 20:af1a6cd7469b 5
brass_phoenix 20:af1a6cd7469b 6
MAHCSnijders 50:5d2176b93a65 7 // Pass the two values that you want the x and y outputs in, as 3rd and 4th arguments (3rd and 4th arguments are outpurs).
brass_phoenix 20:af1a6cd7469b 8 void forward_kinematics(double main_angle, double sec_angle, double &output_x, double &output_y)
brass_phoenix 20:af1a6cd7469b 9 {
brass_phoenix 20:af1a6cd7469b 10 // Calculation of position joint 1 expressed in frame 0
brass_phoenix 20:af1a6cd7469b 11 float J1x_0 = L6 + L0 + L1*cos(sec_angle);
brass_phoenix 20:af1a6cd7469b 12 float J1y_0 = L1*sin(sec_angle);
brass_phoenix 20:af1a6cd7469b 13
brass_phoenix 20:af1a6cd7469b 14 // Calculation of position joint 3 expressed in frame 0
brass_phoenix 20:af1a6cd7469b 15 float J3x_0 = L6 + L4*cos(main_angle);
brass_phoenix 20:af1a6cd7469b 16 float J3y_0 = L4*sin(main_angle);
brass_phoenix 20:af1a6cd7469b 17
brass_phoenix 20:af1a6cd7469b 18 // Calculation of Joint 2 expressed in frame 2
brass_phoenix 20:af1a6cd7469b 19 float m_y = J3y_0 - J1y_0;
brass_phoenix 20:af1a6cd7469b 20 float m_x = J1x_0 - J3x_0;
brass_phoenix 20:af1a6cd7469b 21 float m = sqrt(pow(m_y,2) + pow(m_x,2)); // Radius between Joint 1 and 3
brass_phoenix 20:af1a6cd7469b 22 float delta = acos(- ( pow(m,2) - pow(L2,2) - pow(L3,2))/(2*L2*L3) );
brass_phoenix 20:af1a6cd7469b 23 float mu = acos( (pow(L2,2) - pow(L3,2) + pow(m,2))/(2*m*L2) ); // Angle between L2 and m
brass_phoenix 20:af1a6cd7469b 24
brass_phoenix 20:af1a6cd7469b 25 float t_y = J3y_0;
brass_phoenix 20:af1a6cd7469b 26 float t_x = (L0 + L6) - J3x_0;
brass_phoenix 20:af1a6cd7469b 27 float t = sqrt(pow(t_y,2) + pow(t_x,2)); // Radius between frame 1 and Joint 3
brass_phoenix 20:af1a6cd7469b 28 float phi = acos( (pow(L1,2) - pow(t,2) + pow(m,2))/(2*m*L1) ); // Angle between L1 and m
brass_phoenix 20:af1a6cd7469b 29
brass_phoenix 20:af1a6cd7469b 30 float q2 = PI - mu - phi; // Angle that L2 makes in frame 2
brass_phoenix 20:af1a6cd7469b 31 float J2x_2 = L2*cos(q2);
brass_phoenix 20:af1a6cd7469b 32 float J2y_2 = L2*sin(q2);
brass_phoenix 20:af1a6cd7469b 33
brass_phoenix 20:af1a6cd7469b 34 // Calculation of Joint 2 expressed in frame 0
brass_phoenix 20:af1a6cd7469b 35 float J1x_1 = L1*cos(sec_angle); // Joint 1 expressed in frame 1
brass_phoenix 20:af1a6cd7469b 36 float J1y_1 = L1*sin(sec_angle);
brass_phoenix 20:af1a6cd7469b 37 float J2x_0 = J2x_2*cos(sec_angle) - J2y_2 * sin(sec_angle) + J1x_1 + L0 + L6; // Joint 2 expressed in frame 0
brass_phoenix 20:af1a6cd7469b 38 float J2y_0 = J2x_2*sin(sec_angle) + J2y_2 * cos(sec_angle) + J1y_1;
brass_phoenix 20:af1a6cd7469b 39
brass_phoenix 20:af1a6cd7469b 40 // Calculation of End-effector
brass_phoenix 22:720a410c4980 41 float f_x = J2x_0 - L6;
brass_phoenix 20:af1a6cd7469b 42 float f_y = J2y_0;
brass_phoenix 20:af1a6cd7469b 43 float f = sqrt(pow(f_x,2) + pow(f_y,2)); // Radius between motor 1 and Joint 2
brass_phoenix 20:af1a6cd7469b 44 float xhi = acos( -(pow(f,2) - pow(L3,2) - pow(L4,2))/(2*L3*L4) ); // Angle between L3 and L4
brass_phoenix 20:af1a6cd7469b 45 float omega = PI - xhi; // Angle between L4 and L5
brass_phoenix 20:af1a6cd7469b 46 float n = sqrt(pow(L4,2) + pow(L5,2) - 2*L4*L5*cos(omega)); // Radius between end effector and motor 1
brass_phoenix 20:af1a6cd7469b 47
brass_phoenix 20:af1a6cd7469b 48 float theta = acos( (pow(L4,2) - pow(L5,2) + pow(n,2))/(2*n*L4) ); // Angle between n and L4
brass_phoenix 20:af1a6cd7469b 49 float rho = PI - theta - main_angle; // Angle between n and L4
brass_phoenix 20:af1a6cd7469b 50
brass_phoenix 20:af1a6cd7469b 51 float Pe_x = L6 - n*cos(rho); // y-coordinate end-effector
brass_phoenix 20:af1a6cd7469b 52 float Pe_y = n*sin(rho); // x-coordinate end-effector
brass_phoenix 20:af1a6cd7469b 53
brass_phoenix 20:af1a6cd7469b 54
brass_phoenix 20:af1a6cd7469b 55 output_x = Pe_x;
brass_phoenix 20:af1a6cd7469b 56 output_y = Pe_y;
brass_phoenix 20:af1a6cd7469b 57 }