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

Committer:
tverouden
Date:
Thu Nov 01 10:16:10 2018 +0000
Revision:
16:c2986e890040
Parent:
15:6566c5dedeeb
Parent:
14:2c0bf576a0e7
Child:
17:b04e1938491a
Merged LED & EMG stuff

Who changed what in which revision?

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