Tommie Verouden / Mbed 2 deprecated StateMachine

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   biquadFilter FastPWM MODSERIAL QEI mbed

main.cpp@14:2c0bf576a0e7, 2018-11-01 (annotated)

Committer:
EvaKrolis
Date:
Thu Nov 01 10:02:07 2018 +0000
Revision:
14:2c0bf576a0e7
Parent:
13:4ba8f63e6ff4
Child:
16:c2986e890040
StateMachine with EMG and Kinematics functions and parameters

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tverouden 0:c0c35b95765f 1 // ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡ PREPARATION ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡
tverouden 0:c0c35b95765f 2 // Libraries
tverouden 0:c0c35b95765f 3 #include "mbed.h"
tverouden 2:d70795e4e0bf 4 #include "BiQuad.h"
tverouden 0:c0c35b95765f 5 #include "FastPWM.h"
tverouden 0:c0c35b95765f 6 #include "HIDScope.h"
tverouden 0:c0c35b95765f 7 #include "MODSERIAL.h"
EvaKrolis 13:4ba8f63e6ff4 8 #include <algorithm>
EvaKrolis 14:2c0bf576a0e7 9 #define PI 3.14159265
tverouden 0:c0c35b95765f 10
tverouden 8:8cef1050ebd9 11 // LEDs
tverouden 8:8cef1050ebd9 12 DigitalOut ledRed(LED_RED,1); // red LED K64F
tverouden 8:8cef1050ebd9 13 DigitalOut ledGreen(LED_GREEN,1); // green LED K64F
tverouden 8:8cef1050ebd9 14 DigitalOut ledBlue(LED_BLUE,1); // blue LED K64F
tverouden 8:8cef1050ebd9 15 // DigitalOut ledBio1(,1); // led 1 Biorobotics shield // LED pins
tverouden 8:8cef1050ebd9 16 // DigitalOut ledBio2(,1); // led 2 Biorobotics shield
tverouden 8:8cef1050ebd9 17
tverouden 11:2d1dfebae948 18 Ticker blinkTimer; // LED ticker
tverouden 8:8cef1050ebd9 19
tverouden 8:8cef1050ebd9 20 // Buttons/inputs
tverouden 4:5ce2c8864908 21 InterruptIn buttonBio1(D0); // button 1 BioRobotics shield
tverouden 4:5ce2c8864908 22 InterruptIn buttonBio2(D1); // button 2 BioRobotics shield
tverouden 0:c0c35b95765f 23 InterruptIn buttonK64F(SW3); // button on K64F
tverouden 8:8cef1050ebd9 24 InterruptIn buttonEmergency(SW2); // emergency button on K64F
tverouden 0:c0c35b95765f 25
tverouden 8:8cef1050ebd9 26 // Motor pins
tverouden 8:8cef1050ebd9 27
tverouden 8:8cef1050ebd9 28
tverouden 8:8cef1050ebd9 29 // PC communication
tverouden 4:5ce2c8864908 30 MODSERIAL pc(USBTX, USBRX); // communication with pc
tverouden 4:5ce2c8864908 31
tverouden 0:c0c35b95765f 32 // Define & initialise state machine
tverouden 11:2d1dfebae948 33 enum states { calibratingMotors, calibratingEMG,
tverouden 7:ef5966469621 34 homing, operating, reading, failing, demoing
tverouden 2:d70795e4e0bf 35 };
tverouden 11:2d1dfebae948 36 states currentState = calibratingMotors;// start in waiting mode
tverouden 11:2d1dfebae948 37 bool changeState = true; // initialise the first state
tverouden 2:d70795e4e0bf 38
EvaKrolis 14:2c0bf576a0e7 39 //------------------------ Parameters for the EMG ----------------------------
tverouden 3:9c63fc5f157e 40
EvaKrolis 13:4ba8f63e6ff4 41 //EMG inputs
EvaKrolis 13:4ba8f63e6ff4 42 AnalogIn EMG0In(A0); //EMG input 0
EvaKrolis 13:4ba8f63e6ff4 43 AnalogIn EMG1In(A1); //EMG input 1
EvaKrolis 13:4ba8f63e6ff4 44
EvaKrolis 13:4ba8f63e6ff4 45 //Timers
EvaKrolis 13:4ba8f63e6ff4 46 Ticker ReadUseEMG0_timer; //Timer to read, filter and use the EMG
EvaKrolis 13:4ba8f63e6ff4 47 Ticker EMGCalibration0_timer; //Timer for the calibration of the EMG
EvaKrolis 13:4ba8f63e6ff4 48 Ticker FindMax0_timer; //Timer for finding the max muscle
EvaKrolis 13:4ba8f63e6ff4 49 Ticker ReadUseEMG1_timer; //Timer to read, filter and use the EMG
EvaKrolis 13:4ba8f63e6ff4 50 Ticker EMGCalibration1_timer; //Timer for the calibration of the EMG
EvaKrolis 13:4ba8f63e6ff4 51 Ticker FindMax1_timer; //Timer for finding the max muscle
EvaKrolis 13:4ba8f63e6ff4 52 Ticker SwitchState_timer; //Timer to switch from the Calibration to the working mode
EvaKrolis 13:4ba8f63e6ff4 53
EvaKrolis 13:4ba8f63e6ff4 54 //Constants
EvaKrolis 13:4ba8f63e6ff4 55 const int Length = 10000; //Length of the array for the calibration
EvaKrolis 13:4ba8f63e6ff4 56 const int Parts = 50; //Mean average filter over 50 values
EvaKrolis 13:4ba8f63e6ff4 57
EvaKrolis 13:4ba8f63e6ff4 58 //Parameters for the first EMG signal
EvaKrolis 13:4ba8f63e6ff4 59 float EMG0; //float for EMG input
EvaKrolis 13:4ba8f63e6ff4 60 float EMG0filt; //float for filtered EMG
EvaKrolis 13:4ba8f63e6ff4 61 float EMG0filtArray[Parts]; //Array for the filtered array
EvaKrolis 13:4ba8f63e6ff4 62 float EMG0Average; //float for the value after Moving Average Filter
EvaKrolis 13:4ba8f63e6ff4 63 float Sum0 = 0; //Sum0 for the moving average filter
EvaKrolis 13:4ba8f63e6ff4 64 float EMG0Calibrate[Length]; //Array for the calibration
EvaKrolis 13:4ba8f63e6ff4 65 int ReadCal0 = 0; //Integer to read over the calibration array
EvaKrolis 13:4ba8f63e6ff4 66 float MaxValue0 = 0; //float to save the max muscle
EvaKrolis 13:4ba8f63e6ff4 67 float Threshold0 = 0; //Threshold for the first EMG signal
EvaKrolis 13:4ba8f63e6ff4 68
EvaKrolis 13:4ba8f63e6ff4 69 //Parameters for the second EMG signal
EvaKrolis 13:4ba8f63e6ff4 70 float EMG1; //float for EMG input
EvaKrolis 13:4ba8f63e6ff4 71 float EMG1filt; //float for filtered EMG
EvaKrolis 13:4ba8f63e6ff4 72 float EMG1filtArray[Parts]; //Array for the filtered array
EvaKrolis 13:4ba8f63e6ff4 73 float EMG1Average; //float for the value after Moving Average Filter
EvaKrolis 13:4ba8f63e6ff4 74 float Sum1 = 0; //Sum0 for the moving average filter
EvaKrolis 13:4ba8f63e6ff4 75 float EMG1Calibrate[Length]; //Array for the calibration
EvaKrolis 13:4ba8f63e6ff4 76 int ReadCal1 = 0; //Integer to read over the calibration array
EvaKrolis 13:4ba8f63e6ff4 77 float MaxValue1 = 0; //float to save the max muscle
EvaKrolis 13:4ba8f63e6ff4 78 float Threshold1 = 0; //Threshold for the second EMG signal
EvaKrolis 13:4ba8f63e6ff4 79
EvaKrolis 13:4ba8f63e6ff4 80 //Filter variables
EvaKrolis 13:4ba8f63e6ff4 81 BiQuad Notch50_0(0.7887,0,0.7887,0,0.5774); //Make Notch filter around 50 Hz
EvaKrolis 13:4ba8f63e6ff4 82 BiQuad Notch50_1(0.7887,0,0.7887,0,0.5774); //Make Notch filter around 50 Hz
EvaKrolis 13:4ba8f63e6ff4 83 BiQuad High0(0.8006,-1.6012,0.8006,-1.561,0.6414); //Make high-pass filter
EvaKrolis 13:4ba8f63e6ff4 84 BiQuad High1(0.8006,-1.6012,0.8006,-1.561,0.6414); //Make high-pass filter
EvaKrolis 13:4ba8f63e6ff4 85 BiQuadChain filter0; //Make chain of filters for the first EMG signal
EvaKrolis 13:4ba8f63e6ff4 86 BiQuadChain filter1; //Make chain of filters for the second EMG signal
EvaKrolis 13:4ba8f63e6ff4 87
EvaKrolis 13:4ba8f63e6ff4 88 //Bool for movement
EvaKrolis 13:4ba8f63e6ff4 89 bool xMove = false; //Bool for the x-movement
EvaKrolis 13:4ba8f63e6ff4 90 bool yMove = false; //Bool for the y-movement
tverouden 3:9c63fc5f157e 91
EvaKrolis 14:2c0bf576a0e7 92 // -------------------- Parameters for the kinematics -------------------------
EvaKrolis 14:2c0bf576a0e7 93
EvaKrolis 14:2c0bf576a0e7 94 //Constants
EvaKrolis 14:2c0bf576a0e7 95 const double ll = 230; //Length of the lower arm
EvaKrolis 14:2c0bf576a0e7 96 const double lu = 198; //Length of the upper arm
EvaKrolis 14:2c0bf576a0e7 97 const double lb = 50; //Length of the part between the upper arms
EvaKrolis 14:2c0bf576a0e7 98 const double le = 79; //Length of the end-effector beam
EvaKrolis 14:2c0bf576a0e7 99 const double xbase = 450-lb; //Length between the motors
EvaKrolis 14:2c0bf576a0e7 100
EvaKrolis 14:2c0bf576a0e7 101 //General parameters
EvaKrolis 14:2c0bf576a0e7 102 double theta1 = PI*0.49; //Angle of the left motor
EvaKrolis 14:2c0bf576a0e7 103 double theta4 = PI*0.49; //Angle of the right motor
EvaKrolis 14:2c0bf576a0e7 104 double thetaflip = 0; //Angle of the flipping motor
EvaKrolis 14:2c0bf576a0e7 105 double prefx; //Desired x velocity
EvaKrolis 14:2c0bf576a0e7 106 double prefy; //Desired y velocity "
EvaKrolis 14:2c0bf576a0e7 107 double deltat = 0.01; //Time step (dependent on sample frequency)
EvaKrolis 14:2c0bf576a0e7 108
EvaKrolis 14:2c0bf576a0e7 109 //Kinematics parameters for x
EvaKrolis 14:2c0bf576a0e7 110 double xendsum;
EvaKrolis 14:2c0bf576a0e7 111 double xendsqrt1;
EvaKrolis 14:2c0bf576a0e7 112 double xendsqrt2;
EvaKrolis 14:2c0bf576a0e7 113 double xend;
EvaKrolis 14:2c0bf576a0e7 114 double jacobiana;
EvaKrolis 14:2c0bf576a0e7 115 double jacobianc;
EvaKrolis 14:2c0bf576a0e7 116
EvaKrolis 14:2c0bf576a0e7 117 //Kinematics parameters for y
EvaKrolis 14:2c0bf576a0e7 118 double yendsum;
EvaKrolis 14:2c0bf576a0e7 119 double yendsqrt1;
EvaKrolis 14:2c0bf576a0e7 120 double yendsqrt2;
EvaKrolis 14:2c0bf576a0e7 121 double yend;
EvaKrolis 14:2c0bf576a0e7 122 double jacobianb;
EvaKrolis 14:2c0bf576a0e7 123 double jacobiand;
EvaKrolis 14:2c0bf576a0e7 124
EvaKrolis 14:2c0bf576a0e7 125 //Tickers
EvaKrolis 14:2c0bf576a0e7 126 Ticker kin; //Timer for calculating x,y,theta1,theta4
EvaKrolis 14:2c0bf576a0e7 127 Ticker simulateval; //Timer that prints the values for x,y, and angles
EvaKrolis 14:2c0bf576a0e7 128
EvaKrolis 14:2c0bf576a0e7 129 // ---------------------- Parameters for the motors ---------------------------
EvaKrolis 14:2c0bf576a0e7 130
tverouden 4:5ce2c8864908 131 // ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡ FUNCTIONS ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡
tverouden 6:f32352bc5078 132 // ============================ GENERAL FUNCTIONS =============================
tverouden 6:f32352bc5078 133 void stopProgram(void)
tverouden 6:f32352bc5078 134 {
tverouden 6:f32352bc5078 135 // Error message
tverouden 6:f32352bc5078 136 pc.printf("[ERROR] emergency button pressed\r\n");
tverouden 6:f32352bc5078 137 currentState = failing; // change to state
tverouden 6:f32352bc5078 138 changeState = true; // next loop, switch states
tverouden 6:f32352bc5078 139 }
tverouden 8:8cef1050ebd9 140
tverouden 8:8cef1050ebd9 141 void blinkLED(DigitalOut led) // blinkTimer.attach(&blinkLED,0.5) aanroepen bij initialisation, bij verlaten state:
tverouden 8:8cef1050ebd9 142 { // blinkTimer.detach
tverouden 8:8cef1050ebd9 143 led =! led; // toggle LED
tverouden 10:584b7d2c2d63 144 }
tverouden 4:5ce2c8864908 145
tverouden 4:5ce2c8864908 146 // ============================= EMG FUNCTIONS ===============================
tverouden 4:5ce2c8864908 147
EvaKrolis 13:4ba8f63e6ff4 148 //Function to read and filter the EMG
EvaKrolis 13:4ba8f63e6ff4 149 void ReadUseEMG0(){
EvaKrolis 13:4ba8f63e6ff4 150 for(int i = Parts ; i > 0 ; i--){ //Make a first in, first out array
EvaKrolis 13:4ba8f63e6ff4 151 EMG0filtArray[i] = EMG0filtArray[i-1]; //Every value moves one up
EvaKrolis 13:4ba8f63e6ff4 152 }
EvaKrolis 13:4ba8f63e6ff4 153
EvaKrolis 13:4ba8f63e6ff4 154 Sum0 = 0;
EvaKrolis 13:4ba8f63e6ff4 155 EMG0 = EMG0In; //Save EMG input in float
EvaKrolis 13:4ba8f63e6ff4 156 EMG0filt = filter0.step(EMG0); //Filter the signal
EvaKrolis 13:4ba8f63e6ff4 157 EMG0filt = abs(EMG0filt); //Take the absolute value
EvaKrolis 13:4ba8f63e6ff4 158 EMG0filtArray[0] = EMG0filt; //Save the filtered signal on the first place in the array
EvaKrolis 13:4ba8f63e6ff4 159
EvaKrolis 13:4ba8f63e6ff4 160 for(int i = 0 ; i < Parts ; i++){ //Moving Average filter
EvaKrolis 13:4ba8f63e6ff4 161 Sum0 += EMG0filtArray[i]; //Sum the new value and the previous 49
EvaKrolis 13:4ba8f63e6ff4 162 }
EvaKrolis 13:4ba8f63e6ff4 163 EMG0Average = (float)Sum0/Parts; //Divide the sum by 50
EvaKrolis 13:4ba8f63e6ff4 164
EvaKrolis 13:4ba8f63e6ff4 165 if (EMG0Average > Threshold0){ //If the value is higher than the threshold value
EvaKrolis 13:4ba8f63e6ff4 166 xMove = true; //Set movement to true
EvaKrolis 13:4ba8f63e6ff4 167 }
EvaKrolis 13:4ba8f63e6ff4 168 else{
EvaKrolis 13:4ba8f63e6ff4 169 xMove = false; //Otherwise set movement to false
EvaKrolis 13:4ba8f63e6ff4 170 }
EvaKrolis 13:4ba8f63e6ff4 171 }
EvaKrolis 13:4ba8f63e6ff4 172
EvaKrolis 13:4ba8f63e6ff4 173 //Function to read and filter the EMG
EvaKrolis 13:4ba8f63e6ff4 174 void ReadUseEMG1(){
EvaKrolis 13:4ba8f63e6ff4 175 for(int i = Parts ; i > 0 ; i--){ //Make a first in, first out array
EvaKrolis 13:4ba8f63e6ff4 176 EMG1filtArray[i] = EMG1filtArray[i-1]; //Every value moves one up
EvaKrolis 13:4ba8f63e6ff4 177 }
EvaKrolis 13:4ba8f63e6ff4 178
EvaKrolis 13:4ba8f63e6ff4 179 Sum1 = 0;
EvaKrolis 13:4ba8f63e6ff4 180 EMG1 = EMG1In; //Save EMG input in float
EvaKrolis 13:4ba8f63e6ff4 181 EMG1filt = filter1.step(EMG1); //Filter the signal
EvaKrolis 13:4ba8f63e6ff4 182 EMG1filt = abs(EMG1filt); //Take the absolute value
EvaKrolis 13:4ba8f63e6ff4 183 EMG1filtArray[0] = EMG1filt; //Save the filtered signal on the first place in the array
EvaKrolis 13:4ba8f63e6ff4 184
EvaKrolis 13:4ba8f63e6ff4 185 for(int i = 0 ; i < Parts ; i++){ //Moving Average filter
EvaKrolis 13:4ba8f63e6ff4 186 Sum1 += EMG1filtArray[i]; //Sum the new value and the previous 49
EvaKrolis 13:4ba8f63e6ff4 187 }
EvaKrolis 13:4ba8f63e6ff4 188 EMG1Average = (float)Sum1/Parts; //Divide the sum by 50
EvaKrolis 13:4ba8f63e6ff4 189
EvaKrolis 13:4ba8f63e6ff4 190 if (EMG1Average > Threshold1){ //If the value is higher than the threshold value
EvaKrolis 13:4ba8f63e6ff4 191 yMove = true; //Set y movement to true
EvaKrolis 13:4ba8f63e6ff4 192 }
EvaKrolis 13:4ba8f63e6ff4 193 else{
EvaKrolis 13:4ba8f63e6ff4 194 yMove = false; //Otherwise set y movement to false
EvaKrolis 13:4ba8f63e6ff4 195 }
EvaKrolis 13:4ba8f63e6ff4 196 }
EvaKrolis 13:4ba8f63e6ff4 197
EvaKrolis 13:4ba8f63e6ff4 198 //Function to make an array during the calibration
EvaKrolis 13:4ba8f63e6ff4 199 void CalibrateEMG0(){
EvaKrolis 13:4ba8f63e6ff4 200 for(int i = Parts ; i > 0 ; i--){ //Make a first in, first out array
EvaKrolis 13:4ba8f63e6ff4 201 EMG0filtArray[i] = EMG0filtArray[i-1]; //Every value moves one up
EvaKrolis 13:4ba8f63e6ff4 202 }
EvaKrolis 13:4ba8f63e6ff4 203
EvaKrolis 13:4ba8f63e6ff4 204 Sum0 = 0;
EvaKrolis 13:4ba8f63e6ff4 205 EMG0 = EMG0In; //Save EMG input in float
EvaKrolis 13:4ba8f63e6ff4 206 EMG0filt = filter0.step(EMG0); //Filter the signal
EvaKrolis 13:4ba8f63e6ff4 207 EMG0filt = abs(EMG0filt); //Take the absolute value
EvaKrolis 13:4ba8f63e6ff4 208 EMG0filtArray[0] = EMG0filt; //Save the filtered signal on the first place in the array
EvaKrolis 13:4ba8f63e6ff4 209
EvaKrolis 13:4ba8f63e6ff4 210 for(int i = 0 ; i < Parts ; i++){ //Moving Average filter
EvaKrolis 13:4ba8f63e6ff4 211 Sum0 += EMG0filtArray[i]; //Sum the new value and the previous 49
EvaKrolis 13:4ba8f63e6ff4 212 }
EvaKrolis 13:4ba8f63e6ff4 213 EMG0Calibrate[ReadCal0] = (float)Sum0/Parts; //Divide the sum by 50
EvaKrolis 13:4ba8f63e6ff4 214
EvaKrolis 13:4ba8f63e6ff4 215 ReadCal0++;
EvaKrolis 13:4ba8f63e6ff4 216 }
EvaKrolis 13:4ba8f63e6ff4 217
EvaKrolis 13:4ba8f63e6ff4 218 //Function to make an array during the calibration
EvaKrolis 13:4ba8f63e6ff4 219 void CalibrateEMG1(){
EvaKrolis 13:4ba8f63e6ff4 220 for(int i = Parts ; i > 0 ; i--){ //Make a first in, first out array
EvaKrolis 13:4ba8f63e6ff4 221 EMG1filtArray[i] = EMG1filtArray[i-1]; //Every value moves one up
EvaKrolis 13:4ba8f63e6ff4 222 }
EvaKrolis 13:4ba8f63e6ff4 223
EvaKrolis 13:4ba8f63e6ff4 224 Sum1 = 0;
EvaKrolis 13:4ba8f63e6ff4 225 EMG1 = EMG1In; //Save EMG input in float
EvaKrolis 13:4ba8f63e6ff4 226 EMG1filt = filter1.step(EMG1); //Filter the signal
EvaKrolis 13:4ba8f63e6ff4 227 EMG1filt = abs(EMG1filt); //Take the absolute value
EvaKrolis 13:4ba8f63e6ff4 228 EMG1filtArray[0] = EMG1filt; //Save the filtered signal on the first place in the array
EvaKrolis 13:4ba8f63e6ff4 229
EvaKrolis 13:4ba8f63e6ff4 230 for(int i = 0 ; i < Parts ; i++){ //Moving Average filter
EvaKrolis 13:4ba8f63e6ff4 231 Sum1 += EMG1filtArray[i]; //Sum the new value and the previous 49
EvaKrolis 13:4ba8f63e6ff4 232 }
EvaKrolis 13:4ba8f63e6ff4 233 EMG1Calibrate[ReadCal1] = (float)Sum1/Parts; //Divide the sum by 50
EvaKrolis 13:4ba8f63e6ff4 234
EvaKrolis 13:4ba8f63e6ff4 235 ReadCal1++;
EvaKrolis 13:4ba8f63e6ff4 236 }
EvaKrolis 13:4ba8f63e6ff4 237
EvaKrolis 13:4ba8f63e6ff4 238 //Function to find the max value during the calibration
EvaKrolis 13:4ba8f63e6ff4 239 void FindMax0(){
EvaKrolis 13:4ba8f63e6ff4 240 MaxValue0 = *max_element(EMG0Calibrate+500,EMG0Calibrate+Length); //Find max value, but discard the first 100 values
EvaKrolis 13:4ba8f63e6ff4 241 Threshold0 = 0.30f*MaxValue0; //The threshold is a percentage of the max value
EvaKrolis 13:4ba8f63e6ff4 242 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
EvaKrolis 13:4ba8f63e6ff4 243 FindMax0_timer.detach(); //Detach the timer, so you only use this once
EvaKrolis 13:4ba8f63e6ff4 244 }
EvaKrolis 13:4ba8f63e6ff4 245
EvaKrolis 13:4ba8f63e6ff4 246 //Function to find the max value during the calibration
EvaKrolis 13:4ba8f63e6ff4 247 void FindMax1(){
EvaKrolis 13:4ba8f63e6ff4 248 MaxValue1 = *max_element(EMG1Calibrate+500,EMG1Calibrate+Length); //Find max value, but discard the first 100 values
EvaKrolis 13:4ba8f63e6ff4 249 Threshold1 = 0.30f*MaxValue1; //The threshold is a percentage of the max value
EvaKrolis 13:4ba8f63e6ff4 250 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
EvaKrolis 13:4ba8f63e6ff4 251 FindMax1_timer.detach(); //Detach the timer, so you only use this once
EvaKrolis 13:4ba8f63e6ff4 252 }
tverouden 4:5ce2c8864908 253
tverouden 12:323ac4e27a0d 254 // ========================= KINEMATICS FUNCTIONS ============================
tverouden 12:323ac4e27a0d 255
EvaKrolis 14:2c0bf576a0e7 256 // Function to calculate the inverse kinematics
EvaKrolis 14:2c0bf576a0e7 257 void inverse(double prex, double prey)
EvaKrolis 14:2c0bf576a0e7 258 {
EvaKrolis 14:2c0bf576a0e7 259 /*
EvaKrolis 14:2c0bf576a0e7 260 qn = qn-1 + (jacobian^-1)*dPref/dt *deltaT
EvaKrolis 14:2c0bf576a0e7 261 ofwel
EvaKrolis 14:2c0bf576a0e7 262 thetai+1 = thetai +(jacobian)^-1*vector(deltaX, DeltaY)
EvaKrolis 14:2c0bf576a0e7 263 waar Pref = emg signaal
EvaKrolis 14:2c0bf576a0e7 264 */ //achtergrondinfo hierboven...
EvaKrolis 14:2c0bf576a0e7 265 //
EvaKrolis 14:2c0bf576a0e7 266
EvaKrolis 14:2c0bf576a0e7 267 theta1 += (prefx*jacobiana+jacobianb*prey)*deltat; //theta 1 is zichzelf plus wat hier staat (is kinematics)
EvaKrolis 14:2c0bf576a0e7 268 theta4 += (prefx*jacobianc+jacobiand*prey)*deltat;//" "
EvaKrolis 14:2c0bf576a0e7 269 //Hier worden xend en yend doorgerekend, die formules kan je overslaan
EvaKrolis 14:2c0bf576a0e7 270 xendsum = lb + xbase +ll*(cos(theta1) - cos(theta4));
EvaKrolis 14:2c0bf576a0e7 271 xendsqrt1 = 2*sqrt(-xbase*xbase/4 + lu*lu + ll*(xbase*(cos(theta1)+cos(theta4))/2) -ll*(1+ cos(theta1+theta4)))*(-sin(theta1)+sin(theta4));
EvaKrolis 14:2c0bf576a0e7 272 xendsqrt2 = sqrt(pow((-xbase/ll+cos(theta1)+cos(theta4)),2)+ pow(sin(theta1) - sin(theta4),2));
EvaKrolis 14:2c0bf576a0e7 273 xend = (xendsum + xendsqrt1/xendsqrt2)/2;
EvaKrolis 14:2c0bf576a0e7 274 //hieronder rekenen we yendeffector door;
EvaKrolis 14:2c0bf576a0e7 275 yendsum = -le + ll/2*(sin(theta1)+sin(theta4));
EvaKrolis 14:2c0bf576a0e7 276 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))));
EvaKrolis 14:2c0bf576a0e7 277 yendsqrt2 = sqrt(pow((-xbase/ll + cos(theta1)+ cos(theta4)),2)+ pow((sin(theta1)-sin(theta4)),2));
EvaKrolis 14:2c0bf576a0e7 278 yend = (yendsum + yendsqrt1/yendsqrt2);
EvaKrolis 14:2c0bf576a0e7 279
EvaKrolis 14:2c0bf576a0e7 280 }
EvaKrolis 14:2c0bf576a0e7 281
EvaKrolis 14:2c0bf576a0e7 282 // Function for the Jacobian
EvaKrolis 14:2c0bf576a0e7 283 void kinematics()
EvaKrolis 14:2c0bf576a0e7 284 {
EvaKrolis 14:2c0bf576a0e7 285
EvaKrolis 14:2c0bf576a0e7 286 jacobiana = (500*(-(((-(xbase/ll) + cos(theta1) + cos(theta4))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(theta4)) - ll*(1 + cos(theta1 + theta4))))/2.))/
EvaKrolis 14:2c0bf576a0e7 287 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2))) - (ll*(sin(theta1) + sin(theta4)))/2. +
EvaKrolis 14:2c0bf576a0e7 288 ((-xbase + ll*(cos(theta1) + cos(0.002 + theta4)))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(0.002 + theta4)) - ll*(1 + cos(0.002 + theta1 + theta4))))/2.))/
EvaKrolis 14:2c0bf576a0e7 289 (ll*sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.002 + theta4),2) + pow(sin(theta1) - sin(0.002 + theta4),2))) + (ll*(sin(theta1) + sin(0.002 + theta4)))/2.))/
EvaKrolis 14:2c0bf576a0e7 290 (-125000*(-(((-(xbase/ll) + cos(theta1) + cos(theta4))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(theta4)) - ll*(1 + cos(theta1 + theta4))))/2.))/
EvaKrolis 14:2c0bf576a0e7 291 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2))) +
EvaKrolis 14:2c0bf576a0e7 292 ((-xbase + ll*(cos(0.002 + theta1) + cos(theta4)))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(0.002 + theta1) + cos(theta4)) - ll*(1 + cos(0.002 + theta1 + theta4))))/2.))/
EvaKrolis 14:2c0bf576a0e7 293 (ll*sqrt(pow(-(xbase/ll) + cos(0.002 + theta1) + cos(theta4),2) + pow(sin(0.002 + theta1) - sin(theta4),2))) - (ll*(sin(theta1) + sin(theta4)))/2. + (ll*(sin(0.002 + theta1) + sin(theta4)))/2.)*
EvaKrolis 14:2c0bf576a0e7 294 (ll*(-cos(theta1) + cos(theta4)) + ll*(cos(theta1) - cos(0.002 + theta4)) + (sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(theta1) + 2*xbase*ll*cos(theta4) - 2*pow(ll,2)*cos(theta1 + theta4))*
EvaKrolis 14:2c0bf576a0e7 295 (sin(theta1) - sin(theta4)))/sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2)) +
EvaKrolis 14:2c0bf576a0e7 296 (sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(theta1) + 2*xbase*ll*cos(0.002 + theta4) - 2*pow(ll,2)*cos(0.002 + theta1 + theta4))*(-sin(theta1) + sin(0.002 + theta4)))/
EvaKrolis 14:2c0bf576a0e7 297 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.002 + theta4),2) + pow(sin(theta1) - sin(0.002 + theta4),2))) +
EvaKrolis 14:2c0bf576a0e7 298 125000*(ll*(cos(0.002 + theta1) - cos(theta4)) + ll*(-cos(theta1) + cos(theta4)) + (sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(theta1) + 2*xbase*ll*cos(theta4) - 2*pow(ll,2)*cos(theta1 + theta4))*
EvaKrolis 14:2c0bf576a0e7 299 (sin(theta1) - sin(theta4)))/sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2)) +
EvaKrolis 14:2c0bf576a0e7 300 (sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(0.002 + theta1) + 2*xbase*ll*cos(theta4) - 2*pow(ll,2)*cos(0.002 + theta1 + theta4))*(-sin(0.002 + theta1) + sin(theta4)))/
EvaKrolis 14:2c0bf576a0e7 301 sqrt(pow(-(xbase/ll) + cos(0.002 + theta1) + cos(theta4),2) + pow(sin(0.002 + theta1) - sin(theta4),2)))*
EvaKrolis 14:2c0bf576a0e7 302 (-(((-(xbase/ll) + cos(theta1) + cos(theta4))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(theta4)) - ll*(1 + cos(theta1 + theta4))))/2.))/
EvaKrolis 14:2c0bf576a0e7 303 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2))) - (ll*(sin(theta1) + sin(theta4)))/2. +
EvaKrolis 14:2c0bf576a0e7 304 ((-xbase + ll*(cos(theta1) + cos(0.002 + theta4)))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(0.002 + theta4)) - ll*(1 + cos(0.002 + theta1 + theta4))))/2.))/
EvaKrolis 14:2c0bf576a0e7 305 (ll*sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.002 + theta4),2) + pow(sin(theta1) - sin(0.002 + theta4),2))) + (ll*(sin(theta1) + sin(0.002 + theta4)))/2.));
EvaKrolis 14:2c0bf576a0e7 306
EvaKrolis 14:2c0bf576a0e7 307 jacobianb = (-250*(ll*(-cos(theta1) + cos(theta4)) + ll*(cos(theta1) - cos(0.002 + theta4)) + (sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(theta1) + 2*xbase*ll*cos(theta4) - 2*pow(ll,2)*cos(theta1 + theta4))*
EvaKrolis 14:2c0bf576a0e7 308 (sin(theta1) - sin(theta4)))/sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2)) +
EvaKrolis 14:2c0bf576a0e7 309 (sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(theta1) + 2*xbase*ll*cos(0.002 + theta4) - 2*pow(ll,2)*cos(0.002 + theta1 + theta4))*(-sin(theta1) + sin(0.002 + theta4)))/
EvaKrolis 14:2c0bf576a0e7 310 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.002 + theta4),2) + pow(sin(theta1) - sin(0.002 + theta4),2))))/
EvaKrolis 14:2c0bf576a0e7 311 (-125000*(-(((-(xbase/ll) + cos(theta1) + cos(theta4))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(theta4)) - ll*(1 + cos(theta1 + theta4))))/2.))/
EvaKrolis 14:2c0bf576a0e7 312 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2))) +
EvaKrolis 14:2c0bf576a0e7 313 ((-xbase + ll*(cos(0.002 + theta1) + cos(theta4)))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(0.002 + theta1) + cos(theta4)) - ll*(1 + cos(0.002 + theta1 + theta4))))/2.))/
EvaKrolis 14:2c0bf576a0e7 314 (ll*sqrt(pow(-(xbase/ll) + cos(0.002 + theta1) + cos(theta4),2) + pow(sin(0.002 + theta1) - sin(theta4),2))) - (ll*(sin(theta1) + sin(theta4)))/2. + (ll*(sin(0.002 + theta1) + sin(theta4)))/2.)*
EvaKrolis 14:2c0bf576a0e7 315 (ll*(-cos(theta1) + cos(theta4)) + ll*(cos(theta1) - cos(0.002 + theta4)) + (sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(theta1) + 2*xbase*ll*cos(theta4) - 2*pow(ll,2)*cos(theta1 + theta4))*
EvaKrolis 14:2c0bf576a0e7 316 (sin(theta1) - sin(theta4)))/sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2)) +
EvaKrolis 14:2c0bf576a0e7 317 (sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(theta1) + 2*xbase*ll*cos(0.002 + theta4) - 2*pow(ll,2)*cos(0.002 + theta1 + theta4))*(-sin(theta1) + sin(0.002 + theta4)))/
EvaKrolis 14:2c0bf576a0e7 318 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.002 + theta4),2) + pow(sin(theta1) - sin(0.002 + theta4),2))) +
EvaKrolis 14:2c0bf576a0e7 319 125000*(ll*(cos(0.002 + theta1) - cos(theta4)) + ll*(-cos(theta1) + cos(theta4)) + (sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(theta1) + 2*xbase*ll*cos(theta4) - 2*pow(ll,2)*cos(theta1 + theta4))*
EvaKrolis 14:2c0bf576a0e7 320 (sin(theta1) - sin(theta4)))/sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2)) +
EvaKrolis 14:2c0bf576a0e7 321 (sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(0.002 + theta1) + 2*xbase*ll*cos(theta4) - 2*pow(ll,2)*cos(0.002 + theta1 + theta4))*(-sin(0.002 + theta1) + sin(theta4)))/
EvaKrolis 14:2c0bf576a0e7 322 sqrt(pow(-(xbase/ll) + cos(0.002 + theta1) + cos(theta4),2) + pow(sin(0.002 + theta1) - sin(theta4),2)))*
EvaKrolis 14:2c0bf576a0e7 323 (-(((-(xbase/ll) + cos(theta1) + cos(theta4))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(theta4)) - ll*(1 + cos(theta1 + theta4))))/2.))/
EvaKrolis 14:2c0bf576a0e7 324 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2))) - (ll*(sin(theta1) + sin(theta4)))/2. +
EvaKrolis 14:2c0bf576a0e7 325 ((-xbase + ll*(cos(theta1) + cos(0.002 + theta4)))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(0.002 + theta4)) - ll*(1 + cos(0.002 + theta1 + theta4))))/2.))/
EvaKrolis 14:2c0bf576a0e7 326 (ll*sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.002 + theta4),2) + pow(sin(theta1) - sin(0.002 + theta4),2))) + (ll*(sin(theta1) + sin(0.002 + theta4)))/2.));
EvaKrolis 14:2c0bf576a0e7 327
EvaKrolis 14:2c0bf576a0e7 328 jacobianc = (-500*(-(((-(xbase/ll) + cos(theta1) + cos(theta4))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(theta4)) - ll*(1 + cos(theta1 + theta4))))/2.))/
EvaKrolis 14:2c0bf576a0e7 329 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2))) +
EvaKrolis 14:2c0bf576a0e7 330 ((-xbase + ll*(cos(0.002 + theta1) + cos(theta4)))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(0.002 + theta1) + cos(theta4)) - ll*(1 + cos(0.002 + theta1 + theta4))))/2.))/
EvaKrolis 14:2c0bf576a0e7 331 (ll*sqrt(pow(-(xbase/ll) + cos(0.002 + theta1) + cos(theta4),2) + pow(sin(0.002 + theta1) - sin(theta4),2))) - (ll*(sin(theta1) + sin(theta4)))/2. + (ll*(sin(0.002 + theta1) + sin(theta4)))/2.))/
EvaKrolis 14:2c0bf576a0e7 332 (-125000*(-(((-(xbase/ll) + cos(theta1) + cos(theta4))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(theta4)) - ll*(1 + cos(theta1 + theta4))))/2.))/
EvaKrolis 14:2c0bf576a0e7 333 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2))) +
EvaKrolis 14:2c0bf576a0e7 334 ((-xbase + ll*(cos(0.002 + theta1) + cos(theta4)))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(0.002 + theta1) + cos(theta4)) - ll*(1 + cos(0.002 + theta1 + theta4))))/2.))/
EvaKrolis 14:2c0bf576a0e7 335 (ll*sqrt(pow(-(xbase/ll) + cos(0.002 + theta1) + cos(theta4),2) + pow(sin(0.002 + theta1) - sin(theta4),2))) - (ll*(sin(theta1) + sin(theta4)))/2. + (ll*(sin(0.002 + theta1) + sin(theta4)))/2.)*
EvaKrolis 14:2c0bf576a0e7 336 (ll*(-cos(theta1) + cos(theta4)) + ll*(cos(theta1) - cos(0.002 + theta4)) + (sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(theta1) + 2*xbase*ll*cos(theta4) - 2*pow(ll,2)*cos(theta1 + theta4))*
EvaKrolis 14:2c0bf576a0e7 337 (sin(theta1) - sin(theta4)))/sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2)) +
EvaKrolis 14:2c0bf576a0e7 338 (sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(theta1) + 2*xbase*ll*cos(0.002 + theta4) - 2*pow(ll,2)*cos(0.002 + theta1 + theta4))*(-sin(theta1) + sin(0.002 + theta4)))/
EvaKrolis 14:2c0bf576a0e7 339 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.002 + theta4),2) + pow(sin(theta1) - sin(0.002 + theta4),2))) +
EvaKrolis 14:2c0bf576a0e7 340 125000*(ll*(cos(0.002 + theta1) - cos(theta4)) + ll*(-cos(theta1) + cos(theta4)) + (sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(theta1) + 2*xbase*ll*cos(theta4) - 2*pow(ll,2)*cos(theta1 + theta4))*
EvaKrolis 14:2c0bf576a0e7 341 (sin(theta1) - sin(theta4)))/sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2)) +
EvaKrolis 14:2c0bf576a0e7 342 (sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(0.002 + theta1) + 2*xbase*ll*cos(theta4) - 2*pow(ll,2)*cos(0.002 + theta1 + theta4))*(-sin(0.002 + theta1) + sin(theta4)))/
EvaKrolis 14:2c0bf576a0e7 343 sqrt(pow(-(xbase/ll) + cos(0.002 + theta1) + cos(theta4),2) + pow(sin(0.002 + theta1) - sin(theta4),2)))*
EvaKrolis 14:2c0bf576a0e7 344 (-(((-(xbase/ll) + cos(theta1) + cos(theta4))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(theta4)) - ll*(1 + cos(theta1 + theta4))))/2.))/
EvaKrolis 14:2c0bf576a0e7 345 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2))) - (ll*(sin(theta1) + sin(theta4)))/2. +
EvaKrolis 14:2c0bf576a0e7 346 ((-xbase + ll*(cos(theta1) + cos(0.002 + theta4)))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(0.002 + theta4)) - ll*(1 + cos(0.002 + theta1 + theta4))))/2.))/
EvaKrolis 14:2c0bf576a0e7 347 (ll*sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.002 + theta4),2) + pow(sin(theta1) - sin(0.002 + theta4),2))) + (ll*(sin(theta1) + sin(0.002 + theta4)))/2.));
EvaKrolis 14:2c0bf576a0e7 348
EvaKrolis 14:2c0bf576a0e7 349 jacobiand = (250*(ll*(cos(0.002 + theta1) - cos(theta4)) + ll*(-cos(theta1) + cos(theta4)) + (sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(theta1) + 2*xbase*ll*cos(theta4) - 2*pow(ll,2)*cos(theta1 + theta4))*
EvaKrolis 14:2c0bf576a0e7 350 (sin(theta1) - sin(theta4)))/sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2)) +
EvaKrolis 14:2c0bf576a0e7 351 (sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(0.002 + theta1) + 2*xbase*ll*cos(theta4) - 2*pow(ll,2)*cos(0.002 + theta1 + theta4))*(-sin(0.002 + theta1) + sin(theta4)))/
EvaKrolis 14:2c0bf576a0e7 352 sqrt(pow(-(xbase/ll) + cos(0.002 + theta1) + cos(theta4),2) + pow(sin(0.002 + theta1) - sin(theta4),2))))/
EvaKrolis 14:2c0bf576a0e7 353 (-125000*(-(((-(xbase/ll) + cos(theta1) + cos(theta4))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(theta4)) - ll*(1 + cos(theta1 + theta4))))/2.))/
EvaKrolis 14:2c0bf576a0e7 354 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2))) +
EvaKrolis 14:2c0bf576a0e7 355 ((-xbase + ll*(cos(0.002 + theta1) + cos(theta4)))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(0.002 + theta1) + cos(theta4)) - ll*(1 + cos(0.002 + theta1 + theta4))))/2.))/
EvaKrolis 14:2c0bf576a0e7 356 (ll*sqrt(pow(-(xbase/ll) + cos(0.002 + theta1) + cos(theta4),2) + pow(sin(0.002 + theta1) - sin(theta4),2))) - (ll*(sin(theta1) + sin(theta4)))/2. + (ll*(sin(0.002 + theta1) + sin(theta4)))/2.)*
EvaKrolis 14:2c0bf576a0e7 357 (ll*(-cos(theta1) + cos(theta4)) + ll*(cos(theta1) - cos(0.002 + theta4)) + (sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(theta1) + 2*xbase*ll*cos(theta4) - 2*pow(ll,2)*cos(theta1 + theta4))*
EvaKrolis 14:2c0bf576a0e7 358 (sin(theta1) - sin(theta4)))/sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2)) +
EvaKrolis 14:2c0bf576a0e7 359 (sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(theta1) + 2*xbase*ll*cos(0.002 + theta4) - 2*pow(ll,2)*cos(0.002 + theta1 + theta4))*(-sin(theta1) + sin(0.002 + theta4)))/
EvaKrolis 14:2c0bf576a0e7 360 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.002 + theta4),2) + pow(sin(theta1) - sin(0.002 + theta4),2))) +
EvaKrolis 14:2c0bf576a0e7 361 125000*(ll*(cos(0.002 + theta1) - cos(theta4)) + ll*(-cos(theta1) + cos(theta4)) + (sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(theta1) + 2*xbase*ll*cos(theta4) - 2*pow(ll,2)*cos(theta1 + theta4))*
EvaKrolis 14:2c0bf576a0e7 362 (sin(theta1) - sin(theta4)))/sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2)) +
EvaKrolis 14:2c0bf576a0e7 363 (sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(0.002 + theta1) + 2*xbase*ll*cos(theta4) - 2*pow(ll,2)*cos(0.002 + theta1 + theta4))*(-sin(0.002 + theta1) + sin(theta4)))/
EvaKrolis 14:2c0bf576a0e7 364 sqrt(pow(-(xbase/ll) + cos(0.002 + theta1) + cos(theta4),2) + pow(sin(0.002 + theta1) - sin(theta4),2)))*
EvaKrolis 14:2c0bf576a0e7 365 (-(((-(xbase/ll) + cos(theta1) + cos(theta4))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(theta4)) - ll*(1 + cos(theta1 + theta4))))/2.))/
EvaKrolis 14:2c0bf576a0e7 366 sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2))) - (ll*(sin(theta1) + sin(theta4)))/2. +
EvaKrolis 14:2c0bf576a0e7 367 ((-xbase + ll*(cos(theta1) + cos(0.002 + theta4)))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(0.002 + theta4)) - ll*(1 + cos(0.002 + theta1 + theta4))))/2.))/
EvaKrolis 14:2c0bf576a0e7 368 (ll*sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.002 + theta4),2) + pow(sin(theta1) - sin(0.002 + theta4),2))) + (ll*(sin(theta1) + sin(0.002 + theta4)))/2.));
EvaKrolis 14:2c0bf576a0e7 369
EvaKrolis 14:2c0bf576a0e7 370 prefx = 1*xMove; //If the EMG is true, the x will move with 1
EvaKrolis 14:2c0bf576a0e7 371 prefy = 1*yMove; //If the EMG is true, the y will move with 1
EvaKrolis 14:2c0bf576a0e7 372 inverse(prefx, prefy);
EvaKrolis 14:2c0bf576a0e7 373 }
tverouden 12:323ac4e27a0d 374
EvaKrolis 13:4ba8f63e6ff4 375 // ============================ MOTOR FUNCTIONS ==============================
EvaKrolis 13:4ba8f63e6ff4 376
EvaKrolis 13:4ba8f63e6ff4 377
tverouden 2:d70795e4e0bf 378 // ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡ STATE MACHINE ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡
tverouden 7:ef5966469621 379 void stateMachine(void)
tverouden 3:9c63fc5f157e 380 {
tverouden 5:04b26b2f536a 381 switch (currentState) { // determine which state Odin is in // Van een aantal if statements moeten de conditions nog bepaald worden, niet vergeten
tverouden 3:9c63fc5f157e 382
tverouden 4:5ce2c8864908 383 // ========================= MOTOR CALIBRATION MODE ==========================
tverouden 4:5ce2c8864908 384 case calibratingMotors:
tverouden 4:5ce2c8864908 385 // ------------------------- initialisation --------------------------
tverouden 4:5ce2c8864908 386 if (changeState) { // when entering the state
tverouden 4:5ce2c8864908 387 pc.printf("[MODE] calibrating motors...r\n");
tverouden 5:04b26b2f536a 388 // print current state
tverouden 4:5ce2c8864908 389 changeState = false; // stay in this state
tverouden 4:5ce2c8864908 390
tverouden 4:5ce2c8864908 391 // Actions when entering state
tverouden 4:5ce2c8864908 392 /* */
tverouden 4:5ce2c8864908 393
tverouden 4:5ce2c8864908 394 }
tverouden 4:5ce2c8864908 395 // ----------------------------- action ------------------------------
tverouden 5:04b26b2f536a 396 // Actions for each loop iteration
tverouden 5:04b26b2f536a 397 /* */
tverouden 4:5ce2c8864908 398
tverouden 4:5ce2c8864908 399 // --------------------------- transition ----------------------------
tverouden 4:5ce2c8864908 400 // Transition condition #1: after 3 sec in state && all motor // Als vóór het einde van de 3 seconden alle motoren al snelheid 0 hebben, zitten ze zo dus door te draaien terwijl dat niet kan
tverouden 4:5ce2c8864908 401 // velocities zero, start calibrating EMG-x // dat lijkt me niet de bedoeling
tverouden 7:ef5966469621 402 if (1) { // CONDITION
tverouden 5:04b26b2f536a 403 // Actions when leaving state
tverouden 4:5ce2c8864908 404 /* */
tverouden 5:04b26b2f536a 405
tverouden 10:584b7d2c2d63 406 currentState = calibratingEMG; // change to state
tverouden 4:5ce2c8864908 407 changeState = true; // next loop, switch states
tverouden 4:5ce2c8864908 408 }
tverouden 4:5ce2c8864908 409 break; // end case
tverouden 4:5ce2c8864908 410
tverouden 7:ef5966469621 411 // =========================== EMG CALIBRATION MODE ===========================
tverouden 7:ef5966469621 412 case calibratingEMG:
tverouden 4:5ce2c8864908 413 // ------------------------- initialisation --------------------------
tverouden 3:9c63fc5f157e 414 if (changeState) { // when entering the state
tverouden 4:5ce2c8864908 415 pc.printf("[MODE] calibrating EMG 1 (x-direction)...\r\n");
tverouden 5:04b26b2f536a 416 // print current state
tverouden 4:5ce2c8864908 417 changeState = false; // stay in this state
tverouden 4:5ce2c8864908 418
tverouden 4:5ce2c8864908 419 // Actions when entering state
tverouden 4:5ce2c8864908 420 /* */
tverouden 4:5ce2c8864908 421
tverouden 4:5ce2c8864908 422 }
tverouden 4:5ce2c8864908 423 // ----------------------------- action ------------------------------
tverouden 5:04b26b2f536a 424 // Actions for each loop iteration
tverouden 5:04b26b2f536a 425 /* */
tverouden 4:5ce2c8864908 426
tverouden 4:5ce2c8864908 427 // --------------------------- transition ----------------------------
tverouden 7:ef5966469621 428 // Transition condition #1: after 20 sec in state
tverouden 7:ef5966469621 429 if (1) { // CONDITION
tverouden 4:5ce2c8864908 430 // Actions when leaving state
tverouden 4:5ce2c8864908 431 /* */
tverouden 5:04b26b2f536a 432
tverouden 4:5ce2c8864908 433 currentState = homing; // change to state
tverouden 4:5ce2c8864908 434 changeState = true; // next loop, switch states
tverouden 5:04b26b2f536a 435 }
tverouden 5:04b26b2f536a 436 break; // end case
tverouden 4:5ce2c8864908 437
tverouden 4:5ce2c8864908 438 // ============================== HOMING MODE ================================
tverouden 4:5ce2c8864908 439 case homing:
tverouden 7:ef5966469621 440 // ------------------------- initialisation -------------------------- // Evt. ook checken op snelheid als mensen zo dom zijn om tijdens homing op random knopjes te drukken
tverouden 4:5ce2c8864908 441 if (changeState) { // when entering the state
tverouden 4:5ce2c8864908 442 pc.printf("[MODE] homing...\r\n");
tverouden 5:04b26b2f536a 443 // print current state
tverouden 4:5ce2c8864908 444 changeState = false; // stay in this state
tverouden 4:5ce2c8864908 445
tverouden 4:5ce2c8864908 446
tverouden 4:5ce2c8864908 447 // Actions when entering state
tverouden 4:5ce2c8864908 448 /* */
tverouden 4:5ce2c8864908 449
tverouden 4:5ce2c8864908 450 }
tverouden 4:5ce2c8864908 451 // ----------------------------- action ------------------------------
tverouden 5:04b26b2f536a 452 // Actions for each loop iteration
tverouden 5:04b26b2f536a 453 /* */
tverouden 4:5ce2c8864908 454
tverouden 7:ef5966469621 455 // --------------------------- transition ---------------------------- // Volgorde veranderen voor logica?
tverouden 4:5ce2c8864908 456 // Transition condition #1: with button press, enter demo mode
tverouden 4:5ce2c8864908 457 if (buttonBio1 == true) { // Het is niet nodig om hier ook nog "&& currentState == homing" toe te voegen, want hij bereikt dit stuk code alleen in homing mode
tverouden 4:5ce2c8864908 458 // Actions when leaving state
tverouden 4:5ce2c8864908 459 /* */
tverouden 5:04b26b2f536a 460
tverouden 4:5ce2c8864908 461 currentState = demoing; // change to state
tverouden 4:5ce2c8864908 462 changeState = true; // next loop, switch states
tverouden 4:5ce2c8864908 463 }
tverouden 4:5ce2c8864908 464 // Transition condition #2: with button press, enter operation mode
tverouden 4:5ce2c8864908 465 if (buttonBio2 == true) {
tverouden 4:5ce2c8864908 466 // Actions when leaving state
tverouden 4:5ce2c8864908 467 /* */
tverouden 5:04b26b2f536a 468
tverouden 4:5ce2c8864908 469 currentState = operating; // change to state
tverouden 4:5ce2c8864908 470 changeState = true; // next loop, switch states
tverouden 5:04b26b2f536a 471 }
tverouden 4:5ce2c8864908 472 break; // end case
tverouden 4:5ce2c8864908 473
tverouden 4:5ce2c8864908 474 // ============================= OPERATING MODE ==============================
tverouden 4:5ce2c8864908 475 case operating:
tverouden 4:5ce2c8864908 476 // ------------------------- initialisation --------------------------
tverouden 4:5ce2c8864908 477 if (changeState) { // when entering the state
tverouden 4:5ce2c8864908 478 pc.printf("[MODE] operating...\r\n");
tverouden 5:04b26b2f536a 479 // print current state
tverouden 5:04b26b2f536a 480 changeState = false; // stay in this state
tverouden 5:04b26b2f536a 481
tverouden 5:04b26b2f536a 482 // Actions when entering state
tverouden 5:04b26b2f536a 483 /* */
tverouden 5:04b26b2f536a 484
tverouden 5:04b26b2f536a 485 }
tverouden 5:04b26b2f536a 486 // ----------------------------- action ------------------------------
tverouden 5:04b26b2f536a 487 // Actions for each loop iteration
tverouden 5:04b26b2f536a 488 /* */
tverouden 5:04b26b2f536a 489
tverouden 5:04b26b2f536a 490 // --------------------------- transition ----------------------------
tverouden 7:ef5966469621 491 // Transition condition #1: with button press, back to homing mode // Is het nodig om vanuit operating mode naar homing mode terug te kunnen gaan? -> ja, voor als je een demo wilt starten
tverouden 5:04b26b2f536a 492 if (buttonBio2 == true) {
tverouden 5:04b26b2f536a 493 // Actions when leaving state
tverouden 5:04b26b2f536a 494 /* */
tverouden 5:04b26b2f536a 495
tverouden 5:04b26b2f536a 496 currentState = homing; // change to state
tverouden 5:04b26b2f536a 497 changeState = true; // next loop, switch states
tverouden 5:04b26b2f536a 498 }
tverouden 7:ef5966469621 499 // Transition condition #2: motor angle error < certain value,
tverouden 7:ef5966469621 500 // start reading
tverouden 7:ef5966469621 501 if (1) { // CONDITION
tverouden 5:04b26b2f536a 502 // Actions when leaving state
tverouden 5:04b26b2f536a 503 /* */
tverouden 5:04b26b2f536a 504
tverouden 5:04b26b2f536a 505 currentState = homing; // change to state
tverouden 5:04b26b2f536a 506 changeState = true; // next loop, switch states
tverouden 5:04b26b2f536a 507 }
tverouden 5:04b26b2f536a 508 break; // end case
tverouden 5:04b26b2f536a 509
tverouden 7:ef5966469621 510 // ============================== READING MODE =============================== // Beweegt deze modus zowel heen als weer en zo ja, hoe bepaalt hij wat moet gebeuren?
tverouden 7:ef5966469621 511 case reading:
tverouden 5:04b26b2f536a 512 // ------------------------- initialisation --------------------------
tverouden 5:04b26b2f536a 513 if (changeState) { // when entering the state
tverouden 7:ef5966469621 514 pc.printf("[MODE] reading...\r\n");
tverouden 5:04b26b2f536a 515 // print current state
tverouden 5:04b26b2f536a 516 changeState = false; // stay in this state
tverouden 5:04b26b2f536a 517
tverouden 5:04b26b2f536a 518 // Actions when entering state
tverouden 5:04b26b2f536a 519 /* */
tverouden 5:04b26b2f536a 520
tverouden 5:04b26b2f536a 521 }
tverouden 5:04b26b2f536a 522 // ----------------------------- action ------------------------------
tverouden 5:04b26b2f536a 523 // Actions for each loop iteration
tverouden 5:04b26b2f536a 524 /* */
tverouden 5:04b26b2f536a 525
tverouden 5:04b26b2f536a 526 // --------------------------- transition ----------------------------
tverouden 7:ef5966469621 527 // Transition condition #1: with button press, back to homing mode // Hier automatisch terug naar operating mode!
tverouden 5:04b26b2f536a 528 if (1) { // En hij gaat nu terug naar homing mode, maar dan moet je dus elke keer weer kiezen voor demo of operation mode?
tverouden 7:ef5966469621 529 // Actions when leaving state // CONDITION
tverouden 5:04b26b2f536a 530 /* */
tverouden 5:04b26b2f536a 531
tverouden 5:04b26b2f536a 532 currentState = homing; // change to state
tverouden 5:04b26b2f536a 533 changeState = true; // next loop, switch states
tverouden 5:04b26b2f536a 534 }
tverouden 5:04b26b2f536a 535 break; // end case
tverouden 5:04b26b2f536a 536
tverouden 5:04b26b2f536a 537 // ============================== DEMOING MODE ===============================
tverouden 5:04b26b2f536a 538 case demoing:
tverouden 5:04b26b2f536a 539 // ------------------------- initialisation --------------------------
tverouden 5:04b26b2f536a 540 if (changeState) { // when entering the state
tverouden 5:04b26b2f536a 541 pc.printf("[MODE] demoing...\r\n");
tverouden 5:04b26b2f536a 542 // print current state
tverouden 5:04b26b2f536a 543 changeState = false; // stay in this state
tverouden 5:04b26b2f536a 544
tverouden 5:04b26b2f536a 545 // Actions when entering state
tverouden 5:04b26b2f536a 546 /* */
tverouden 5:04b26b2f536a 547
tverouden 5:04b26b2f536a 548 }
tverouden 5:04b26b2f536a 549 // ----------------------------- action ------------------------------
tverouden 5:04b26b2f536a 550 // Actions for each loop iteration
tverouden 5:04b26b2f536a 551 /* */
tverouden 5:04b26b2f536a 552
tverouden 5:04b26b2f536a 553 // --------------------------- transition ----------------------------
tverouden 5:04b26b2f536a 554 // Transition condition #1: with button press, back to homing mode
tverouden 5:04b26b2f536a 555 if (1) {
tverouden 5:04b26b2f536a 556 // Actions when leaving state
tverouden 5:04b26b2f536a 557 /* */
tverouden 5:04b26b2f536a 558
tverouden 5:04b26b2f536a 559 currentState = homing; // change to state
tverouden 5:04b26b2f536a 560 changeState = true; // next loop, switch states
tverouden 5:04b26b2f536a 561 }
tverouden 7:ef5966469621 562 // Transition condition #2: after 3 sec relaxation, start reading // In 3 seconden zijn de bladzijden uit zichzelf alweer helemaal teruggegaan. Misschien dit gewoon doen na het voorgedefinieerde pad
tverouden 5:04b26b2f536a 563 if (1) {
tverouden 5:04b26b2f536a 564 // Actions when leaving state
tverouden 5:04b26b2f536a 565 /* */
tverouden 5:04b26b2f536a 566
tverouden 7:ef5966469621 567 currentState = reading; // change to state
tverouden 5:04b26b2f536a 568 changeState = true; // next loop, switch states
tverouden 5:04b26b2f536a 569 }
tverouden 5:04b26b2f536a 570 break; // end case
tverouden 5:04b26b2f536a 571
tverouden 5:04b26b2f536a 572 // =============================== FAILING MODE ================================
tverouden 5:04b26b2f536a 573 case failing:
tverouden 5:04b26b2f536a 574 // ------------------------- initialisation --------------------------
tverouden 5:04b26b2f536a 575 if (changeState) { // when entering the state
tverouden 5:04b26b2f536a 576 pc.printf("[ERROR] entering failure mode\r\n");
tverouden 7:ef5966469621 577 // print current state
tverouden 3:9c63fc5f157e 578 changeState = false; // stay in this state
tverouden 3:9c63fc5f157e 579
tverouden 3:9c63fc5f157e 580 // Actions when entering state
tverouden 7:ef5966469621 581 ledGreen = 1; // red LED on // Blijft dit aan?
tverouden 6:f32352bc5078 582 ledBlue = 1;
tverouden 6:f32352bc5078 583 ledRed = 0;
tverouden 4:5ce2c8864908 584
tverouden 6:f32352bc5078 585 // pin3 = 0; // all motor forces to zero // Pins nog niet gedefiniëerd
tverouden 6:f32352bc5078 586 // pin5 = 0;
tverouden 6:f32352bc5078 587 // pin6 = 0;
tverouden 6:f32352bc5078 588 exit (0); // abort mission
tverouden 4:5ce2c8864908 589 }
tverouden 4:5ce2c8864908 590 break; // end case
tverouden 4:5ce2c8864908 591
tverouden 4:5ce2c8864908 592 // ============================== DEFAULT MODE =================================
tverouden 3:9c63fc5f157e 593 default:
tverouden 4:5ce2c8864908 594 // ---------------------------- enter failing mode -----------------------------
tverouden 4:5ce2c8864908 595 currentState = failing; // change to state
tverouden 4:5ce2c8864908 596 changeState = true; // next loop, switch states
tverouden 4:5ce2c8864908 597 // print current state
tverouden 4:5ce2c8864908 598 pc.printf("[ERROR] unknown or unimplemented state reached\r\n");
tverouden 3:9c63fc5f157e 599
tverouden 4:5ce2c8864908 600 } // end switch
tverouden 4:5ce2c8864908 601 } // end stateMachine
tverouden 3:9c63fc5f157e 602
tverouden 3:9c63fc5f157e 603
tverouden 2:d70795e4e0bf 604
tverouden 2:d70795e4e0bf 605 // ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡ MAIN LOOP ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡
tverouden 2:d70795e4e0bf 606
tverouden 3:9c63fc5f157e 607 int main()
tverouden 3:9c63fc5f157e 608 {
tverouden 8:8cef1050ebd9 609 // ================================ EMERGENCY ================================
tverouden 7:ef5966469621 610 //If the emergency button is pressed, stop program via failing state
tverouden 8:8cef1050ebd9 611 buttonEmergency.rise(stopProgram); // Automatische triggers voor failure mode?
tverouden 8:8cef1050ebd9 612
tverouden 8:8cef1050ebd9 613 // ================================ EMERGENCY ================================
tverouden 8:8cef1050ebd9 614 pc.baud(115200); // communication with terminal // Baud rate
tverouden 6:f32352bc5078 615
tverouden 2:d70795e4e0bf 616 // ==================================== LOOP ===================================
tverouden 2:d70795e4e0bf 617 while (true) { // loop forever
tverouden 2:d70795e4e0bf 618
tverouden 2:d70795e4e0bf 619 }
tverouden 2:d70795e4e0bf 620 }