Eva Krolis / Mbed 2 deprecated Kinematics

Good Jacobian and code Not been tested

Dependencies:   MODSERIAL biquadFilter mbed

Fork of Kinematics by Ramon Waninge

main.cpp@13:f77c5f196161, 2018-10-31 (annotated)

Committer:
Ramonwaninge
Date:
Wed Oct 31 17:36:26 2018 +0000
Revision:
13:f77c5f196161
Parent:
12:8d3bc1fa2321
Child:
14:e3fe54f0a4b4
Working on combining with EMG

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Ramonwaninge 13:f77c5f196161 1 //Libraries
Ramonwaninge 0:779fe292e912 2 #include "mbed.h"
Ramonwaninge 2:0a7a3c0c08d3 3 #include <math.h>
Ramonwaninge 2:0a7a3c0c08d3 4 #include <cmath>
Ramonwaninge 12:8d3bc1fa2321 5 #include "MODSERIAL.h"
Ramonwaninge 13:f77c5f196161 6 #include "BiQuad.h"
Ramonwaninge 13:f77c5f196161 7 #include <algorithm>
Ramonwaninge 0:779fe292e912 8 #define PI 3.14159265
Ramonwaninge 3:de8d3ca44a3e 9 MODSERIAL pc(USBTX, USBRX); // connecting to pc
Ramonwaninge 13:f77c5f196161 10
Ramonwaninge 13:f77c5f196161 11
Ramonwaninge 13:f77c5f196161 12
Ramonwaninge 13:f77c5f196161 13
Ramonwaninge 8:697aa3c94209 14
Ramonwaninge 10:2b965defcde5 15 // nog te verwijderen/ aan te passen, zijn dubbel gedefinieerd
Ramonwaninge 13:f77c5f196161 16 double omega1;
Ramonwaninge 13:f77c5f196161 17 double omega4;
Ramonwaninge 13:f77c5f196161 18
Ramonwaninge 0:779fe292e912 19
Ramonwaninge 10:2b965defcde5 20 //Joe dit zijn de inputsignalen (en tussenvariabelen)
Ramonwaninge 12:8d3bc1fa2321 21 //vorige theta
Ramonwaninge 12:8d3bc1fa2321 22 double theta1 = PI*0.49; // huidige/nieuwe theta
Ramonwaninge 12:8d3bc1fa2321 23 double theta4 = PI*0.49;
Ramonwaninge 12:8d3bc1fa2321 24 bool emg1;
Ramonwaninge 12:8d3bc1fa2321 25 bool emg2;
Ramonwaninge 12:8d3bc1fa2321 26 bool emg3;
Ramonwaninge 8:697aa3c94209 27 double thetaflip = 0;
Ramonwaninge 13:f77c5f196161 28
Ramonwaninge 10:2b965defcde5 29 double prefx;
Ramonwaninge 10:2b965defcde5 30 double prefy;
Ramonwaninge 13:f77c5f196161 31 double deltat = 0.01; //tijdstap(moet nog aangepast worden)
Ramonwaninge 12:8d3bc1fa2321 32 //Joe dit zijn de constantes
Ramonwaninge 12:8d3bc1fa2321 33 double ll = 200.0;
Ramonwaninge 12:8d3bc1fa2321 34 double lu = 170.0;
Ramonwaninge 12:8d3bc1fa2321 35 double lb = 10.0;
Ramonwaninge 12:8d3bc1fa2321 36 double le = 79.0;
Ramonwaninge 8:697aa3c94209 37 double xbase = 340;
Ramonwaninge 2:0a7a3c0c08d3 38
Ramonwaninge 10:2b965defcde5 39
Ramonwaninge 2:0a7a3c0c08d3 40 //forward kinematics, Check mathematica! Omdat mbed in paniek raakt met meerdere wortels, hebben we de vergelijking opgedeeld in 3 stukken
Ramonwaninge 10:2b965defcde5 41 //check void forward voor de berekeningen
Ramonwaninge 2:0a7a3c0c08d3 42 //First define the position equation of x
Ramonwaninge 5:d78ed3a3e66a 43 double xendsum;
Ramonwaninge 5:d78ed3a3e66a 44 double xendsqrt1;
Ramonwaninge 5:d78ed3a3e66a 45 double xendsqrt2;
Ramonwaninge 5:d78ed3a3e66a 46 double xend;
Ramonwaninge 12:8d3bc1fa2321 47 double jacobiana;
Ramonwaninge 12:8d3bc1fa2321 48 double jacobianc;
Ramonwaninge 13:f77c5f196161 49 //Now define the position equation of y
Ramonwaninge 6:59744dfe8ea7 50 double yendsum;
Ramonwaninge 6:59744dfe8ea7 51 double yendsqrt1;
Ramonwaninge 6:59744dfe8ea7 52 double yendsqrt2;
Ramonwaninge 6:59744dfe8ea7 53 double yend;
Ramonwaninge 12:8d3bc1fa2321 54 double jacobianb;
Ramonwaninge 12:8d3bc1fa2321 55 double jacobiand;
Ramonwaninge 2:0a7a3c0c08d3 56
Ramonwaninge 2:0a7a3c0c08d3 57
Ramonwaninge 13:f77c5f196161 58 //Timers and Tickers
Ramonwaninge 13:f77c5f196161 59 Ticker kin; //Timer for calculating x,y,theta1,theta4
Ramonwaninge 13:f77c5f196161 60 Ticker simulateval; //Timer that prints the values for x,y, and angles
Ramonwaninge 13:f77c5f196161 61 Ticker rekenen; //Not used right now
Ramonwaninge 13:f77c5f196161 62 Ticker ReadUseEMG0_timer; //Timer to read, filter and use the EMG
Ramonwaninge 13:f77c5f196161 63 Ticker EMGCalibration0_timer; //Timer for the calibration of the EMG
Ramonwaninge 13:f77c5f196161 64 Ticker FindMax0_timer; //Timer for finding the max muscle
Ramonwaninge 13:f77c5f196161 65 Ticker ReadUseEMG1_timer; //Timer to read, filter and use the EMG
Ramonwaninge 13:f77c5f196161 66 Ticker EMGCalibration1_timer; //Timer for the calibration of the EMG
Ramonwaninge 13:f77c5f196161 67 Ticker FindMax1_timer; //Timer for finding the max muscle
Ramonwaninge 13:f77c5f196161 68 Ticker SwitchState_timer; //Timer to switch from the Calibration to the working mode
Ramonwaninge 13:f77c5f196161 69
Ramonwaninge 1:f63be2020475 70
Ramonwaninge 0:779fe292e912 71
Ramonwaninge 12:8d3bc1fa2321 72 //dit wordt aangeroepen in de tickerfunctie
Ramonwaninge 12:8d3bc1fa2321 73 void inverse(double prex, double prey){
Ramonwaninge 12:8d3bc1fa2321 74 /*
Ramonwaninge 12:8d3bc1fa2321 75 qn = qn-1 + (jacobian^-1)*dPref/dt *deltaT
Ramonwaninge 12:8d3bc1fa2321 76 ofwel
Ramonwaninge 12:8d3bc1fa2321 77 thetai+1 = thetai +(jacobian)^-1*vector(deltaX, DeltaY)
Ramonwaninge 12:8d3bc1fa2321 78 waar Pref = emg signaal
Ramonwaninge 12:8d3bc1fa2321 79 */ //achtergrondinfo hierboven...
Ramonwaninge 12:8d3bc1fa2321 80 //
Ramonwaninge 12:8d3bc1fa2321 81
Ramonwaninge 12:8d3bc1fa2321 82 theta1 += (prefx*jacobiana+jacobianb*prey)*deltat; //theta 1 is zichzelf plus wat hier staat (is kinematics)
Ramonwaninge 12:8d3bc1fa2321 83 theta4 += (prefx*jacobianc+jacobiand*prey)*deltat;//" "
Ramonwaninge 12:8d3bc1fa2321 84 //Hier worden xend en yend doorgerekend, die formules kan je overslaan
Ramonwaninge 4:49dfbfcd3577 85 xendsum = lb + xbase +ll*(cos(theta1) - cos(theta4));
Ramonwaninge 4:49dfbfcd3577 86 xendsqrt1 = 2*sqrt(-xbase*xbase/4 + lu*lu + ll*(xbase*(cos(theta1)+cos(theta4))/2) -ll*(1+ cos(theta1+theta4)))*(-sin(theta1)+sin(theta4));
Ramonwaninge 4:49dfbfcd3577 87 xendsqrt2 = sqrt(pow((-xbase/ll+cos(theta1)+cos(theta4)),2)+ pow(sin(theta1) - sin(theta4),2));
Ramonwaninge 2:0a7a3c0c08d3 88 xend = (xendsum + xendsqrt1/xendsqrt2)/2;
Ramonwaninge 12:8d3bc1fa2321 89 //hieronder rekenen we yendeffector door;
Ramonwaninge 6:59744dfe8ea7 90 yendsum = -le + ll/2*(sin(theta1)+sin(theta4));
Ramonwaninge 12:8d3bc1fa2321 91 yendsqrt1 = (-xbase/ll + cos(theta1)+cos(theta4))*sqrt(-xbase*xbase/4 + lu*lu + ll/2*(xbase*(cos(theta1)+cos(theta4))- ll*(1+cos(theta1+theta4))));
Ramonwaninge 6:59744dfe8ea7 92 yendsqrt2 = sqrt(pow((-xbase/ll + cos(theta1)+ cos(theta4)),2)+ pow((sin(theta1)-sin(theta4)),2));
Ramonwaninge 6:59744dfe8ea7 93 yend = (yendsum + yendsqrt1/yendsqrt2);
Ramonwaninge 12:8d3bc1fa2321 94
Ramonwaninge 3:de8d3ca44a3e 95 }
Ramonwaninge 12:8d3bc1fa2321 96 //deze onderstaande tickerfunctie wordt aangeroepen
Ramonwaninge 12:8d3bc1fa2321 97 void kinematics()
Ramonwaninge 12:8d3bc1fa2321 98 {
Ramonwaninge 12:8d3bc1fa2321 99
Ramonwaninge 12:8d3bc1fa2321 100 //Hieronder rekenen we eerst de aparte dingen van de jacobiaan uit. (sla maar over)
Ramonwaninge 12:8d3bc1fa2321 101
Ramonwaninge 12:8d3bc1fa2321 102 jacobiana = (500*(-(ll*(sin(theta1) - sin(0.001 - theta4)))/2. - ((-(xbase/ll) + cos(theta1) + cos(0.001 - theta4))*
Ramonwaninge 12:8d3bc1fa2321 103 sqrt(pow(lu,2) - (pow(ll,2)*(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)))/4.))/
Ramonwaninge 12:8d3bc1fa2321 104 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)) +
Ramonwaninge 12:8d3bc1fa2321 105 ((-xbase + ll*(cos(theta1) + cos(0.001 + theta4)))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(0.001 + theta4)) - ll*(1 + cos(0.001 + theta1 + theta4))))/2.))/
Ramonwaninge 12:8d3bc1fa2321 106 (ll*sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 + theta4),2) + pow(sin(theta1) - sin(0.001 + theta4),2))) + (ll*(sin(theta1) + sin(0.001 + theta4)))/2.))/
Ramonwaninge 12:8d3bc1fa2321 107 (250000*((lb + xbase + ll*(cos(0.001 + theta1) - cos(theta4)) + (2*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(0.001 + theta1) + cos(theta4)) - ll*(1 + cos(0.001 + theta1 + theta4))))/2.)*(-sin(0.001 + theta1) + sin(theta4)))/
Ramonwaninge 12:8d3bc1fa2321 108 sqrt(pow(-(xbase/ll) + cos(0.001 + theta1) + cos(theta4),2) + pow(sin(0.001 + theta1) - sin(theta4),2)))/2. +
Ramonwaninge 12:8d3bc1fa2321 109 (-lb - xbase - ll*(cos(0.001 - theta1) - cos(theta4)) - (2*(sin(0.001 - theta1) + sin(theta4))*
Ramonwaninge 12:8d3bc1fa2321 110 sqrt(pow(lu,2) - (pow(ll,2)*(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2)))/4.))/
Ramonwaninge 12:8d3bc1fa2321 111 sqrt(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2)))/2.)*
Ramonwaninge 12:8d3bc1fa2321 112 (-(ll*(sin(theta1) - sin(0.001 - theta4)))/2. - ((-(xbase/ll) + cos(theta1) + cos(0.001 - theta4))*
Ramonwaninge 12:8d3bc1fa2321 113 sqrt(pow(lu,2) - (pow(ll,2)*(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)))/4.))/
Ramonwaninge 12:8d3bc1fa2321 114 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)) +
Ramonwaninge 12:8d3bc1fa2321 115 ((-xbase + ll*(cos(theta1) + cos(0.001 + theta4)))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(0.001 + theta4)) - ll*(1 + cos(0.001 + theta1 + theta4))))/2.))/
Ramonwaninge 12:8d3bc1fa2321 116 (ll*sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 + theta4),2) + pow(sin(theta1) - sin(0.001 + theta4),2))) + (ll*(sin(theta1) + sin(0.001 + theta4)))/2.) -
Ramonwaninge 12:8d3bc1fa2321 117 250000*(((-xbase + ll*(cos(0.001 + theta1) + cos(theta4)))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(0.001 + theta1) + cos(theta4)) - ll*(1 + cos(0.001 + theta1 + theta4))))/2.))/
Ramonwaninge 12:8d3bc1fa2321 118 (ll*sqrt(pow(-(xbase/ll) + cos(0.001 + theta1) + cos(theta4),2) + pow(sin(0.001 + theta1) - sin(theta4),2))) - (ll*(-sin(0.001 - theta1) + sin(theta4)))/2. + (ll*(sin(0.001 + theta1) + sin(theta4)))/2. -
Ramonwaninge 12:8d3bc1fa2321 119 ((-xbase + ll*(cos(0.001 - theta1) + cos(theta4)))*sqrt(pow(lu,2) - (pow(ll,2)*(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2)))/4.))/
Ramonwaninge 12:8d3bc1fa2321 120 (ll*sqrt(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2))))*
Ramonwaninge 12:8d3bc1fa2321 121 ((-lb - xbase - ll*(cos(theta1) - cos(0.001 - theta4)) + (2*(sin(theta1) + sin(0.001 - theta4))*
Ramonwaninge 12:8d3bc1fa2321 122 sqrt(pow(lu,2) - (pow(ll,2)*(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)))/4.))/
Ramonwaninge 12:8d3bc1fa2321 123 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)))/2. +
Ramonwaninge 12:8d3bc1fa2321 124 (lb + xbase + ll*(cos(theta1) - cos(0.001 + theta4)) + (2*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(0.001 + theta4)) - ll*(1 + cos(0.001 + theta1 + theta4))))/2.)*(-sin(theta1) + sin(0.001 + theta4)))/
Ramonwaninge 12:8d3bc1fa2321 125 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 + theta4),2) + pow(sin(theta1) - sin(0.001 + theta4),2)))/2.));
Ramonwaninge 12:8d3bc1fa2321 126
Ramonwaninge 12:8d3bc1fa2321 127 jacobianb = (-500*((-lb - xbase - ll*(cos(theta1) - cos(0.001 - theta4)) + (2*(sin(theta1) + sin(0.001 - theta4))*
Ramonwaninge 12:8d3bc1fa2321 128 sqrt(pow(lu,2) - (pow(ll,2)*(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)))/4.))/
Ramonwaninge 12:8d3bc1fa2321 129 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)))/2. +
Ramonwaninge 12:8d3bc1fa2321 130 (lb + xbase + ll*(cos(theta1) - cos(0.001 + theta4)) + (2*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(0.001 + theta4)) - ll*(1 + cos(0.001 + theta1 + theta4))))/2.)*(-sin(theta1) + sin(0.001 + theta4)))/
Ramonwaninge 12:8d3bc1fa2321 131 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 + theta4),2) + pow(sin(theta1) - sin(0.001 + theta4),2)))/2.))/
Ramonwaninge 12:8d3bc1fa2321 132 (250000*((lb + xbase + ll*(cos(0.001 + theta1) - cos(theta4)) + (2*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(0.001 + theta1) + cos(theta4)) - ll*(1 + cos(0.001 + theta1 + theta4))))/2.)*
Ramonwaninge 12:8d3bc1fa2321 133 (-sin(0.001 + theta1) + sin(theta4)))/sqrt(pow(-(xbase/ll) + cos(0.001 + theta1) + cos(theta4),2) + pow(sin(0.001 + theta1) - sin(theta4),2)))/2. +
Ramonwaninge 12:8d3bc1fa2321 134 (-lb - xbase - ll*(cos(0.001 - theta1) - cos(theta4)) - (2*(sin(0.001 - theta1) + sin(theta4))*
Ramonwaninge 12:8d3bc1fa2321 135 sqrt(pow(lu,2) - (pow(ll,2)*(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2)))/4.))/
Ramonwaninge 12:8d3bc1fa2321 136 sqrt(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2)))/2.)*
Ramonwaninge 12:8d3bc1fa2321 137 (-(ll*(sin(theta1) - sin(0.001 - theta4)))/2. - ((-(xbase/ll) + cos(theta1) + cos(0.001 - theta4))*
Ramonwaninge 12:8d3bc1fa2321 138 sqrt(pow(lu,2) - (pow(ll,2)*(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)))/4.))/
Ramonwaninge 12:8d3bc1fa2321 139 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)) +
Ramonwaninge 12:8d3bc1fa2321 140 ((-xbase + ll*(cos(theta1) + cos(0.001 + theta4)))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(0.001 + theta4)) - ll*(1 + cos(0.001 + theta1 + theta4))))/2.))/
Ramonwaninge 12:8d3bc1fa2321 141 (ll*sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 + theta4),2) + pow(sin(theta1) - sin(0.001 + theta4),2))) + (ll*(sin(theta1) + sin(0.001 + theta4)))/2.) -
Ramonwaninge 12:8d3bc1fa2321 142 250000*(((-xbase + ll*(cos(0.001 + theta1) + cos(theta4)))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(0.001 + theta1) + cos(theta4)) - ll*(1 + cos(0.001 + theta1 + theta4))))/2.))/
Ramonwaninge 12:8d3bc1fa2321 143 (ll*sqrt(pow(-(xbase/ll) + cos(0.001 + theta1) + cos(theta4),2) + pow(sin(0.001 + theta1) - sin(theta4),2))) - (ll*(-sin(0.001 - theta1) + sin(theta4)))/2. + (ll*(sin(0.001 + theta1) + sin(theta4)))/2. -
Ramonwaninge 12:8d3bc1fa2321 144 ((-xbase + ll*(cos(0.001 - theta1) + cos(theta4)))*sqrt(pow(lu,2) - (pow(ll,2)*(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2)))/4.))/
Ramonwaninge 12:8d3bc1fa2321 145 (ll*sqrt(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2))))*
Ramonwaninge 12:8d3bc1fa2321 146 ((-lb - xbase - ll*(cos(theta1) - cos(0.001 - theta4)) + (2*(sin(theta1) + sin(0.001 - theta4))*
Ramonwaninge 12:8d3bc1fa2321 147 sqrt(pow(lu,2) - (pow(ll,2)*(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)))/4.))/
Ramonwaninge 12:8d3bc1fa2321 148 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)))/2. +
Ramonwaninge 12:8d3bc1fa2321 149 (lb + xbase + ll*(cos(theta1) - cos(0.001 + theta4)) + (2*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(0.001 + theta4)) - ll*(1 + cos(0.001 + theta1 + theta4))))/2.)*(-sin(theta1) + sin(0.001 + theta4)))/
Ramonwaninge 12:8d3bc1fa2321 150 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 + theta4),2) + pow(sin(theta1) - sin(0.001 + theta4),2)))/2.));
Ramonwaninge 12:8d3bc1fa2321 151
Ramonwaninge 12:8d3bc1fa2321 152 jacobianc = (-500*(((-xbase + ll*(cos(0.001 + theta1) + cos(theta4)))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(0.001 + theta1) + cos(theta4)) - ll*(1 + cos(0.001 + theta1 + theta4))))/2.))/
Ramonwaninge 12:8d3bc1fa2321 153 (ll*sqrt(pow(-(xbase/ll) + cos(0.001 + theta1) + cos(theta4),2) + pow(sin(0.001 + theta1) - sin(theta4),2))) - (ll*(-sin(0.001 - theta1) + sin(theta4)))/2. + (ll*(sin(0.001 + theta1) + sin(theta4)))/2. -
Ramonwaninge 12:8d3bc1fa2321 154 ((-xbase + ll*(cos(0.001 - theta1) + cos(theta4)))*sqrt(pow(lu,2) - (pow(ll,2)*(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2)))/4.))/
Ramonwaninge 12:8d3bc1fa2321 155 (ll*sqrt(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2)))))/
Ramonwaninge 12:8d3bc1fa2321 156 (250000*((lb + xbase + ll*(cos(0.001 + theta1) - cos(theta4)) + (2*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(0.001 + theta1) + cos(theta4)) - ll*(1 + cos(0.001 + theta1 + theta4))))/2.)*
Ramonwaninge 12:8d3bc1fa2321 157 (-sin(0.001 + theta1) + sin(theta4)))/sqrt(pow(-(xbase/ll) + cos(0.001 + theta1) + cos(theta4),2) + pow(sin(0.001 + theta1) - sin(theta4),2)))/2. +
Ramonwaninge 12:8d3bc1fa2321 158 (-lb - xbase - ll*(cos(0.001 - theta1) - cos(theta4)) - (2*(sin(0.001 - theta1) + sin(theta4))*
Ramonwaninge 12:8d3bc1fa2321 159 sqrt(pow(lu,2) - (pow(ll,2)*(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2)))/4.))/
Ramonwaninge 12:8d3bc1fa2321 160 sqrt(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2)))/2.)*
Ramonwaninge 12:8d3bc1fa2321 161 (-(ll*(sin(theta1) - sin(0.001 - theta4)))/2. - ((-(xbase/ll) + cos(theta1) + cos(0.001 - theta4))*
Ramonwaninge 12:8d3bc1fa2321 162 sqrt(pow(lu,2) - (pow(ll,2)*(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)))/4.))/
Ramonwaninge 12:8d3bc1fa2321 163 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)) +
Ramonwaninge 12:8d3bc1fa2321 164 ((-xbase + ll*(cos(theta1) + cos(0.001 + theta4)))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(0.001 + theta4)) - ll*(1 + cos(0.001 + theta1 + theta4))))/2.))/
Ramonwaninge 12:8d3bc1fa2321 165 (ll*sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 + theta4),2) + pow(sin(theta1) - sin(0.001 + theta4),2))) + (ll*(sin(theta1) + sin(0.001 + theta4)))/2.) -
Ramonwaninge 12:8d3bc1fa2321 166 250000*(((-xbase + ll*(cos(0.001 + theta1) + cos(theta4)))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(0.001 + theta1) + cos(theta4)) - ll*(1 + cos(0.001 + theta1 + theta4))))/2.))/
Ramonwaninge 12:8d3bc1fa2321 167 (ll*sqrt(pow(-(xbase/ll) + cos(0.001 + theta1) + cos(theta4),2) + pow(sin(0.001 + theta1) - sin(theta4),2))) - (ll*(-sin(0.001 - theta1) + sin(theta4)))/2. + (ll*(sin(0.001 + theta1) + sin(theta4)))/2. -
Ramonwaninge 12:8d3bc1fa2321 168 ((-xbase + ll*(cos(0.001 - theta1) + cos(theta4)))*sqrt(pow(lu,2) - (pow(ll,2)*(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2)))/4.))/
Ramonwaninge 12:8d3bc1fa2321 169 (ll*sqrt(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2))))*
Ramonwaninge 12:8d3bc1fa2321 170 ((-lb - xbase - ll*(cos(theta1) - cos(0.001 - theta4)) + (2*(sin(theta1) + sin(0.001 - theta4))*
Ramonwaninge 12:8d3bc1fa2321 171 sqrt(pow(lu,2) - (pow(ll,2)*(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)))/4.))/
Ramonwaninge 12:8d3bc1fa2321 172 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)))/2. +
Ramonwaninge 12:8d3bc1fa2321 173 (lb + xbase + ll*(cos(theta1) - cos(0.001 + theta4)) + (2*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(0.001 + theta4)) - ll*(1 + cos(0.001 + theta1 + theta4))))/2.)*(-sin(theta1) + sin(0.001 + theta4)))/
Ramonwaninge 12:8d3bc1fa2321 174 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 + theta4),2) + pow(sin(theta1) - sin(0.001 + theta4),2)))/2.));
Ramonwaninge 12:8d3bc1fa2321 175
Ramonwaninge 12:8d3bc1fa2321 176 jacobiand = (500*((lb + xbase + ll*(cos(0.001 + theta1) - cos(theta4)) + (2*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(0.001 + theta1) + cos(theta4)) - ll*(1 + cos(0.001 + theta1 + theta4))))/2.)*(-sin(0.001 + theta1) + sin(theta4)))/
Ramonwaninge 12:8d3bc1fa2321 177 sqrt(pow(-(xbase/ll) + cos(0.001 + theta1) + cos(theta4),2) + pow(sin(0.001 + theta1) - sin(theta4),2)))/2. +
Ramonwaninge 12:8d3bc1fa2321 178 (-lb - xbase - ll*(cos(0.001 - theta1) - cos(theta4)) - (2*(sin(0.001 - theta1) + sin(theta4))*
Ramonwaninge 12:8d3bc1fa2321 179 sqrt(pow(lu,2) - (pow(ll,2)*(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2)))/4.))/
Ramonwaninge 12:8d3bc1fa2321 180 sqrt(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2)))/2.))/
Ramonwaninge 12:8d3bc1fa2321 181 (250000*((lb + xbase + ll*(cos(0.001 + theta1) - cos(theta4)) + (2*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(0.001 + theta1) + cos(theta4)) - ll*(1 + cos(0.001 + theta1 + theta4))))/2.)*
Ramonwaninge 12:8d3bc1fa2321 182 (-sin(0.001 + theta1) + sin(theta4)))/sqrt(pow(-(xbase/ll) + cos(0.001 + theta1) + cos(theta4),2) + pow(sin(0.001 + theta1) - sin(theta4),2)))/2. +
Ramonwaninge 12:8d3bc1fa2321 183 (-lb - xbase - ll*(cos(0.001 - theta1) - cos(theta4)) - (2*(sin(0.001 - theta1) + sin(theta4))*
Ramonwaninge 12:8d3bc1fa2321 184 sqrt(pow(lu,2) - (pow(ll,2)*(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2)))/4.))/
Ramonwaninge 12:8d3bc1fa2321 185 sqrt(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2)))/2.)*
Ramonwaninge 12:8d3bc1fa2321 186 (-(ll*(sin(theta1) - sin(0.001 - theta4)))/2. - ((-(xbase/ll) + cos(theta1) + cos(0.001 - theta4))*
Ramonwaninge 12:8d3bc1fa2321 187 sqrt(pow(lu,2) - (pow(ll,2)*(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)))/4.))/
Ramonwaninge 12:8d3bc1fa2321 188 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)) +
Ramonwaninge 12:8d3bc1fa2321 189 ((-xbase + ll*(cos(theta1) + cos(0.001 + theta4)))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(0.001 + theta4)) - ll*(1 + cos(0.001 + theta1 + theta4))))/2.))/
Ramonwaninge 12:8d3bc1fa2321 190 (ll*sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 + theta4),2) + pow(sin(theta1) - sin(0.001 + theta4),2))) + (ll*(sin(theta1) + sin(0.001 + theta4)))/2.) -
Ramonwaninge 12:8d3bc1fa2321 191 250000*(((-xbase + ll*(cos(0.001 + theta1) + cos(theta4)))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(0.001 + theta1) + cos(theta4)) - ll*(1 + cos(0.001 + theta1 + theta4))))/2.))/
Ramonwaninge 12:8d3bc1fa2321 192 (ll*sqrt(pow(-(xbase/ll) + cos(0.001 + theta1) + cos(theta4),2) + pow(sin(0.001 + theta1) - sin(theta4),2))) - (ll*(-sin(0.001 - theta1) + sin(theta4)))/2. + (ll*(sin(0.001 + theta1) + sin(theta4)))/2. -
Ramonwaninge 12:8d3bc1fa2321 193 ((-xbase + ll*(cos(0.001 - theta1) + cos(theta4)))*sqrt(pow(lu,2) - (pow(ll,2)*(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2)))/4.))/
Ramonwaninge 12:8d3bc1fa2321 194 (ll*sqrt(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2))))*
Ramonwaninge 12:8d3bc1fa2321 195 ((-lb - xbase - ll*(cos(theta1) - cos(0.001 - theta4)) + (2*(sin(theta1) + sin(0.001 - theta4))*
Ramonwaninge 12:8d3bc1fa2321 196 sqrt(pow(lu,2) - (pow(ll,2)*(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)))/4.))/
Ramonwaninge 12:8d3bc1fa2321 197 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)))/2. +
Ramonwaninge 12:8d3bc1fa2321 198 (lb + xbase + ll*(cos(theta1) - cos(0.001 + theta4)) + (2*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(0.001 + theta4)) - ll*(1 + cos(0.001 + theta1 + theta4))))/2.)*(-sin(theta1) + sin(0.001 + theta4)))/
Ramonwaninge 12:8d3bc1fa2321 199 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 + theta4),2) + pow(sin(theta1) - sin(0.001 + theta4),2)))/2.));
Ramonwaninge 12:8d3bc1fa2321 200
Ramonwaninge 12:8d3bc1fa2321 201 //vanaf hier weer door met lezen!
Ramonwaninge 12:8d3bc1fa2321 202 prefx = 1*(!button1); //sw3, dit is belangrijk! prefx staat voor P_(reference) en het is de snelheid van de endeffector als
Ramonwaninge 12:8d3bc1fa2321 203 // de button ingedrukt wordt (als emg = boven treshold) is de prefx 1 (da's de rode 1)
Ramonwaninge 12:8d3bc1fa2321 204 prefy = 1*(!button2); //sw2,
Ramonwaninge 12:8d3bc1fa2321 205 inverse(prefx, prefy);
Ramonwaninge 12:8d3bc1fa2321 206 }
Ramonwaninge 12:8d3bc1fa2321 207
Ramonwaninge 12:8d3bc1fa2321 208
Ramonwaninge 12:8d3bc1fa2321 209
Ramonwaninge 12:8d3bc1fa2321 210
Ramonwaninge 12:8d3bc1fa2321 211
Ramonwaninge 12:8d3bc1fa2321 212
Ramonwaninge 12:8d3bc1fa2321 213 /*In de nieuwe versie hieronder is forward overbodig geworden, sla maar over
Ramonwaninge 12:8d3bc1fa2321 214
Ramonwaninge 12:8d3bc1fa2321 215 Joe, hieronder staan de functies die door de tickers aangeroepen worden
Ramonwaninge 12:8d3bc1fa2321 216 void forward(){ dit is de ticker die zegt, als button=0, theta 1 wordt groter. dan worden x en y doorgerekend
Ramonwaninge 12:8d3bc1fa2321 217 hieronder moet veranderd worden naar if button1 == 0, x = x+eenbeetje
Ramonwaninge 12:8d3bc1fa2321 218 maar daar moet eerst inverse kinematics voor gebeuren.
Ramonwaninge 12:8d3bc1fa2321 219 if (button1 == 0){ als emg1==voorbij treshold, komt waarschijnlijk in de inverse functie
Ramonwaninge 12:8d3bc1fa2321 220 theta1 = PI*(theta1/PI + 0.1);
Ramonwaninge 12:8d3bc1fa2321 221 hij is geblokt omdat ik de knop nodig heb
Ramonwaninge 12:8d3bc1fa2321 222 default = als x = xbase/2... break, okee dit moet hier niet, maar weet niet waar wel...
Ramonwaninge 12:8d3bc1fa2321 223 }
Ramonwaninge 12:8d3bc1fa2321 224 else {theta1 = theta1;}
Ramonwaninge 12:8d3bc1fa2321 225 hieronder komen de doorrekeningen van de hoeken naar de coordinaten, check de mathematicafile voor de afleiding*/
Ramonwaninge 12:8d3bc1fa2321 226
Ramonwaninge 12:8d3bc1fa2321 227 /*void demomode(){} //Komt nog...
Ramonwaninge 12:8d3bc1fa2321 228
Ramonwaninge 12:8d3bc1fa2321 229 als emg2 == voorbij treshold,
Ramonwaninge 12:8d3bc1fa2321 230 double theta1 -> plus counts (emg*richting)
Ramonwaninge 12:8d3bc1fa2321 231 double theta4 -> plus counts (emg*richting)
Ramonwaninge 12:8d3bc1fa2321 232 reken y door
Ramonwaninge 12:8d3bc1fa2321 233
Ramonwaninge 12:8d3bc1fa2321 234 void flip(){
Ramonwaninge 10:2b965defcde5 235 if(button2==0){thetaflip = PI*(thetaflip/PI+0.5);} // button2==0 moet veranderd naar emg3>= treshold
Ramonwaninge 12:8d3bc1fa2321 236 }suppressed omdat ik button 2 nodig heb...*/
Ramonwaninge 12:8d3bc1fa2321 237
Ramonwaninge 7:b59b762c537e 238
Ramonwaninge 10:2b965defcde5 239 // de beweging voor de xcoordinaat!
Ramonwaninge 12:8d3bc1fa2321 240
Ramonwaninge 10:2b965defcde5 241
Ramonwaninge 12:8d3bc1fa2321 242 //tot aan hier overslaan
Ramonwaninge 13:f77c5f196161 243 void printvalue(){
Ramonwaninge 13:f77c5f196161 244 pc.printf("\n\r %f %f \n\r %f %f", theta4,theta1, xend, yend); // in teraterm zijn de bovenste twee waardes hoeken, de onderste twee zijn de x en y coordinaat
Ramonwaninge 10:2b965defcde5 245
Ramonwaninge 13:f77c5f196161 246 }
Ramonwaninge 0:779fe292e912 247
Ramonwaninge 10:2b965defcde5 248
Ramonwaninge 0:779fe292e912 249 int main()
Ramonwaninge 0:779fe292e912 250 {
Ramonwaninge 12:8d3bc1fa2321 251 //Initial conditions
Ramonwaninge 12:8d3bc1fa2321 252
Ramonwaninge 12:8d3bc1fa2321 253 theta1 = PI*0.49;
Ramonwaninge 12:8d3bc1fa2321 254 theta4 = PI*0.49;
Ramonwaninge 2:0a7a3c0c08d3 255 pc.baud(115200);
Ramonwaninge 12:8d3bc1fa2321 256 //default = theta1 = theta4 = pi/2,
Ramonwaninge 13:f77c5f196161 257 kin.attach(kinematics, 0.01); // roep de ticker aan (
Ramonwaninge 13:f77c5f196161 258 simulateval.attach(printvalue, 1);
Ramonwaninge 13:f77c5f196161 259 greenled = 1; //First turn the LEDs off
Ramonwaninge 13:f77c5f196161 260 blueled = 1;
Ramonwaninge 13:f77c5f196161 261 redled = 1;
Ramonwaninge 13:f77c5f196161 262 filter0.add(&Notch50_0).add(&High0); //Make filter chain for the first EMG
Ramonwaninge 13:f77c5f196161 263 filter1.add(&Notch50_1).add(&High1); //Make filter chain for the second EMG
Ramonwaninge 13:f77c5f196161 264 button.rise(StopProgram); //If the button is pressed, stop program
Ramonwaninge 12:8d3bc1fa2321 265
Ramonwaninge 12:8d3bc1fa2321 266 pc.printf("%f", theta1);
Ramonwaninge 12:8d3bc1fa2321 267 while(true) {
Ramonwaninge 12:8d3bc1fa2321 268
Ramonwaninge 13:f77c5f196161 269 StateMachine(); //Start the state machine
Ramonwaninge 12:8d3bc1fa2321 270
Ramonwaninge 12:8d3bc1fa2321 271
Ramonwaninge 8:697aa3c94209 272 }
Ramonwaninge 0:779fe292e912 273 }