Anke Post / Mbed 2 deprecated Script_voor_project

Inverse_kinematica

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@5:ac4b1c0366b3, 2019-10-29 (annotated)

Committer:
AnkePost
Date:
Tue Oct 29 16:40:38 2019 +0000
Revision:
5:ac4b1c0366b3
Parent:
4:739136c479bd
Child:
6:c550e499896f
Inverse kinematica

Who changed what in which revision?

UserRevisionLine numberNew contents of line
RobertoO 0:67c50348f842 1 #include "mbed.h"
AnkePost 3:710f993d5848 2 #define M_PI 3.14159265358979323846 /* pi */
WiesjeRoskamp 2:aee655d11b6d 3 #include <math.h>
RobertoO 0:67c50348f842 4
AnkePost 4:739136c479bd 5 double vx; //Gelijk aan variabele die snelheid aangeeft in EMG (in x-richting)
AnkePost 4:739136c479bd 6 double vy; //Gelijk aan variabele die snelheid aangeeft in EMG (in y-richting)
AnkePost 4:739136c479bd 7
AnkePost 5:ac4b1c0366b3 8 double Joint_1_position_x;
AnkePost 5:ac4b1c0366b3 9 double Joint_2_position_x;
AnkePost 5:ac4b1c0366b3 10
AnkePost 5:ac4b1c0366b3 11 double Joint_1_position_y;
AnkePost 5:ac4b1c0366b3 12 double Joint_2_position_y;
AnkePost 5:ac4b1c0366b3 13
AnkePost 4:739136c479bd 14 double Joint_velocity_x[2][1];
AnkePost 4:739136c479bd 15 double Joint_velocity_y[2][1];
AnkePost 4:739136c479bd 16
AnkePost 5:ac4b1c0366b3 17
AnkePost 4:739136c479bd 18 // Defining motor angles
AnkePost 4:739136c479bd 19 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
AnkePost 4:739136c479bd 20 double M2 = 30*M_PI/180; //Zelfde als hierboven, current encoder position
AnkePost 4:739136c479bd 21
AnkePost 4:739136c479bd 22 // Calculating joint angles based on motor angles (current encoder values)
AnkePost 4:739136c479bd 23 double q1 = M1; //Relative angle joint 1 (rad)
AnkePost 4:739136c479bd 24 double q2 = M2 - M1; //Relative angle joint 2 (rad)
WiesjeRoskamp 2:aee655d11b6d 25
AnkePost 3:710f993d5848 26 void Inverse_Kinematics()
WiesjeRoskamp 2:aee655d11b6d 27 {
AnkePost 4:739136c479bd 28 // Defining joint velocities based om calculations of matlab
WiesjeRoskamp 2:aee655d11b6d 29
AnkePost 4:739136c479bd 30 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)));
AnkePost 4:739136c479bd 31 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)));
RobertoO 0:67c50348f842 32
AnkePost 4:739136c479bd 33 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)));
AnkePost 4:739136c479bd 34 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)));
AnkePost 4:739136c479bd 35
AnkePost 4:739136c479bd 36 // Integratie
AnkePost 4:739136c479bd 37 Joint_1_position_x = Joint_1_position_x + Joint_velocity_x[0][0]*delta_t;
AnkePost 4:739136c479bd 38 Joint_2_position_x = Joint_2_position_x + Joint_velocity_x[1][0]*delta_t;
AnkePost 3:710f993d5848 39
AnkePost 4:739136c479bd 40 Joint1_position_y = Joint_1_position_y + Joint_velocity_y[0][0]*delta_t;
AnkePost 4:739136c479bd 41 Joint2_position_y = Joint_2_position_y + Joint_velocity_y[1][0]*delta_t;
AnkePost 3:710f993d5848 42
AnkePost 4:739136c479bd 43 double Motor_1_velocity_x = Joint_velocity_x[0][0];
AnkePost 4:739136c479bd 44 double Motor_2_velocity_x = Joint_velocity_x[0][0] + Joint_velocity_x[1][0];
AnkePost 3:710f993d5848 45
AnkePost 4:739136c479bd 46 double Motor_1_velocity_y = Joint_velocity_y[0][0];
AnkePost 4:739136c479bd 47 double Motor_2_velocity_y = Joint_velocity_y[0][0] + Joint_velocity_y[1][0];
AnkePost 4:739136c479bd 48
AnkePost 3:710f993d5848 49 }
RobertoO 0:67c50348f842 50
RobertoO 0:67c50348f842 51 int main()
RobertoO 0:67c50348f842 52 {
AnkePost 4:739136c479bd 53 Inverse_Kinematics ();
RobertoO 0:67c50348f842 54 }
WiesjeRoskamp 2:aee655d11b6d 55
WiesjeRoskamp 2:aee655d11b6d 56
WiesjeRoskamp 2:aee655d11b6d 57
WiesjeRoskamp 2:aee655d11b6d 58