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@18:c74dfa681e09, 2018-11-01 (annotated)

Committer:
tverouden
Date:
Thu Nov 01 10:29:30 2018 +0000
Revision:
18:c74dfa681e09
Parent:
17:b04e1938491a
Child:
19:2797bb471f9f
Fixed emergency button LED (works!); Removed Biorobotics shield LED pins

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