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

Committer:
EvaKrolis
Date:
Wed Oct 31 20:09:58 2018 +0000
Revision:
16:deb42ce3c3a1
Parent:
15:38258e6b6e91
Child:
20:11fe0aa7f111
It has been tested and it works!;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Ramonwaninge 15:38258e6b6e91 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 15:38258e6b6e91 11 MODSERIAL pc(USBTX, USBRX); // connecting to pc
Ramonwaninge 14:e3fe54f0a4b4 12 AnalogIn EMG0In(A0); //EMG input 0
Ramonwaninge 14:e3fe54f0a4b4 13 AnalogIn EMG1In(A1); //EMG input 1
Ramonwaninge 14:e3fe54f0a4b4 14 InterruptIn button(SW3); //Define button
Ramonwaninge 14:e3fe54f0a4b4 15 DigitalOut greenled(LED_GREEN); //Green led
Ramonwaninge 14:e3fe54f0a4b4 16 DigitalOut blueled(LED_BLUE); //Blue led
Ramonwaninge 14:e3fe54f0a4b4 17 DigitalOut redled(LED_RED); //Red led
Ramonwaninge 13:f77c5f196161 18
Ramonwaninge 13:f77c5f196161 19
Ramonwaninge 13:f77c5f196161 20
Ramonwaninge 8:697aa3c94209 21
Ramonwaninge 10:2b965defcde5 22 // nog te verwijderen/ aan te passen, zijn dubbel gedefinieerd
Ramonwaninge 13:f77c5f196161 23 double omega1;
Ramonwaninge 13:f77c5f196161 24 double omega4;
Ramonwaninge 13:f77c5f196161 25
Ramonwaninge 15:38258e6b6e91 26 //Constants
EvaKrolis 16:deb42ce3c3a1 27 const double ll = 198;
EvaKrolis 16:deb42ce3c3a1 28 const double lu = 230;
EvaKrolis 16:deb42ce3c3a1 29 const double lb = 50;
EvaKrolis 16:deb42ce3c3a1 30 const double le = 79;
EvaKrolis 16:deb42ce3c3a1 31 const double xbase = 450-lb;
Ramonwaninge 15:38258e6b6e91 32 const int Length = 10000; //Length of the array for the calibration
Ramonwaninge 15:38258e6b6e91 33 const int Parts = 50; //Mean average filter over 50 values
Ramonwaninge 15:38258e6b6e91 34
Ramonwaninge 0:779fe292e912 35
Ramonwaninge 14:e3fe54f0a4b4 36 //parameters for kinematics
Ramonwaninge 12:8d3bc1fa2321 37 //vorige theta
Ramonwaninge 12:8d3bc1fa2321 38 double theta1 = PI*0.49; // huidige/nieuwe theta
Ramonwaninge 12:8d3bc1fa2321 39 double theta4 = PI*0.49;
Ramonwaninge 14:e3fe54f0a4b4 40 double thetaflip = 0; //angle of the flipping motor
Ramonwaninge 14:e3fe54f0a4b4 41 double prefx; //Preference for x, will later be defined due to the motor
Ramonwaninge 14:e3fe54f0a4b4 42 double prefy; //" "
Ramonwaninge 14:e3fe54f0a4b4 43 double deltat = 0.01; //tijdstap(moet nog aangepast worden)
Ramonwaninge 14:e3fe54f0a4b4 44
Ramonwaninge 14:e3fe54f0a4b4 45 //Parameters for the first EMG signal
Ramonwaninge 14:e3fe54f0a4b4 46 float EMG0; //float for EMG input
Ramonwaninge 14:e3fe54f0a4b4 47 float EMG0filt; //float for filtered EMG
Ramonwaninge 14:e3fe54f0a4b4 48 float EMG0filtArray[Parts]; //Array for the filtered array
Ramonwaninge 14:e3fe54f0a4b4 49 float EMG0Average; //float for the value after Moving Average Filter
Ramonwaninge 14:e3fe54f0a4b4 50 float Sum0 = 0; //Sum0 for the moving average filter
Ramonwaninge 15:38258e6b6e91 51 float EMG0Calibrate[Length]; //Array for the calibration
Ramonwaninge 14:e3fe54f0a4b4 52 int ReadCal0 = 0; //Integer to read over the calibration array
Ramonwaninge 14:e3fe54f0a4b4 53 float MaxValue0 = 0; //float to save the max muscle
Ramonwaninge 14:e3fe54f0a4b4 54 float Threshold0 = 0; //Threshold for the first EMG signal
Ramonwaninge 13:f77c5f196161 55
Ramonwaninge 14:e3fe54f0a4b4 56 //Parameters for the second EMG signal
Ramonwaninge 14:e3fe54f0a4b4 57 float EMG1; //float for EMG input
Ramonwaninge 14:e3fe54f0a4b4 58 float EMG1filt; //float for filtered EMG
Ramonwaninge 14:e3fe54f0a4b4 59 float EMG1filtArray[Parts]; //Array for the filtered array
Ramonwaninge 14:e3fe54f0a4b4 60 float EMG1Average; //float for the value after Moving Average Filter
Ramonwaninge 14:e3fe54f0a4b4 61 float Sum1 = 0; //Sum0 for the moving average filter
Ramonwaninge 15:38258e6b6e91 62 float EMG1Calibrate[Length]; //Array for the calibration
Ramonwaninge 14:e3fe54f0a4b4 63 int ReadCal1 = 0; //Integer to read over the calibration array
Ramonwaninge 14:e3fe54f0a4b4 64 float MaxValue1 = 0; //float to save the max muscle
Ramonwaninge 14:e3fe54f0a4b4 65 float Threshold1 = 0; //Threshold for the second EMG signal
Ramonwaninge 14:e3fe54f0a4b4 66
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 15:38258e6b6e91 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 15:38258e6b6e91 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
EvaKrolis 16:deb42ce3c3a1 115 States CurrentState = WorkingMode; //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 15:38258e6b6e91 120 void ReadUseEMG0()
Ramonwaninge 15:38258e6b6e91 121 {
Ramonwaninge 15:38258e6b6e91 122 for(int i = Parts ; i > 0 ; i--) { //Make a first in, first out array
Ramonwaninge 14:e3fe54f0a4b4 123 EMG0filtArray[i] = EMG0filtArray[i-1]; //Every value moves one up
Ramonwaninge 14:e3fe54f0a4b4 124 }
Ramonwaninge 15:38258e6b6e91 125
Ramonwaninge 15:38258e6b6e91 126 Sum0 = 0;
Ramonwaninge 15:38258e6b6e91 127 EMG0 = EMG0In; //Save EMG input in float
Ramonwaninge 15:38258e6b6e91 128 EMG0filt = filter0.step(EMG0); //Filter the signal
Ramonwaninge 15:38258e6b6e91 129 EMG0filt = abs(EMG0filt); //Take the absolute value
Ramonwaninge 15:38258e6b6e91 130 EMG0filtArray[0] = EMG0filt; //Save the filtered signal on the first place in the array
Ramonwaninge 15:38258e6b6e91 131
Ramonwaninge 15:38258e6b6e91 132 for(int i = 0 ; i < Parts ; i++) { //Moving Average filter
Ramonwaninge 15:38258e6b6e91 133 Sum0 += EMG0filtArray[i]; //Sum the new value and the previous 49
Ramonwaninge 15:38258e6b6e91 134 }
Ramonwaninge 15:38258e6b6e91 135 EMG0Average = (float)Sum0/Parts; //Divide the sum by 50
Ramonwaninge 15:38258e6b6e91 136
Ramonwaninge 15:38258e6b6e91 137 if (EMG0Average > Threshold0) { //If the value is higher than the threshold value
Ramonwaninge 15:38258e6b6e91 138 redled = 0; //Turn the LED on
Ramonwaninge 15:38258e6b6e91 139 xMove = true; //Set movement to true
Ramonwaninge 15:38258e6b6e91 140 } else {
Ramonwaninge 15:38258e6b6e91 141 redled = 1; //Otherwise turn the LED off
Ramonwaninge 15:38258e6b6e91 142 xMove = false; //Otherwise set movement to false
Ramonwaninge 15:38258e6b6e91 143 }
Ramonwaninge 15:38258e6b6e91 144 }
Ramonwaninge 15:38258e6b6e91 145 //Function to read and filter the EMG
Ramonwaninge 15:38258e6b6e91 146 void ReadUseEMG1()
Ramonwaninge 15:38258e6b6e91 147 {
Ramonwaninge 15:38258e6b6e91 148 for(int i = Parts ; i > 0 ; i--) { //Make a first in, first out array
Ramonwaninge 15:38258e6b6e91 149 EMG1filtArray[i] = EMG1filtArray[i-1]; //Every value moves one up
Ramonwaninge 15:38258e6b6e91 150 }
Ramonwaninge 15:38258e6b6e91 151
Ramonwaninge 15:38258e6b6e91 152 Sum1 = 0;
Ramonwaninge 15:38258e6b6e91 153 EMG1 = EMG1In; //Save EMG input in float
Ramonwaninge 15:38258e6b6e91 154 EMG1filt = filter1.step(EMG1); //Filter the signal
Ramonwaninge 15:38258e6b6e91 155 EMG1filt = abs(EMG1filt); //Take the absolute value
Ramonwaninge 15:38258e6b6e91 156 EMG1filtArray[0] = EMG1filt; //Save the filtered signal on the first place in the array
Ramonwaninge 15:38258e6b6e91 157
Ramonwaninge 15:38258e6b6e91 158 for(int i = 0 ; i < Parts ; i++) { //Moving Average filter
Ramonwaninge 15:38258e6b6e91 159 Sum1 += EMG1filtArray[i]; //Sum the new value and the previous 49
Ramonwaninge 15:38258e6b6e91 160 }
Ramonwaninge 15:38258e6b6e91 161 EMG1Average = (float)Sum1/Parts; //Divide the sum by 50
Ramonwaninge 15:38258e6b6e91 162
Ramonwaninge 15:38258e6b6e91 163 if (EMG1Average > Threshold1) { //If the value is higher than the threshold value
Ramonwaninge 15:38258e6b6e91 164 greenled = 0; //Turn the LED on
Ramonwaninge 15:38258e6b6e91 165 yMove = true; //Set y movement to true
Ramonwaninge 15:38258e6b6e91 166 } else {
Ramonwaninge 15:38258e6b6e91 167 greenled = 1; //Otherwise turn the LED off
EvaKrolis 16:deb42ce3c3a1 168 yMove = false; //Otherwise set y movement to false
Ramonwaninge 15:38258e6b6e91 169 }
Ramonwaninge 15:38258e6b6e91 170 }
Ramonwaninge 15:38258e6b6e91 171
Ramonwaninge 15:38258e6b6e91 172
Ramonwaninge 15:38258e6b6e91 173 //Function to make an array during the calibration
Ramonwaninge 15:38258e6b6e91 174 void CalibrateEMG0()
Ramonwaninge 15:38258e6b6e91 175 {
Ramonwaninge 15:38258e6b6e91 176 for(int i = Parts ; i > 0 ; i--) { //Make a first in, first out array
Ramonwaninge 15:38258e6b6e91 177 EMG0filtArray[i] = EMG0filtArray[i-1]; //Every value moves one up
Ramonwaninge 15:38258e6b6e91 178 }
Ramonwaninge 15:38258e6b6e91 179
Ramonwaninge 14:e3fe54f0a4b4 180 Sum0 = 0;
Ramonwaninge 14:e3fe54f0a4b4 181 EMG0 = EMG0In; //Save EMG input in float
Ramonwaninge 14:e3fe54f0a4b4 182 EMG0filt = filter0.step(EMG0); //Filter the signal
Ramonwaninge 14:e3fe54f0a4b4 183 EMG0filt = abs(EMG0filt); //Take the absolute value
Ramonwaninge 14:e3fe54f0a4b4 184 EMG0filtArray[0] = EMG0filt; //Save the filtered signal on the first place in the array
Ramonwaninge 15:38258e6b6e91 185
Ramonwaninge 15:38258e6b6e91 186 for(int i = 0 ; i < Parts ; i++) { //Moving Average filter
Ramonwaninge 14:e3fe54f0a4b4 187 Sum0 += EMG0filtArray[i]; //Sum the new value and the previous 49
Ramonwaninge 14:e3fe54f0a4b4 188 }
Ramonwaninge 15:38258e6b6e91 189 EMG0Calibrate[ReadCal0] = (float)Sum0/Parts; //Divide the sum by 50
Ramonwaninge 15:38258e6b6e91 190
Ramonwaninge 15:38258e6b6e91 191 ReadCal0++;
Ramonwaninge 15:38258e6b6e91 192 }
Ramonwaninge 15:38258e6b6e91 193
Ramonwaninge 15:38258e6b6e91 194 //Function to make an array during the calibration
Ramonwaninge 15:38258e6b6e91 195 void CalibrateEMG1()
Ramonwaninge 15:38258e6b6e91 196 {
Ramonwaninge 15:38258e6b6e91 197 for(int i = Parts ; i > 0 ; i--) { //Make a first in, first out array
Ramonwaninge 15:38258e6b6e91 198 EMG1filtArray[i] = EMG1filtArray[i-1]; //Every value moves one up
Ramonwaninge 15:38258e6b6e91 199 }
Ramonwaninge 15:38258e6b6e91 200
Ramonwaninge 15:38258e6b6e91 201 Sum1 = 0;
Ramonwaninge 15:38258e6b6e91 202 EMG1 = EMG1In; //Save EMG input in float
Ramonwaninge 15:38258e6b6e91 203 EMG1filt = filter1.step(EMG1); //Filter the signal
Ramonwaninge 15:38258e6b6e91 204 EMG1filt = abs(EMG1filt); //Take the absolute value
Ramonwaninge 15:38258e6b6e91 205 EMG1filtArray[0] = EMG1filt; //Save the filtered signal on the first place in the array
Ramonwaninge 15:38258e6b6e91 206
Ramonwaninge 15:38258e6b6e91 207 for(int i = 0 ; i < Parts ; i++) { //Moving Average filter
Ramonwaninge 15:38258e6b6e91 208 Sum1 += EMG1filtArray[i]; //Sum the new value and the previous 49
Ramonwaninge 14:e3fe54f0a4b4 209 }
Ramonwaninge 15:38258e6b6e91 210 EMG1Calibrate[ReadCal1] = (float)Sum1/Parts; //Divide the sum by 50
Ramonwaninge 15:38258e6b6e91 211
Ramonwaninge 15:38258e6b6e91 212 ReadCal1++;
Ramonwaninge 15:38258e6b6e91 213 }
Ramonwaninge 15:38258e6b6e91 214
Ramonwaninge 15:38258e6b6e91 215 //Function to find the max value during the calibration
Ramonwaninge 15:38258e6b6e91 216 void FindMax0()
Ramonwaninge 15:38258e6b6e91 217 {
Ramonwaninge 15:38258e6b6e91 218 MaxValue0 = *max_element(EMG0Calibrate+500,EMG0Calibrate+Length); //Find max value, but discard the first 100 values
Ramonwaninge 15:38258e6b6e91 219 Threshold0 = 0.30f*MaxValue0; //The threshold is a percentage of the max value
Ramonwaninge 15:38258e6b6e91 220 pc.printf("The calibration value of the first EMG is %f.\n\r The threshold is %f. \n\r",MaxValue0,Threshold0); //Print the max value and the threshold
Ramonwaninge 15:38258e6b6e91 221 FindMax0_timer.detach(); //Detach the timer, so you only use this once
Ramonwaninge 15:38258e6b6e91 222 }
Ramonwaninge 15:38258e6b6e91 223
Ramonwaninge 15:38258e6b6e91 224 //Function to find the max value during the calibration
Ramonwaninge 15:38258e6b6e91 225 void FindMax1()
Ramonwaninge 15:38258e6b6e91 226 {
Ramonwaninge 15:38258e6b6e91 227 MaxValue1 = *max_element(EMG1Calibrate+500,EMG1Calibrate+Length); //Find max value, but discard the first 100 values
Ramonwaninge 15:38258e6b6e91 228 Threshold1 = 0.30f*MaxValue1; //The threshold is a percentage of the max value
Ramonwaninge 15:38258e6b6e91 229 pc.printf("The calibration value of the second EMG is %f.\n\r The threshold is %f. \n\r",MaxValue1,Threshold1); //Print the Max value and the threshold
Ramonwaninge 15:38258e6b6e91 230 FindMax1_timer.detach(); //Detach the timer, so you only use this once
Ramonwaninge 15:38258e6b6e91 231 }
Ramonwaninge 15:38258e6b6e91 232
Ramonwaninge 15:38258e6b6e91 233 //Function to stop the reading of the EMG
Ramonwaninge 15:38258e6b6e91 234 void StopProgram()
Ramonwaninge 15:38258e6b6e91 235 {
Ramonwaninge 15:38258e6b6e91 236 greenled = 1; //Turn the LEDs off
Ramonwaninge 15:38258e6b6e91 237 blueled = 1;
Ramonwaninge 15:38258e6b6e91 238 redled = 1;
Ramonwaninge 15:38258e6b6e91 239 exit (0); //Abort mission!!
Ramonwaninge 15:38258e6b6e91 240 }
Ramonwaninge 15:38258e6b6e91 241
Ramonwaninge 15:38258e6b6e91 242 //Function to switch a state
Ramonwaninge 15:38258e6b6e91 243 void SwitchState()
Ramonwaninge 15:38258e6b6e91 244 {
Ramonwaninge 15:38258e6b6e91 245 SwitchStateBool = true; //Set the bool for the start of a state to true
Ramonwaninge 15:38258e6b6e91 246 SwitchState_timer.detach(); //Use this function once
Ramonwaninge 14:e3fe54f0a4b4 247 }
Ramonwaninge 14:e3fe54f0a4b4 248
Ramonwaninge 14:e3fe54f0a4b4 249
Ramonwaninge 14:e3fe54f0a4b4 250
Ramonwaninge 12:8d3bc1fa2321 251 //dit wordt aangeroepen in de tickerfunctie
Ramonwaninge 15:38258e6b6e91 252 void inverse(double prex, double prey)
Ramonwaninge 15:38258e6b6e91 253 {
Ramonwaninge 12:8d3bc1fa2321 254 /*
Ramonwaninge 12:8d3bc1fa2321 255 qn = qn-1 + (jacobian^-1)*dPref/dt *deltaT
Ramonwaninge 12:8d3bc1fa2321 256 ofwel
Ramonwaninge 12:8d3bc1fa2321 257 thetai+1 = thetai +(jacobian)^-1*vector(deltaX, DeltaY)
Ramonwaninge 12:8d3bc1fa2321 258 waar Pref = emg signaal
Ramonwaninge 12:8d3bc1fa2321 259 */ //achtergrondinfo hierboven...
Ramonwaninge 15:38258e6b6e91 260 //
Ramonwaninge 15:38258e6b6e91 261
Ramonwaninge 12:8d3bc1fa2321 262 theta1 += (prefx*jacobiana+jacobianb*prey)*deltat; //theta 1 is zichzelf plus wat hier staat (is kinematics)
Ramonwaninge 12:8d3bc1fa2321 263 theta4 += (prefx*jacobianc+jacobiand*prey)*deltat;//" "
Ramonwaninge 15:38258e6b6e91 264 //Hier worden xend en yend doorgerekend, die formules kan je overslaan
Ramonwaninge 4:49dfbfcd3577 265 xendsum = lb + xbase +ll*(cos(theta1) - cos(theta4));
Ramonwaninge 4:49dfbfcd3577 266 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 267 xendsqrt2 = sqrt(pow((-xbase/ll+cos(theta1)+cos(theta4)),2)+ pow(sin(theta1) - sin(theta4),2));
Ramonwaninge 2:0a7a3c0c08d3 268 xend = (xendsum + xendsqrt1/xendsqrt2)/2;
Ramonwaninge 12:8d3bc1fa2321 269 //hieronder rekenen we yendeffector door;
Ramonwaninge 6:59744dfe8ea7 270 yendsum = -le + ll/2*(sin(theta1)+sin(theta4));
Ramonwaninge 12:8d3bc1fa2321 271 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 272 yendsqrt2 = sqrt(pow((-xbase/ll + cos(theta1)+ cos(theta4)),2)+ pow((sin(theta1)-sin(theta4)),2));
Ramonwaninge 6:59744dfe8ea7 273 yend = (yendsum + yendsqrt1/yendsqrt2);
Ramonwaninge 12:8d3bc1fa2321 274
Ramonwaninge 3:de8d3ca44a3e 275 }
Ramonwaninge 12:8d3bc1fa2321 276 //deze onderstaande tickerfunctie wordt aangeroepen
Ramonwaninge 12:8d3bc1fa2321 277 void kinematics()
Ramonwaninge 12:8d3bc1fa2321 278 {
Ramonwaninge 12:8d3bc1fa2321 279
Ramonwaninge 12:8d3bc1fa2321 280 //Hieronder rekenen we eerst de aparte dingen van de jacobiaan uit. (sla maar over)
Ramonwaninge 12:8d3bc1fa2321 281
Ramonwaninge 12:8d3bc1fa2321 282 jacobiana = (500*(-(ll*(sin(theta1) - sin(0.001 - theta4)))/2. - ((-(xbase/ll) + cos(theta1) + cos(0.001 - theta4))*
Ramonwaninge 12:8d3bc1fa2321 283 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 284 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)) +
Ramonwaninge 12:8d3bc1fa2321 285 ((-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 286 (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 287 (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 288 sqrt(pow(-(xbase/ll) + cos(0.001 + theta1) + cos(theta4),2) + pow(sin(0.001 + theta1) - sin(theta4),2)))/2. +
Ramonwaninge 12:8d3bc1fa2321 289 (-lb - xbase - ll*(cos(0.001 - theta1) - cos(theta4)) - (2*(sin(0.001 - theta1) + sin(theta4))*
Ramonwaninge 12:8d3bc1fa2321 290 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 291 sqrt(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2)))/2.)*
Ramonwaninge 12:8d3bc1fa2321 292 (-(ll*(sin(theta1) - sin(0.001 - theta4)))/2. - ((-(xbase/ll) + cos(theta1) + cos(0.001 - theta4))*
Ramonwaninge 12:8d3bc1fa2321 293 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 294 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)) +
Ramonwaninge 12:8d3bc1fa2321 295 ((-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 296 (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 297 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 298 (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 299 ((-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 300 (ll*sqrt(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2))))*
Ramonwaninge 12:8d3bc1fa2321 301 ((-lb - xbase - ll*(cos(theta1) - cos(0.001 - theta4)) + (2*(sin(theta1) + sin(0.001 - theta4))*
Ramonwaninge 12:8d3bc1fa2321 302 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 303 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)))/2. +
Ramonwaninge 12:8d3bc1fa2321 304 (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 305 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 + theta4),2) + pow(sin(theta1) - sin(0.001 + theta4),2)))/2.));
Ramonwaninge 12:8d3bc1fa2321 306
Ramonwaninge 12:8d3bc1fa2321 307 jacobianb = (-500*((-lb - xbase - ll*(cos(theta1) - cos(0.001 - theta4)) + (2*(sin(theta1) + sin(0.001 - theta4))*
Ramonwaninge 12:8d3bc1fa2321 308 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 309 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)))/2. +
Ramonwaninge 12:8d3bc1fa2321 310 (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 311 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 + theta4),2) + pow(sin(theta1) - sin(0.001 + theta4),2)))/2.))/
Ramonwaninge 12:8d3bc1fa2321 312 (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 313 (-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 314 (-lb - xbase - ll*(cos(0.001 - theta1) - cos(theta4)) - (2*(sin(0.001 - theta1) + sin(theta4))*
Ramonwaninge 12:8d3bc1fa2321 315 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 316 sqrt(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2)))/2.)*
Ramonwaninge 12:8d3bc1fa2321 317 (-(ll*(sin(theta1) - sin(0.001 - theta4)))/2. - ((-(xbase/ll) + cos(theta1) + cos(0.001 - theta4))*
Ramonwaninge 12:8d3bc1fa2321 318 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 319 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)) +
Ramonwaninge 12:8d3bc1fa2321 320 ((-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 321 (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 322 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 323 (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 324 ((-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 325 (ll*sqrt(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2))))*
Ramonwaninge 12:8d3bc1fa2321 326 ((-lb - xbase - ll*(cos(theta1) - cos(0.001 - theta4)) + (2*(sin(theta1) + sin(0.001 - theta4))*
Ramonwaninge 12:8d3bc1fa2321 327 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 328 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)))/2. +
Ramonwaninge 12:8d3bc1fa2321 329 (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 330 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 + theta4),2) + pow(sin(theta1) - sin(0.001 + theta4),2)))/2.));
Ramonwaninge 12:8d3bc1fa2321 331
Ramonwaninge 12:8d3bc1fa2321 332 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 333 (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 334 ((-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 335 (ll*sqrt(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2)))))/
Ramonwaninge 12:8d3bc1fa2321 336 (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 337 (-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 338 (-lb - xbase - ll*(cos(0.001 - theta1) - cos(theta4)) - (2*(sin(0.001 - theta1) + sin(theta4))*
Ramonwaninge 12:8d3bc1fa2321 339 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 340 sqrt(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2)))/2.)*
Ramonwaninge 12:8d3bc1fa2321 341 (-(ll*(sin(theta1) - sin(0.001 - theta4)))/2. - ((-(xbase/ll) + cos(theta1) + cos(0.001 - theta4))*
Ramonwaninge 12:8d3bc1fa2321 342 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 343 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)) +
Ramonwaninge 12:8d3bc1fa2321 344 ((-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 345 (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 346 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 347 (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 348 ((-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 349 (ll*sqrt(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2))))*
Ramonwaninge 12:8d3bc1fa2321 350 ((-lb - xbase - ll*(cos(theta1) - cos(0.001 - theta4)) + (2*(sin(theta1) + sin(0.001 - theta4))*
Ramonwaninge 12:8d3bc1fa2321 351 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 352 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)))/2. +
Ramonwaninge 12:8d3bc1fa2321 353 (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 354 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 + theta4),2) + pow(sin(theta1) - sin(0.001 + theta4),2)))/2.));
Ramonwaninge 12:8d3bc1fa2321 355
Ramonwaninge 12:8d3bc1fa2321 356 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 357 sqrt(pow(-(xbase/ll) + cos(0.001 + theta1) + cos(theta4),2) + pow(sin(0.001 + theta1) - sin(theta4),2)))/2. +
Ramonwaninge 12:8d3bc1fa2321 358 (-lb - xbase - ll*(cos(0.001 - theta1) - cos(theta4)) - (2*(sin(0.001 - theta1) + sin(theta4))*
Ramonwaninge 12:8d3bc1fa2321 359 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 360 sqrt(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2)))/2.))/
Ramonwaninge 12:8d3bc1fa2321 361 (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 362 (-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 363 (-lb - xbase - ll*(cos(0.001 - theta1) - cos(theta4)) - (2*(sin(0.001 - theta1) + sin(theta4))*
Ramonwaninge 12:8d3bc1fa2321 364 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 365 sqrt(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2)))/2.)*
Ramonwaninge 12:8d3bc1fa2321 366 (-(ll*(sin(theta1) - sin(0.001 - theta4)))/2. - ((-(xbase/ll) + cos(theta1) + cos(0.001 - theta4))*
Ramonwaninge 12:8d3bc1fa2321 367 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 368 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)) +
Ramonwaninge 12:8d3bc1fa2321 369 ((-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 370 (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 371 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 372 (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 373 ((-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 374 (ll*sqrt(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2))))*
Ramonwaninge 12:8d3bc1fa2321 375 ((-lb - xbase - ll*(cos(theta1) - cos(0.001 - theta4)) + (2*(sin(theta1) + sin(0.001 - theta4))*
Ramonwaninge 12:8d3bc1fa2321 376 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 377 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)))/2. +
Ramonwaninge 12:8d3bc1fa2321 378 (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 379 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 + theta4),2) + pow(sin(theta1) - sin(0.001 + theta4),2)))/2.));
Ramonwaninge 12:8d3bc1fa2321 380
Ramonwaninge 15:38258e6b6e91 381 //vanaf hier weer door met lezen!
EvaKrolis 16:deb42ce3c3a1 382 prefx = 10*(xMove); //sw3, dit is belangrijk! prefx staat voor P_(reference) en het is de snelheid van de endeffector als
Ramonwaninge 15:38258e6b6e91 383 // de button ingedrukt wordt (als emg = boven treshold) is de prefx 1 (da's de rode 1)
EvaKrolis 16:deb42ce3c3a1 384 prefy = 10*(yMove); //sw2,
Ramonwaninge 12:8d3bc1fa2321 385 inverse(prefx, prefy);
Ramonwaninge 12:8d3bc1fa2321 386 }
Ramonwaninge 12:8d3bc1fa2321 387
EvaKrolis 16:deb42ce3c3a1 388 void printvalue()
EvaKrolis 16:deb42ce3c3a1 389 {
EvaKrolis 16:deb42ce3c3a1 390 pc.printf("X-value: %f \t Y-value: %f \n\r",xend, yend); // in teraterm zijn de bovenste twee waardes hoeken, de onderste twee zijn de x en y coordinaat
EvaKrolis 16:deb42ce3c3a1 391
EvaKrolis 16:deb42ce3c3a1 392 }
Ramonwaninge 12:8d3bc1fa2321 393
Ramonwaninge 15:38258e6b6e91 394 //State Machine
Ramonwaninge 15:38258e6b6e91 395 void StateMachine()
Ramonwaninge 15:38258e6b6e91 396 {
Ramonwaninge 15:38258e6b6e91 397 switch(CurrentState) { //Determine in which state you are
Ramonwaninge 15:38258e6b6e91 398
Ramonwaninge 15:38258e6b6e91 399 case Calibration: //Calibration mode
Ramonwaninge 15:38258e6b6e91 400 if(StateBool) { //If you go into this state
Ramonwaninge 15:38258e6b6e91 401 pc.printf("You can start calibrating. \n\r"); //Print that you are in this state
Ramonwaninge 15:38258e6b6e91 402 StateBool = false; //Set the bool for the start of a state to false
Ramonwaninge 15:38258e6b6e91 403 EMGCalibration0_timer.attach(&CalibrateEMG0,0.005); //Start EMG calibration every 0.005 seconds
Ramonwaninge 15:38258e6b6e91 404 EMGCalibration1_timer.attach(&CalibrateEMG1,0.005); //Start EMG calibration every 0.005 seconds
Ramonwaninge 15:38258e6b6e91 405 FindMax0_timer.attach(&FindMax0,15); //Find the maximum value after 15 seconds
Ramonwaninge 15:38258e6b6e91 406 FindMax1_timer.attach(&FindMax1,15); //Find the maximum value after 15 seconds
Ramonwaninge 15:38258e6b6e91 407 SwitchState_timer.attach(&SwitchState,20); //Switch to the next state after 16 seconds
Ramonwaninge 15:38258e6b6e91 408 blueled = 0;
Ramonwaninge 15:38258e6b6e91 409 }
Ramonwaninge 15:38258e6b6e91 410
Ramonwaninge 15:38258e6b6e91 411 if (SwitchStateBool) { //If the bool is changed
Ramonwaninge 15:38258e6b6e91 412 CurrentState = WorkingMode; //Change the state to the working mode
Ramonwaninge 15:38258e6b6e91 413 StateBool = true; //Set the start of a state bool to true
Ramonwaninge 15:38258e6b6e91 414 SwitchStateBool = false; //Set the switch bool to false
Ramonwaninge 15:38258e6b6e91 415 }
Ramonwaninge 15:38258e6b6e91 416 break;
Ramonwaninge 15:38258e6b6e91 417
Ramonwaninge 15:38258e6b6e91 418 case WorkingMode: //Mode to get the robot working
Ramonwaninge 15:38258e6b6e91 419 if (StateBool) { //If you start to go in this state
Ramonwaninge 15:38258e6b6e91 420 pc.printf("You are know in the working mode. \r\n"); //Print in which mode you are
Ramonwaninge 15:38258e6b6e91 421 StateBool = false; //Set the start of state bool to true
Ramonwaninge 15:38258e6b6e91 422 EMGCalibration0_timer.detach(); //Detach the the calibration
Ramonwaninge 15:38258e6b6e91 423 EMGCalibration1_timer.detach(); //Detach the calibration
Ramonwaninge 15:38258e6b6e91 424 ReadUseEMG0_timer.attach(&ReadUseEMG0, 0.005); //Start the use of EMG
Ramonwaninge 15:38258e6b6e91 425 ReadUseEMG1_timer.attach(&ReadUseEMG1,0.005); //Start the use of EMG
EvaKrolis 16:deb42ce3c3a1 426 kin.attach(kinematics, 1); // roep de ticker aan (
EvaKrolis 16:deb42ce3c3a1 427 simulateval.attach(printvalue, 1);
Ramonwaninge 15:38258e6b6e91 428 }
Ramonwaninge 15:38258e6b6e91 429 blueled = 1; //Set the blue led off
EvaKrolis 16:deb42ce3c3a1 430 //pc.printf("First EMG: %f, Second EMG: %f \n\r",EMG0Average,EMG1Average);
Ramonwaninge 15:38258e6b6e91 431 break;
Ramonwaninge 15:38258e6b6e91 432 }
Ramonwaninge 15:38258e6b6e91 433 }
Ramonwaninge 15:38258e6b6e91 434
Ramonwaninge 0:779fe292e912 435 int main()
Ramonwaninge 0:779fe292e912 436 {
Ramonwaninge 12:8d3bc1fa2321 437 //Initial conditions
Ramonwaninge 12:8d3bc1fa2321 438
Ramonwaninge 12:8d3bc1fa2321 439 theta1 = PI*0.49;
Ramonwaninge 12:8d3bc1fa2321 440 theta4 = PI*0.49;
Ramonwaninge 2:0a7a3c0c08d3 441 pc.baud(115200);
Ramonwaninge 13:f77c5f196161 442 greenled = 1; //First turn the LEDs off
Ramonwaninge 13:f77c5f196161 443 blueled = 1;
Ramonwaninge 13:f77c5f196161 444 redled = 1;
Ramonwaninge 13:f77c5f196161 445 filter0.add(&Notch50_0).add(&High0); //Make filter chain for the first EMG
Ramonwaninge 13:f77c5f196161 446 filter1.add(&Notch50_1).add(&High1); //Make filter chain for the second EMG
Ramonwaninge 13:f77c5f196161 447 button.rise(StopProgram); //If the button is pressed, stop program
EvaKrolis 16:deb42ce3c3a1 448
Ramonwaninge 12:8d3bc1fa2321 449 while(true) {
Ramonwaninge 12:8d3bc1fa2321 450
Ramonwaninge 13:f77c5f196161 451 StateMachine(); //Start the state machine
Ramonwaninge 8:697aa3c94209 452 }
Ramonwaninge 0:779fe292e912 453 }