Anke Post
Inverse_kinematica
Dependencies: mbed QEI HIDScope biquadFilter MODSERIAL FastPWM
Diff: main.cpp
- Revision:
- 4:739136c479bd
- Parent:
- 3:710f993d5848
- Child:
- 5:ac4b1c0366b3
--- a/main.cpp Tue Oct 29 08:11:46 2019 +0000
+++ b/main.cpp Tue Oct 29 16:36:54 2019 +0000
@@ -1,78 +1,49 @@
#include "mbed.h"
-//#include "HIDScope.h"
-//#include "QEI.h"
-#include "MODSERIAL.h"
-//#include "BiQuad.h"
-//#include "FastPWM.h"
#define M_PI 3.14159265358979323846 /* pi */
#include <math.h>
-double vx = 5; //Gelijk aan variabele die snelheid aangeeft in EMG (in x-richting)
-double vy = 3; //Gelijk aan variabele die snelheid aangeeft in EMG (in y-richting)
+double vx; //Gelijk aan variabele die snelheid aangeeft in EMG (in x-richting)
+double vy; //Gelijk aan variabele die snelheid aangeeft in EMG (in y-richting)
+
+double Joint_velocity_x[2][1];
+double Joint_velocity_y[2][1];
+
+// Defining motor angles
+double M1 = 20*M_PI/180; //Vervang 20 door encoder_value_motor_1 (deg), de naam die is gegeven aan hoek die uit encoder counts is gekomen, current encoder position
+double M2 = 30*M_PI/180; //Zelfde als hierboven, current encoder position
+
+// Calculating joint angles based on motor angles (current encoder values)
+double q1 = M1; //Relative angle joint 1 (rad)
+double q2 = M2 - M1; //Relative angle joint 2 (rad)
void Inverse_Kinematics()
{
- // Defining variables used for Brockett
- const double x_coordinate_in_reference = 0.14; //Nog invullen!! x-coordinate endpoint in reference (m)
- const double y_coordinate_in_reference = 0; //Nog invullen!! y-coordinate endpoint in reference (m)
+ // Defining joint velocities based om calculations of matlab
- const double phi_d = 20; //Nog invullen!! Absolute angle joint 1 in reference (deg)
- const double phi = phi_d*M_PI/180; //(rad)
-
- // Defining motor angles
- double M1 = 20*M_PI/180; //Vervang 20 door encoder_value_motor_1 (deg), de naam die is gegeven aan hoek die uit encoder counts is gekomen, current encoder position
- double M2 = 30*M_PI/180; //Zelfde als hierboven, current encoder position
+ Joint_velocity_x[0][0] = (vx*(exp(q2*sqrt(-1.0))*(-1.798599718146044E18+6.546367607647411E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*sqrt(-1.0))*(-1.798599718146044E18-6.546367607647411E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*2.0*sqrt(-1.0))*(1.168095770314175E18+1.321169505615574E18*sqrt(-1.0))+exp(q1*sqrt(-1.0))*exp(q2*sqrt(-1.0))*cos(q1+3.141592653589793/9.0)*3.828059683264922E18+1.168095770314175E18-1.321169505615574E18*sqrt(-1.0))*(2.0E1/1.7E1))/(cos(q1+3.141592653589793/9.0)*(6.605847528077872E17+5.840478851570874E17*sqrt(-1.0))+sin(q1+3.141592653589793/9.0)*(-5.840478851570874E17+6.605847528077872E17*sqrt(-1.0))+exp(q2*sqrt(-1.0))*cos(q1+3.141592653589793/9.0)*(-3.273183803823705E17-8.992998590730221E17*sqrt(-1.0))+exp(q2*sqrt(-1.0))*sin(q1+3.141592653589793/9.0)*(8.992998590730221E17-3.273183803823705E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*sqrt(-1.0))*cos(q1+3.141592653589793/9.0)*(-3.273183803823705E17+8.992998590730221E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*2.0*sqrt(-1.0))*cos(q1+3.141592653589793/9.0)*(6.605847528077872E17-5.840478851570874E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*sqrt(-1.0))*sin(q1+3.141592653589793/9.0)*(8.992998590730221E17+3.273183803823705E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*2.0*sqrt(-1.0))*sin(q1+3.141592653589793/9.0)*(-5.840478851570874E17-6.605847528077872E17*sqrt(-1.0)));
+ Joint_velocity_x[1][0] = (vx*(exp(q2*sqrt(-1.0))*(1.798599718146044E18-6.546367607647411E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*sqrt(-1.0))*(1.798599718146044E18+6.546367607647411E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*2.0*sqrt(-1.0))*(-1.168095770314175E18-1.321169505615574E18*sqrt(-1.0))+-1.168095770314175E18+1.321169505615574E18*sqrt(-1.0))*(2.0E1/1.7E1))/(cos(q1+3.141592653589793/9.0)*(6.605847528077872E17+5.840478851570874E17*sqrt(-1.0))+sin(q1+3.141592653589793/9.0)*(-5.840478851570874E17+6.605847528077872E17*sqrt(-1.0))+exp(q2*sqrt(-1.0))*cos(q1+3.141592653589793/9.0)*(-3.273183803823705E17-8.992998590730221E17*sqrt(-1.0))+exp(q2*sqrt(-1.0))*sin(q1+3.141592653589793/9.0)*(8.992998590730221E17-3.273183803823705E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*sqrt(-1.0))*cos(q1+3.141592653589793/9.0)*(-3.273183803823705E17+8.992998590730221E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*2.0*sqrt(-1.0))*cos(q1+3.141592653589793/9.0)*(6.605847528077872E17-5.840478851570874E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*sqrt(-1.0))*sin(q1+3.141592653589793/9.0)*(8.992998590730221E17+3.273183803823705E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*2.0*sqrt(-1.0))*sin(q1+3.141592653589793/9.0)*(-5.840478851570874E17-6.605847528077872E17*sqrt(-1.0)));
- // Calculating joint angles based on motor angles (current encoder values)
- double q1 = M1; //Relative angle joint 1 (rad)
- double q2 = M2 - M1; //Relative angle joint 2 (rad)
-
- // Defining variables used for twists in Jacobian
- const double x1 = 0; //x-position of motors (first joint) from the bottom coordinate system (m)
- const double y1 = 0.148; //y-position of motors (first joint) from the bottom coordinate system (m)
-
- double x2 = x1 + 0.425*cos(phi+q1); //x-position of the second joint expressed in the angle of the first joint (m)
- double y2 = y1 + 0.425*sin(phi+q1); //y-position of the second joint expressed in the angle of the first joint (m)
+ Joint_velocity_y[0][0] = (vy*(exp(q2*sqrt(-1.0))*(-3.273183803823705E17-8.992998590730221E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*sqrt(-1.0))*(-3.273183803823705E17+8.992998590730221E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*2.0*sqrt(-1.0))*(6.605847528077872E17-5.840478851570874E17*sqrt(-1.0))+exp(q1*sqrt(-1.0))*exp(q2*sqrt(-1.0))*sin(q1+3.141592653589793/9.0)*1.914029841632461E18+6.605847528077872E17+5.840478851570874E17*sqrt(-1.0))*(4.0E1/1.7E1))/(cos(q1+3.141592653589793/9.0)*(6.605847528077872E17+5.840478851570874E17*sqrt(-1.0))+sin(q1+3.141592653589793/9.0)*(-5.840478851570874E17+6.605847528077872E17*sqrt(-1.0))+exp(q2*sqrt(-1.0))*cos(q1+3.141592653589793/9.0)*(-3.273183803823705E17-8.992998590730221E17*sqrt(-1.0))+exp(q2*sqrt(-1.0))*sin(q1+3.141592653589793/9.0)*(8.992998590730221E17-3.273183803823705E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*sqrt(-1.0))*cos(q1+3.141592653589793/9.0)*(-3.273183803823705E17+8.992998590730221E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*2.0*sqrt(-1.0))*cos(q1+3.141592653589793/9.0)*(6.605847528077872E17-5.840478851570874E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*sqrt(-1.0))*sin(q1+3.141592653589793/9.0)*(8.992998590730221E17+3.273183803823705E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*2.0*sqrt(-1.0))*sin(q1+3.141592653589793/9.0)*(-5.840478851570874E17-6.605847528077872E17*sqrt(-1.0)));
+ Joint_velocity_y[1][0] = (vy*(exp(q2*sqrt(-1.0))*(3.273183803823705E17+8.992998590730221E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*sqrt(-1.0))*(3.273183803823705E17-8.992998590730221E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*2.0*sqrt(-1.0))*(-6.605847528077872E17+5.840478851570874E17*sqrt(-1.0))+-6.605847528077872E17-5.840478851570874E17*sqrt(-1.0))*(4.0E1/1.7E1))/(cos(q1+3.141592653589793/9.0)*(6.605847528077872E17+5.840478851570874E17*sqrt(-1.0))+sin(q1+3.141592653589793/9.0)*(-5.840478851570874E17+6.605847528077872E17*sqrt(-1.0))+exp(q2*sqrt(-1.0))*cos(q1+3.141592653589793/9.0)*(-3.273183803823705E17-8.992998590730221E17*sqrt(-1.0))+exp(q2*sqrt(-1.0))*sin(q1+3.141592653589793/9.0)*(8.992998590730221E17-3.273183803823705E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*sqrt(-1.0))*cos(q1+3.141592653589793/9.0)*(-3.273183803823705E17+8.992998590730221E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*2.0*sqrt(-1.0))*cos(q1+3.141592653589793/9.0)*(6.605847528077872E17-5.840478851570874E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*sqrt(-1.0))*sin(q1+3.141592653589793/9.0)*(8.992998590730221E17+3.273183803823705E17*sqrt(-1.0))+exp(q1*2.0*sqrt(-1.0))*exp(q2*2.0*sqrt(-1.0))*sin(q1+3.141592653589793/9.0)*(-5.840478851570874E17-6.605847528077872E17*sqrt(-1.0)));
+
+ // Integratie
+ Joint_1_position_x = Joint_1_position_x + Joint_velocity_x[0][0]*delta_t;
+ Joint_2_position_x = Joint_2_position_x + Joint_velocity_x[1][0]*delta_t;
- // Calculating end-effector position from joint angles with Brockett
- // Defining H-matrix in reference from end-effector coordinate system to 0
- double H0e_0[3][3];
- H0e_0[0][0] = 1.0;
- H0e_0[0][2] = x_coordinate_in_reference;
- H0e_0[1][1] = 1.0;
- H0e_0[1][2] = y_coordinate_in_reference;
- H0e_0[2][2] = 1.0;
+ Joint1_position_y = Joint_1_position_y + Joint_velocity_y[0][0]*delta_t;
+ Joint2_position_y = Joint_2_position_y + Joint_velocity_y[1][0]*delta_t;
- // Defining variables used for twists in Brockett
- const double x2_brockett = x1 + 0.425*cos(phi);
- const double y2_brockett = y1 + 0.425*sin(phi);
-
- // Calculating Brockett
- double exponent_1[3][3];
- exponent_1[0][0] = cos(q1);
- exponent_1[0][1] = -sin(q1);
- exponent_1[0][2] = x1-x1*cos(q1)+y1*sin(q1);
- exponent_1[1][0] = sin(q1);
- exponent_1[1][1] = cos(q1);
- exponent_1[1][2] = y1-y1*cos(q1)-x1*sin(q1);
- exponent_1[2][2] = 1.0;
+ double Motor_1_velocity_x = Joint_velocity_x[0][0];
+ double Motor_2_velocity_x = Joint_velocity_x[0][0] + Joint_velocity_x[1][0];
- double exponent_2[3][3];
- exponent_2[0][0] = cos(q2);
- exponent_2[0][1] = -sin(q2);
- exponent_2[0][2] = x2_brockett-x2_brockett*cos(q2)+y2_brockett*sin(q2);
- exponent_2[1][0] = sin(q2);
- exponent_2[1][1] = cos(q2);
- exponent_2[1][2] = y2_brockett-y2_brockett*cos(q2)-x2_brockett*sin(q2);
- exponent_2[2][2] = 1.0;
-
- double H0e[3][3];
- H0e = exponent_1[3][3]*exponent_2[3][3];
+ double Motor_1_velocity_y = Joint_velocity_y[0][0];
+ double Motor_2_velocity_y = Joint_velocity_y[0][0] + Joint_velocity_y[1][0];
+
}
int main()
{
-
+ Inverse_Kinematics ();
}