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@14:e3fe54f0a4b4, 2018-10-31 (annotated)

Committer:
Ramonwaninge
Date:
Wed Oct 31 18:15:10 2018 +0000
Revision:
14:e3fe54f0a4b4
Parent:
13:f77c5f196161
Child:
15:38258e6b6e91
Halverwege samenvoegen...

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 14:e3fe54f0a4b4 9
Ramonwaninge 14:e3fe54f0a4b4 10 //Inputs and outputs
Ramonwaninge 3:de8d3ca44a3e 11 MODSERIAL pc(USBTX, USBRX); // connecting to pc
Ramonwaninge 14:e3fe54f0a4b4 12 MODSERIAL pc(USBTX, USBRX); //Use computer
Ramonwaninge 14:e3fe54f0a4b4 13 AnalogIn EMG0In(A0); //EMG input 0
Ramonwaninge 14:e3fe54f0a4b4 14 AnalogIn EMG1In(A1); //EMG input 1
Ramonwaninge 14:e3fe54f0a4b4 15 InterruptIn button(SW3); //Define button
Ramonwaninge 14:e3fe54f0a4b4 16 DigitalOut greenled(LED_GREEN); //Green led
Ramonwaninge 14:e3fe54f0a4b4 17 DigitalOut blueled(LED_BLUE); //Blue led
Ramonwaninge 14:e3fe54f0a4b4 18 DigitalOut redled(LED_RED); //Red led
Ramonwaninge 13:f77c5f196161 19
Ramonwaninge 13:f77c5f196161 20
Ramonwaninge 13:f77c5f196161 21
Ramonwaninge 8:697aa3c94209 22
Ramonwaninge 10:2b965defcde5 23 // nog te verwijderen/ aan te passen, zijn dubbel gedefinieerd
Ramonwaninge 13:f77c5f196161 24 double omega1;
Ramonwaninge 13:f77c5f196161 25 double omega4;
Ramonwaninge 13:f77c5f196161 26
Ramonwaninge 0:779fe292e912 27
Ramonwaninge 14:e3fe54f0a4b4 28 //parameters for kinematics
Ramonwaninge 12:8d3bc1fa2321 29 //vorige theta
Ramonwaninge 12:8d3bc1fa2321 30 double theta1 = PI*0.49; // huidige/nieuwe theta
Ramonwaninge 12:8d3bc1fa2321 31 double theta4 = PI*0.49;
Ramonwaninge 14:e3fe54f0a4b4 32 double thetaflip = 0; //angle of the flipping motor
Ramonwaninge 14:e3fe54f0a4b4 33 double prefx; //Preference for x, will later be defined due to the motor
Ramonwaninge 14:e3fe54f0a4b4 34 double prefy; //" "
Ramonwaninge 14:e3fe54f0a4b4 35 double deltat = 0.01; //tijdstap(moet nog aangepast worden)
Ramonwaninge 14:e3fe54f0a4b4 36
Ramonwaninge 14:e3fe54f0a4b4 37 //Parameters for the first EMG signal
Ramonwaninge 14:e3fe54f0a4b4 38 float EMG0; //float for EMG input
Ramonwaninge 14:e3fe54f0a4b4 39 float EMG0filt; //float for filtered EMG
Ramonwaninge 14:e3fe54f0a4b4 40 float EMG0filtArray[Parts]; //Array for the filtered array
Ramonwaninge 14:e3fe54f0a4b4 41 float EMG0Average; //float for the value after Moving Average Filter
Ramonwaninge 14:e3fe54f0a4b4 42 float Sum0 = 0; //Sum0 for the moving average filter
Ramonwaninge 14:e3fe54f0a4b4 43 float EMG0Calibrate[Length]; //Array for the calibration
Ramonwaninge 14:e3fe54f0a4b4 44 int ReadCal0 = 0; //Integer to read over the calibration array
Ramonwaninge 14:e3fe54f0a4b4 45 float MaxValue0 = 0; //float to save the max muscle
Ramonwaninge 14:e3fe54f0a4b4 46 float Threshold0 = 0; //Threshold for the first EMG signal
Ramonwaninge 13:f77c5f196161 47
Ramonwaninge 14:e3fe54f0a4b4 48 //Parameters for the second EMG signal
Ramonwaninge 14:e3fe54f0a4b4 49 float EMG1; //float for EMG input
Ramonwaninge 14:e3fe54f0a4b4 50 float EMG1filt; //float for filtered EMG
Ramonwaninge 14:e3fe54f0a4b4 51 float EMG1filtArray[Parts]; //Array for the filtered array
Ramonwaninge 14:e3fe54f0a4b4 52 float EMG1Average; //float for the value after Moving Average Filter
Ramonwaninge 14:e3fe54f0a4b4 53 float Sum1 = 0; //Sum0 for the moving average filter
Ramonwaninge 14:e3fe54f0a4b4 54 float EMG1Calibrate[Length]; //Array for the calibration
Ramonwaninge 14:e3fe54f0a4b4 55 int ReadCal1 = 0; //Integer to read over the calibration array
Ramonwaninge 14:e3fe54f0a4b4 56 float MaxValue1 = 0; //float to save the max muscle
Ramonwaninge 14:e3fe54f0a4b4 57 float Threshold1 = 0; //Threshold for the second EMG signal
Ramonwaninge 14:e3fe54f0a4b4 58
Ramonwaninge 14:e3fe54f0a4b4 59 //Constants
Ramonwaninge 14:e3fe54f0a4b4 60 const double ll = 200.0;
Ramonwaninge 14:e3fe54f0a4b4 61 const double lu = 170.0;
Ramonwaninge 14:e3fe54f0a4b4 62 const double lb = 10.0;
Ramonwaninge 14:e3fe54f0a4b4 63 const double le = 79.0;
Ramonwaninge 14:e3fe54f0a4b4 64 const double xbase = 340;
Ramonwaninge 14:e3fe54f0a4b4 65 const int Length = 10000; //Length of the array for the calibration
Ramonwaninge 14:e3fe54f0a4b4 66 const int Parts = 50; //Mean average filter over 50 values
Ramonwaninge 14:e3fe54f0a4b4 67
Ramonwaninge 14:e3fe54f0a4b4 68 //Filter variables
Ramonwaninge 14:e3fe54f0a4b4 69 BiQuad Notch50_0(0.7887,0,0.7887,0,0.5774); //Make Notch filter around 50 Hz
Ramonwaninge 14:e3fe54f0a4b4 70 BiQuad Notch50_1(0.7887,0,0.7887,0,0.5774); //Make Notch filter around 50 Hz
Ramonwaninge 14:e3fe54f0a4b4 71 BiQuad High0(0.8006,-1.6012,0.8006,-1.561,0.6414); //Make high-pass filter
Ramonwaninge 14:e3fe54f0a4b4 72 BiQuad High1(0.8006,-1.6012,0.8006,-1.561,0.6414); //Make high-pass filter
Ramonwaninge 14:e3fe54f0a4b4 73 BiQuadChain filter0; //Make chain of filters for the first EMG signal
Ramonwaninge 14:e3fe54f0a4b4 74 BiQuadChain filter1; //Make chain of filters for the second EMG signal
Ramonwaninge 14:e3fe54f0a4b4 75
Ramonwaninge 2:0a7a3c0c08d3 76
Ramonwaninge 10:2b965defcde5 77
Ramonwaninge 2:0a7a3c0c08d3 78 //forward kinematics, Check mathematica! Omdat mbed in paniek raakt met meerdere wortels, hebben we de vergelijking opgedeeld in 3 stukken
Ramonwaninge 10:2b965defcde5 79 //check void forward voor de berekeningen
Ramonwaninge 2:0a7a3c0c08d3 80 //First define the position equation of x
Ramonwaninge 5:d78ed3a3e66a 81 double xendsum;
Ramonwaninge 5:d78ed3a3e66a 82 double xendsqrt1;
Ramonwaninge 5:d78ed3a3e66a 83 double xendsqrt2;
Ramonwaninge 5:d78ed3a3e66a 84 double xend;
Ramonwaninge 12:8d3bc1fa2321 85 double jacobiana;
Ramonwaninge 12:8d3bc1fa2321 86 double jacobianc;
Ramonwaninge 13:f77c5f196161 87 //Now define the position equation of y
Ramonwaninge 6:59744dfe8ea7 88 double yendsum;
Ramonwaninge 6:59744dfe8ea7 89 double yendsqrt1;
Ramonwaninge 6:59744dfe8ea7 90 double yendsqrt2;
Ramonwaninge 6:59744dfe8ea7 91 double yend;
Ramonwaninge 12:8d3bc1fa2321 92 double jacobianb;
Ramonwaninge 12:8d3bc1fa2321 93 double jacobiand;
Ramonwaninge 2:0a7a3c0c08d3 94
Ramonwaninge 2:0a7a3c0c08d3 95
Ramonwaninge 13:f77c5f196161 96 //Timers and Tickers
Ramonwaninge 13:f77c5f196161 97 Ticker kin; //Timer for calculating x,y,theta1,theta4
Ramonwaninge 13:f77c5f196161 98 Ticker simulateval; //Timer that prints the values for x,y, and angles
Ramonwaninge 13:f77c5f196161 99 Ticker rekenen; //Not used right now
Ramonwaninge 13:f77c5f196161 100 Ticker ReadUseEMG0_timer; //Timer to read, filter and use the EMG
Ramonwaninge 13:f77c5f196161 101 Ticker EMGCalibration0_timer; //Timer for the calibration of the EMG
Ramonwaninge 13:f77c5f196161 102 Ticker FindMax0_timer; //Timer for finding the max muscle
Ramonwaninge 13:f77c5f196161 103 Ticker ReadUseEMG1_timer; //Timer to read, filter and use the EMG
Ramonwaninge 13:f77c5f196161 104 Ticker EMGCalibration1_timer; //Timer for the calibration of the EMG
Ramonwaninge 13:f77c5f196161 105 Ticker FindMax1_timer; //Timer for finding the max muscle
Ramonwaninge 13:f77c5f196161 106 Ticker SwitchState_timer; //Timer to switch from the Calibration to the working mode
Ramonwaninge 13:f77c5f196161 107
Ramonwaninge 1:f63be2020475 108
Ramonwaninge 14:e3fe54f0a4b4 109 //Bool for movement
Ramonwaninge 14:e3fe54f0a4b4 110 bool xMove = false; //Bool for the x-movement
Ramonwaninge 14:e3fe54f0a4b4 111 bool yMove = false; //Bool for the y-movement
Ramonwaninge 14:e3fe54f0a4b4 112
Ramonwaninge 14:e3fe54f0a4b4 113 //Parameters for the state machine
Ramonwaninge 14:e3fe54f0a4b4 114 enum States {Calibration, WorkingMode}; //Initialize state machine
Ramonwaninge 14:e3fe54f0a4b4 115 States CurrentState = Calibration; //Start in the calibration mode
Ramonwaninge 14:e3fe54f0a4b4 116 bool StateBool = true; //Bool to first go in a state
Ramonwaninge 14:e3fe54f0a4b4 117 bool SwitchStateBool = false; //Bool to switch from calibration to working mode
Ramonwaninge 14:e3fe54f0a4b4 118
Ramonwaninge 14:e3fe54f0a4b4 119 //Function to read and filter the EMG
Ramonwaninge 14:e3fe54f0a4b4 120 void ReadUseEMG0(){
Ramonwaninge 14:e3fe54f0a4b4 121 for(int i = Parts ; i > 0 ; i--){ //Make a first in, first out array
Ramonwaninge 14:e3fe54f0a4b4 122 EMG0filtArray[i] = EMG0filtArray[i-1]; //Every value moves one up
Ramonwaninge 14:e3fe54f0a4b4 123 }
Ramonwaninge 14:e3fe54f0a4b4 124
Ramonwaninge 14:e3fe54f0a4b4 125 Sum0 = 0;
Ramonwaninge 14:e3fe54f0a4b4 126 EMG0 = EMG0In; //Save EMG input in float
Ramonwaninge 14:e3fe54f0a4b4 127 EMG0filt = filter0.step(EMG0); //Filter the signal
Ramonwaninge 14:e3fe54f0a4b4 128 EMG0filt = abs(EMG0filt); //Take the absolute value
Ramonwaninge 14:e3fe54f0a4b4 129 EMG0filtArray[0] = EMG0filt; //Save the filtered signal on the first place in the array
Ramonwaninge 14:e3fe54f0a4b4 130
Ramonwaninge 14:e3fe54f0a4b4 131 for(int i = 0 ; i < Parts ; i++){ //Moving Average filter
Ramonwaninge 14:e3fe54f0a4b4 132 Sum0 += EMG0filtArray[i]; //Sum the new value and the previous 49
Ramonwaninge 14:e3fe54f0a4b4 133 }
Ramonwaninge 14:e3fe54f0a4b4 134 EMG0Average = (float)Sum0/Parts; //Divide the sum by 50
Ramonwaninge 14:e3fe54f0a4b4 135
Ramonwaninge 14:e3fe54f0a4b4 136 if (EMG0Average > Threshold0){ //If the value is higher than the threshold value
Ramonwaninge 14:e3fe54f0a4b4 137 redled = 0; //Turn the LED on
Ramonwaninge 14:e3fe54f0a4b4 138 xMove = true; //Set movement to true
Ramonwaninge 14:e3fe54f0a4b4 139 }
Ramonwaninge 14:e3fe54f0a4b4 140 else{
Ramonwaninge 14:e3fe54f0a4b4 141 redled = 1; //Otherwise turn the LED off
Ramonwaninge 14:e3fe54f0a4b4 142 yMove = false; //Otherwise set movement to false
Ramonwaninge 14:e3fe54f0a4b4 143 }
Ramonwaninge 14:e3fe54f0a4b4 144 }
Ramonwaninge 14:e3fe54f0a4b4 145
Ramonwaninge 14:e3fe54f0a4b4 146
Ramonwaninge 14:e3fe54f0a4b4 147
Ramonwaninge 0:779fe292e912 148
Ramonwaninge 12:8d3bc1fa2321 149 //dit wordt aangeroepen in de tickerfunctie
Ramonwaninge 12:8d3bc1fa2321 150 void inverse(double prex, double prey){
Ramonwaninge 12:8d3bc1fa2321 151 /*
Ramonwaninge 12:8d3bc1fa2321 152 qn = qn-1 + (jacobian^-1)*dPref/dt *deltaT
Ramonwaninge 12:8d3bc1fa2321 153 ofwel
Ramonwaninge 12:8d3bc1fa2321 154 thetai+1 = thetai +(jacobian)^-1*vector(deltaX, DeltaY)
Ramonwaninge 12:8d3bc1fa2321 155 waar Pref = emg signaal
Ramonwaninge 12:8d3bc1fa2321 156 */ //achtergrondinfo hierboven...
Ramonwaninge 12:8d3bc1fa2321 157 //
Ramonwaninge 12:8d3bc1fa2321 158
Ramonwaninge 12:8d3bc1fa2321 159 theta1 += (prefx*jacobiana+jacobianb*prey)*deltat; //theta 1 is zichzelf plus wat hier staat (is kinematics)
Ramonwaninge 12:8d3bc1fa2321 160 theta4 += (prefx*jacobianc+jacobiand*prey)*deltat;//" "
Ramonwaninge 12:8d3bc1fa2321 161 //Hier worden xend en yend doorgerekend, die formules kan je overslaan
Ramonwaninge 4:49dfbfcd3577 162 xendsum = lb + xbase +ll*(cos(theta1) - cos(theta4));
Ramonwaninge 4:49dfbfcd3577 163 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 164 xendsqrt2 = sqrt(pow((-xbase/ll+cos(theta1)+cos(theta4)),2)+ pow(sin(theta1) - sin(theta4),2));
Ramonwaninge 2:0a7a3c0c08d3 165 xend = (xendsum + xendsqrt1/xendsqrt2)/2;
Ramonwaninge 12:8d3bc1fa2321 166 //hieronder rekenen we yendeffector door;
Ramonwaninge 6:59744dfe8ea7 167 yendsum = -le + ll/2*(sin(theta1)+sin(theta4));
Ramonwaninge 12:8d3bc1fa2321 168 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 169 yendsqrt2 = sqrt(pow((-xbase/ll + cos(theta1)+ cos(theta4)),2)+ pow((sin(theta1)-sin(theta4)),2));
Ramonwaninge 6:59744dfe8ea7 170 yend = (yendsum + yendsqrt1/yendsqrt2);
Ramonwaninge 12:8d3bc1fa2321 171
Ramonwaninge 3:de8d3ca44a3e 172 }
Ramonwaninge 12:8d3bc1fa2321 173 //deze onderstaande tickerfunctie wordt aangeroepen
Ramonwaninge 12:8d3bc1fa2321 174 void kinematics()
Ramonwaninge 12:8d3bc1fa2321 175 {
Ramonwaninge 12:8d3bc1fa2321 176
Ramonwaninge 12:8d3bc1fa2321 177 //Hieronder rekenen we eerst de aparte dingen van de jacobiaan uit. (sla maar over)
Ramonwaninge 12:8d3bc1fa2321 178
Ramonwaninge 12:8d3bc1fa2321 179 jacobiana = (500*(-(ll*(sin(theta1) - sin(0.001 - theta4)))/2. - ((-(xbase/ll) + cos(theta1) + cos(0.001 - theta4))*
Ramonwaninge 12:8d3bc1fa2321 180 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 181 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)) +
Ramonwaninge 12:8d3bc1fa2321 182 ((-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 183 (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 184 (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 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 (-lb - xbase - ll*(cos(0.001 - theta1) - cos(theta4)) - (2*(sin(0.001 - theta1) + sin(theta4))*
Ramonwaninge 12:8d3bc1fa2321 187 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 188 sqrt(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2)))/2.)*
Ramonwaninge 12:8d3bc1fa2321 189 (-(ll*(sin(theta1) - sin(0.001 - theta4)))/2. - ((-(xbase/ll) + cos(theta1) + cos(0.001 - theta4))*
Ramonwaninge 12:8d3bc1fa2321 190 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 191 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)) +
Ramonwaninge 12:8d3bc1fa2321 192 ((-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 193 (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 194 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 195 (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 196 ((-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 197 (ll*sqrt(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2))))*
Ramonwaninge 12:8d3bc1fa2321 198 ((-lb - xbase - ll*(cos(theta1) - cos(0.001 - theta4)) + (2*(sin(theta1) + sin(0.001 - theta4))*
Ramonwaninge 12:8d3bc1fa2321 199 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 200 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)))/2. +
Ramonwaninge 12:8d3bc1fa2321 201 (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 202 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 + theta4),2) + pow(sin(theta1) - sin(0.001 + theta4),2)))/2.));
Ramonwaninge 12:8d3bc1fa2321 203
Ramonwaninge 12:8d3bc1fa2321 204 jacobianb = (-500*((-lb - xbase - ll*(cos(theta1) - cos(0.001 - theta4)) + (2*(sin(theta1) + sin(0.001 - theta4))*
Ramonwaninge 12:8d3bc1fa2321 205 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 206 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)))/2. +
Ramonwaninge 12:8d3bc1fa2321 207 (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 208 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 + theta4),2) + pow(sin(theta1) - sin(0.001 + theta4),2)))/2.))/
Ramonwaninge 12:8d3bc1fa2321 209 (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 210 (-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 211 (-lb - xbase - ll*(cos(0.001 - theta1) - cos(theta4)) - (2*(sin(0.001 - theta1) + sin(theta4))*
Ramonwaninge 12:8d3bc1fa2321 212 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 213 sqrt(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2)))/2.)*
Ramonwaninge 12:8d3bc1fa2321 214 (-(ll*(sin(theta1) - sin(0.001 - theta4)))/2. - ((-(xbase/ll) + cos(theta1) + cos(0.001 - theta4))*
Ramonwaninge 12:8d3bc1fa2321 215 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 216 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)) +
Ramonwaninge 12:8d3bc1fa2321 217 ((-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 218 (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 219 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 220 (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 221 ((-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 222 (ll*sqrt(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2))))*
Ramonwaninge 12:8d3bc1fa2321 223 ((-lb - xbase - ll*(cos(theta1) - cos(0.001 - theta4)) + (2*(sin(theta1) + sin(0.001 - theta4))*
Ramonwaninge 12:8d3bc1fa2321 224 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 225 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)))/2. +
Ramonwaninge 12:8d3bc1fa2321 226 (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 227 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 + theta4),2) + pow(sin(theta1) - sin(0.001 + theta4),2)))/2.));
Ramonwaninge 12:8d3bc1fa2321 228
Ramonwaninge 12:8d3bc1fa2321 229 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 230 (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 231 ((-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 232 (ll*sqrt(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2)))))/
Ramonwaninge 12:8d3bc1fa2321 233 (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 234 (-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 235 (-lb - xbase - ll*(cos(0.001 - theta1) - cos(theta4)) - (2*(sin(0.001 - theta1) + sin(theta4))*
Ramonwaninge 12:8d3bc1fa2321 236 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 237 sqrt(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2)))/2.)*
Ramonwaninge 12:8d3bc1fa2321 238 (-(ll*(sin(theta1) - sin(0.001 - theta4)))/2. - ((-(xbase/ll) + cos(theta1) + cos(0.001 - theta4))*
Ramonwaninge 12:8d3bc1fa2321 239 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 240 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)) +
Ramonwaninge 12:8d3bc1fa2321 241 ((-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 242 (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 243 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 244 (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 245 ((-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 246 (ll*sqrt(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2))))*
Ramonwaninge 12:8d3bc1fa2321 247 ((-lb - xbase - ll*(cos(theta1) - cos(0.001 - theta4)) + (2*(sin(theta1) + sin(0.001 - theta4))*
Ramonwaninge 12:8d3bc1fa2321 248 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 249 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)))/2. +
Ramonwaninge 12:8d3bc1fa2321 250 (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 251 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 + theta4),2) + pow(sin(theta1) - sin(0.001 + theta4),2)))/2.));
Ramonwaninge 12:8d3bc1fa2321 252
Ramonwaninge 12:8d3bc1fa2321 253 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 254 sqrt(pow(-(xbase/ll) + cos(0.001 + theta1) + cos(theta4),2) + pow(sin(0.001 + theta1) - sin(theta4),2)))/2. +
Ramonwaninge 12:8d3bc1fa2321 255 (-lb - xbase - ll*(cos(0.001 - theta1) - cos(theta4)) - (2*(sin(0.001 - theta1) + sin(theta4))*
Ramonwaninge 12:8d3bc1fa2321 256 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 257 sqrt(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2)))/2.))/
Ramonwaninge 12:8d3bc1fa2321 258 (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 259 (-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 260 (-lb - xbase - ll*(cos(0.001 - theta1) - cos(theta4)) - (2*(sin(0.001 - theta1) + sin(theta4))*
Ramonwaninge 12:8d3bc1fa2321 261 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 262 sqrt(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2)))/2.)*
Ramonwaninge 12:8d3bc1fa2321 263 (-(ll*(sin(theta1) - sin(0.001 - theta4)))/2. - ((-(xbase/ll) + cos(theta1) + cos(0.001 - theta4))*
Ramonwaninge 12:8d3bc1fa2321 264 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 265 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)) +
Ramonwaninge 12:8d3bc1fa2321 266 ((-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 267 (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 268 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 269 (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 270 ((-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 271 (ll*sqrt(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2))))*
Ramonwaninge 12:8d3bc1fa2321 272 ((-lb - xbase - ll*(cos(theta1) - cos(0.001 - theta4)) + (2*(sin(theta1) + sin(0.001 - theta4))*
Ramonwaninge 12:8d3bc1fa2321 273 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 274 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)))/2. +
Ramonwaninge 12:8d3bc1fa2321 275 (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 276 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 + theta4),2) + pow(sin(theta1) - sin(0.001 + theta4),2)))/2.));
Ramonwaninge 12:8d3bc1fa2321 277
Ramonwaninge 12:8d3bc1fa2321 278 //vanaf hier weer door met lezen!
Ramonwaninge 12:8d3bc1fa2321 279 prefx = 1*(!button1); //sw3, dit is belangrijk! prefx staat voor P_(reference) en het is de snelheid van de endeffector als
Ramonwaninge 12:8d3bc1fa2321 280 // de button ingedrukt wordt (als emg = boven treshold) is de prefx 1 (da's de rode 1)
Ramonwaninge 12:8d3bc1fa2321 281 prefy = 1*(!button2); //sw2,
Ramonwaninge 12:8d3bc1fa2321 282 inverse(prefx, prefy);
Ramonwaninge 12:8d3bc1fa2321 283 }
Ramonwaninge 12:8d3bc1fa2321 284
Ramonwaninge 12:8d3bc1fa2321 285
Ramonwaninge 12:8d3bc1fa2321 286
Ramonwaninge 12:8d3bc1fa2321 287
Ramonwaninge 12:8d3bc1fa2321 288
Ramonwaninge 12:8d3bc1fa2321 289
Ramonwaninge 12:8d3bc1fa2321 290 /*In de nieuwe versie hieronder is forward overbodig geworden, sla maar over
Ramonwaninge 12:8d3bc1fa2321 291
Ramonwaninge 12:8d3bc1fa2321 292 Joe, hieronder staan de functies die door de tickers aangeroepen worden
Ramonwaninge 12:8d3bc1fa2321 293 void forward(){ dit is de ticker die zegt, als button=0, theta 1 wordt groter. dan worden x en y doorgerekend
Ramonwaninge 12:8d3bc1fa2321 294 hieronder moet veranderd worden naar if button1 == 0, x = x+eenbeetje
Ramonwaninge 12:8d3bc1fa2321 295 maar daar moet eerst inverse kinematics voor gebeuren.
Ramonwaninge 12:8d3bc1fa2321 296 if (button1 == 0){ als emg1==voorbij treshold, komt waarschijnlijk in de inverse functie
Ramonwaninge 12:8d3bc1fa2321 297 theta1 = PI*(theta1/PI + 0.1);
Ramonwaninge 12:8d3bc1fa2321 298 hij is geblokt omdat ik de knop nodig heb
Ramonwaninge 12:8d3bc1fa2321 299 default = als x = xbase/2... break, okee dit moet hier niet, maar weet niet waar wel...
Ramonwaninge 12:8d3bc1fa2321 300 }
Ramonwaninge 12:8d3bc1fa2321 301 else {theta1 = theta1;}
Ramonwaninge 12:8d3bc1fa2321 302 hieronder komen de doorrekeningen van de hoeken naar de coordinaten, check de mathematicafile voor de afleiding*/
Ramonwaninge 12:8d3bc1fa2321 303
Ramonwaninge 12:8d3bc1fa2321 304 /*void demomode(){} //Komt nog...
Ramonwaninge 12:8d3bc1fa2321 305
Ramonwaninge 12:8d3bc1fa2321 306 als emg2 == voorbij treshold,
Ramonwaninge 12:8d3bc1fa2321 307 double theta1 -> plus counts (emg*richting)
Ramonwaninge 12:8d3bc1fa2321 308 double theta4 -> plus counts (emg*richting)
Ramonwaninge 12:8d3bc1fa2321 309 reken y door
Ramonwaninge 12:8d3bc1fa2321 310
Ramonwaninge 12:8d3bc1fa2321 311 void flip(){
Ramonwaninge 10:2b965defcde5 312 if(button2==0){thetaflip = PI*(thetaflip/PI+0.5);} // button2==0 moet veranderd naar emg3>= treshold
Ramonwaninge 12:8d3bc1fa2321 313 }suppressed omdat ik button 2 nodig heb...*/
Ramonwaninge 12:8d3bc1fa2321 314
Ramonwaninge 7:b59b762c537e 315
Ramonwaninge 10:2b965defcde5 316 // de beweging voor de xcoordinaat!
Ramonwaninge 12:8d3bc1fa2321 317
Ramonwaninge 10:2b965defcde5 318
Ramonwaninge 12:8d3bc1fa2321 319 //tot aan hier overslaan
Ramonwaninge 13:f77c5f196161 320 void printvalue(){
Ramonwaninge 13:f77c5f196161 321 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 322
Ramonwaninge 13:f77c5f196161 323 }
Ramonwaninge 0:779fe292e912 324
Ramonwaninge 10:2b965defcde5 325
Ramonwaninge 0:779fe292e912 326 int main()
Ramonwaninge 0:779fe292e912 327 {
Ramonwaninge 12:8d3bc1fa2321 328 //Initial conditions
Ramonwaninge 12:8d3bc1fa2321 329
Ramonwaninge 12:8d3bc1fa2321 330 theta1 = PI*0.49;
Ramonwaninge 12:8d3bc1fa2321 331 theta4 = PI*0.49;
Ramonwaninge 2:0a7a3c0c08d3 332 pc.baud(115200);
Ramonwaninge 12:8d3bc1fa2321 333 //default = theta1 = theta4 = pi/2,
Ramonwaninge 13:f77c5f196161 334 kin.attach(kinematics, 0.01); // roep de ticker aan (
Ramonwaninge 13:f77c5f196161 335 simulateval.attach(printvalue, 1);
Ramonwaninge 13:f77c5f196161 336 greenled = 1; //First turn the LEDs off
Ramonwaninge 13:f77c5f196161 337 blueled = 1;
Ramonwaninge 13:f77c5f196161 338 redled = 1;
Ramonwaninge 13:f77c5f196161 339 filter0.add(&Notch50_0).add(&High0); //Make filter chain for the first EMG
Ramonwaninge 13:f77c5f196161 340 filter1.add(&Notch50_1).add(&High1); //Make filter chain for the second EMG
Ramonwaninge 13:f77c5f196161 341 button.rise(StopProgram); //If the button is pressed, stop program
Ramonwaninge 12:8d3bc1fa2321 342
Ramonwaninge 12:8d3bc1fa2321 343 pc.printf("%f", theta1);
Ramonwaninge 12:8d3bc1fa2321 344 while(true) {
Ramonwaninge 12:8d3bc1fa2321 345
Ramonwaninge 13:f77c5f196161 346 StateMachine(); //Start the state machine
Ramonwaninge 12:8d3bc1fa2321 347
Ramonwaninge 12:8d3bc1fa2321 348
Ramonwaninge 8:697aa3c94209 349 }
Ramonwaninge 0:779fe292e912 350 }