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@24:0abc564349e1, 2018-11-01 (annotated)

Committer:
tverouden
Date:
Thu Nov 01 12:25:30 2018 +0000
Revision:
24:0abc564349e1
Parent:
23:e282bdb9e9b7
Child:
25:ac139331fe51
Cleaned up more comments; Fully implemented state transition logic; Bug: 20 sec transition still missing

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