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@48:1eb0f9ed6cd9, 2018-11-01 (annotated)

Committer:
efvanmarrewijk
Date:
Thu Nov 01 18:46:33 2018 +0000
Revision:
48:1eb0f9ed6cd9
Parent:
47:7919857587f8
Child:
49:0ada5a721686
Calibration of motor left works; Code for rest of motors (right and flip) deleted

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tverouden 0:c0c35b95765f 1 // ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡ PREPARATION ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡
tverouden 0:c0c35b95765f 2 // Libraries
tverouden 0:c0c35b95765f 3 #include "mbed.h"
tverouden 2:d70795e4e0bf 4 #include "BiQuad.h"
tverouden 0:c0c35b95765f 5 #include "FastPWM.h"
tverouden 0:c0c35b95765f 6 #include "MODSERIAL.h"
efvanmarrewijk 25:ac139331fe51 7 #include "QEI.h"
EvaKrolis 13:4ba8f63e6ff4 8 #include <algorithm>
tverouden 41:5aecc1a27ce6 9 #include <math.h>
tverouden 41:5aecc1a27ce6 10 #include <cmath>
EvaKrolis 14:2c0bf576a0e7 11 #define PI 3.14159265
tverouden 0:c0c35b95765f 12
tverouden 8:8cef1050ebd9 13 // LEDs
tverouden 23:e282bdb9e9b7 14 DigitalOut ledRed(LED_RED,1); // red LED K64F
tverouden 23:e282bdb9e9b7 15 DigitalOut ledGreen(LED_GREEN,1); // green LED K64F
tverouden 23:e282bdb9e9b7 16 DigitalOut ledBlue(LED_BLUE,1); // blue LED K64F
tverouden 8:8cef1050ebd9 17
tverouden 23:e282bdb9e9b7 18 Ticker blinkTimer; // LED ticker
EvaKrolis 36:b233c549dd80 19 Timer EMGtransition_timer; // Timer for the transition from EMG calibration to homing
tverouden 8:8cef1050ebd9 20
tverouden 8:8cef1050ebd9 21 // Buttons/inputs
tverouden 23:e282bdb9e9b7 22 InterruptIn buttonBio1(D0); // button 1 BioRobotics shield
tverouden 23:e282bdb9e9b7 23 InterruptIn buttonBio2(D1); // button 2 BioRobotics shield
tverouden 23:e282bdb9e9b7 24 InterruptIn buttonEmergency(SW2); // emergency button on K64F
tverouden 24:0abc564349e1 25 InterruptIn buttonHome(SW3); // button on K64F
tverouden 0:c0c35b95765f 26
efvanmarrewijk 25:ac139331fe51 27 // Potmeters
efvanmarrewijk 25:ac139331fe51 28 AnalogIn pot1(A1);
efvanmarrewijk 25:ac139331fe51 29 AnalogIn pot2(A2);
efvanmarrewijk 25:ac139331fe51 30
efvanmarrewijk 25:ac139331fe51 31 // Motor pins input
efvanmarrewijk 25:ac139331fe51 32 DigitalIn pin8(D8); // Encoder L B
efvanmarrewijk 25:ac139331fe51 33 DigitalIn pin9(D9); // Encoder L A
efvanmarrewijk 25:ac139331fe51 34 DigitalIn pin10(D10); // Encoder R B
efvanmarrewijk 25:ac139331fe51 35 DigitalIn pin11(D11); // Encoder R A
efvanmarrewijk 25:ac139331fe51 36 DigitalIn pin12(D12); // Encoder F B
efvanmarrewijk 25:ac139331fe51 37 DigitalIn pin13(D13); // Encoder F A
tverouden 8:8cef1050ebd9 38
efvanmarrewijk 25:ac139331fe51 39 // Motor pins output
efvanmarrewijk 25:ac139331fe51 40 DigitalOut pin2(D2); // Motor F direction = motor flip
efvanmarrewijk 25:ac139331fe51 41 FastPWM pin3(A5); // Motor F pwm = motor flip
efvanmarrewijk 25:ac139331fe51 42 DigitalOut pin4(D4); // Motor R direction = motor right
efvanmarrewijk 25:ac139331fe51 43 FastPWM pin5(D5); // Motor R pwm = motor right
efvanmarrewijk 25:ac139331fe51 44 FastPWM pin6(D6); // Motor L pwm = motor left
tverouden 32:a00ff9898574 45 DigitalOut pin7(D7); // Motor L direction = motor left
efvanmarrewijk 25:ac139331fe51 46
efvanmarrewijk 25:ac139331fe51 47 // Utilisation of libraries
tverouden 32:a00ff9898574 48 MODSERIAL pc(USBTX, USBRX); // communication with pc
efvanmarrewijk 25:ac139331fe51 49 QEI encoderL(D9,D8,NC,4200); // Encoder input of motor L
efvanmarrewijk 25:ac139331fe51 50 QEI encoderR(D10,D11,NC,4200); // Encoder input of motor R
efvanmarrewijk 25:ac139331fe51 51 QEI encoderF(D12,D13,NC,4200); // Encoder input of motor F
efvanmarrewijk 25:ac139331fe51 52 Ticker motorL;
efvanmarrewijk 25:ac139331fe51 53 Ticker motorR;
efvanmarrewijk 25:ac139331fe51 54 Ticker motorF;
tverouden 4:5ce2c8864908 55
tverouden 0:c0c35b95765f 56 // Define & initialise state machine
efvanmarrewijk 26:247be0bea9b1 57 const float dt = 0.002f;
tverouden 11:2d1dfebae948 58 enum states { calibratingMotors, calibratingEMG,
tverouden 17:b04e1938491a 59 homing, reading, operating, failing, demoing
tverouden 2:d70795e4e0bf 60 };
EvaKrolis 47:7919857587f8 61 states currentState = calibratingMotors; // start with calibrating motors
tverouden 23:e282bdb9e9b7 62 bool changeState = true; // initialise the first state
tverouden 2:d70795e4e0bf 63
tverouden 23:e282bdb9e9b7 64 Ticker stateTimer; // state machine ticker
tverouden 19:2797bb471f9f 65
EvaKrolis 14:2c0bf576a0e7 66 //------------------------ Parameters for the EMG ----------------------------
tverouden 3:9c63fc5f157e 67
tverouden 23:e282bdb9e9b7 68 // EMG inputs
tverouden 23:e282bdb9e9b7 69 AnalogIn EMG0In(A0); // EMG input 0
tverouden 23:e282bdb9e9b7 70 AnalogIn EMG1In(A1); // EMG input 1
EvaKrolis 13:4ba8f63e6ff4 71
tverouden 23:e282bdb9e9b7 72 // Timers
tverouden 23:e282bdb9e9b7 73 Ticker FindMax0_timer; // Timer for finding the max muscle
tverouden 23:e282bdb9e9b7 74 Ticker FindMax1_timer; // Timer for finding the max muscle
EvaKrolis 13:4ba8f63e6ff4 75
tverouden 23:e282bdb9e9b7 76 // Constants
tverouden 23:e282bdb9e9b7 77 const int Length = 10000; // Length of the array for the calibration
tverouden 23:e282bdb9e9b7 78 const int Parts = 50; // Mean average filter over 50 values
EvaKrolis 13:4ba8f63e6ff4 79
tverouden 23:e282bdb9e9b7 80 // Parameters for the first EMG signal
tverouden 23:e282bdb9e9b7 81 float EMG0; // float for EMG input
tverouden 23:e282bdb9e9b7 82 float EMG0filt; // float for filtered EMG
tverouden 23:e282bdb9e9b7 83 float EMG0filtArray[Parts]; // Array for the filtered array
tverouden 23:e282bdb9e9b7 84 float EMG0Average; // float for the value after Moving Average Filter
tverouden 23:e282bdb9e9b7 85 float Sum0 = 0; // Sum0 for the moving average filter
tverouden 41:5aecc1a27ce6 86 float EMG0Calibrate[Length]; // Array for the calibration
tverouden 23:e282bdb9e9b7 87 int ReadCal0 = 0; // Integer to read over the calibration array
tverouden 23:e282bdb9e9b7 88 float MaxValue0 = 0; // float to save the max muscle
tverouden 23:e282bdb9e9b7 89 float Threshold0 = 0; // Threshold for the first EMG signal
EvaKrolis 13:4ba8f63e6ff4 90
tverouden 23:e282bdb9e9b7 91 // Parameters for the second EMG signal
tverouden 23:e282bdb9e9b7 92 float EMG1; // float for EMG input
tverouden 23:e282bdb9e9b7 93 float EMG1filt; // float for filtered EMG
tverouden 23:e282bdb9e9b7 94 float EMG1filtArray[Parts]; // Array for the filtered array
tverouden 23:e282bdb9e9b7 95 float EMG1Average; // float for the value after Moving Average Filter
tverouden 23:e282bdb9e9b7 96 float Sum1 = 0; // Sum0 for the moving average filter
tverouden 41:5aecc1a27ce6 97 float EMG1Calibrate[Length]; // Array for the calibration
tverouden 23:e282bdb9e9b7 98 int ReadCal1 = 0; // Integer to read over the calibration array
tverouden 23:e282bdb9e9b7 99 float MaxValue1 = 0; // float to save the max muscle
tverouden 23:e282bdb9e9b7 100 float Threshold1 = 0; // Threshold for the second EMG signal
EvaKrolis 13:4ba8f63e6ff4 101
EvaKrolis 13:4ba8f63e6ff4 102 //Filter variables
EvaKrolis 36:b233c549dd80 103 BiQuad Notch50_0(0.9049,-1.4641,0.9049,-1.4641,0.8098); //Make Notch filter around 50 Hz
EvaKrolis 36:b233c549dd80 104 BiQuad Notch50_1(0.9049,-1.4641,0.9049,-1.4641,0.8098); //Make Notch filter around 50 Hz
EvaKrolis 36:b233c549dd80 105 BiQuad Notch100_0(0.8427,-0.5097,0.8247,-0.5097,0.6494); //Make Notch filter around 100 Hz
EvaKrolis 36:b233c549dd80 106 BiQuad Notch100_1(0.8427,-0.5097,0.8247,-0.5097,0.6494); //Make Notch filter around 100 Hz
EvaKrolis 36:b233c549dd80 107 BiQuad Notch150_0(0.7548,0.4665,0.7544,0.4665,0.5095); //Make Notch filter around 150 Hz
EvaKrolis 36:b233c549dd80 108 BiQuad Notch150_1(0.7548,0.4665,0.7544,0.4665,0.5095); //Make Notch filter around 150 Hz
EvaKrolis 36:b233c549dd80 109 BiQuad Notch200_0(0.6919,1.1196,0.6919,1.1196,0.3839); //Make Notch filter around 200 Hz
EvaKrolis 36:b233c549dd80 110 BiQuad Notch200_1(0.6919,1.1196,0.6919,1.1196,0.3839); //Make Notch filter around 200 Hz
EvaKrolis 36:b233c549dd80 111 BiQuad High_0(0.9150,-1.8299,0.9150,-1.8227,0.8372); //Make high-pass filter
EvaKrolis 36:b233c549dd80 112 BiQuad High_1(0.9150,-1.8299,0.9150,-1.8227,0.8372); //Make high-pass filter
EvaKrolis 36:b233c549dd80 113 BiQuad Low_0(0.6389,1.2779,0.6389,1.143,0.4128); //Make low-pass filter
EvaKrolis 36:b233c549dd80 114 BiQuad Low_1(0.6389,1.2779,0.6389,1.143,0.4128); //Make low-pass filter
EvaKrolis 13:4ba8f63e6ff4 115 BiQuadChain filter0; //Make chain of filters for the first EMG signal
EvaKrolis 13:4ba8f63e6ff4 116 BiQuadChain filter1; //Make chain of filters for the second EMG signal
EvaKrolis 13:4ba8f63e6ff4 117
EvaKrolis 13:4ba8f63e6ff4 118 //Bool for movement
EvaKrolis 13:4ba8f63e6ff4 119 bool xMove = false; //Bool for the x-movement
EvaKrolis 13:4ba8f63e6ff4 120 bool yMove = false; //Bool for the y-movement
tverouden 3:9c63fc5f157e 121
EvaKrolis 14:2c0bf576a0e7 122 // -------------------- Parameters for the kinematics -------------------------
EvaKrolis 14:2c0bf576a0e7 123
EvaKrolis 14:2c0bf576a0e7 124 //Constants
EvaKrolis 14:2c0bf576a0e7 125 const double ll = 230; //Length of the lower arm
EvaKrolis 14:2c0bf576a0e7 126 const double lu = 198; //Length of the upper arm
EvaKrolis 14:2c0bf576a0e7 127 const double lb = 50; //Length of the part between the upper arms
EvaKrolis 14:2c0bf576a0e7 128 const double le = 79; //Length of the end-effector beam
EvaKrolis 14:2c0bf576a0e7 129 const double xbase = 450-lb; //Length between the motors
EvaKrolis 14:2c0bf576a0e7 130
EvaKrolis 14:2c0bf576a0e7 131 //General parameters
EvaKrolis 14:2c0bf576a0e7 132 double theta1 = PI*0.49; //Angle of the left motor
EvaKrolis 14:2c0bf576a0e7 133 double theta4 = PI*0.49; //Angle of the right motor
EvaKrolis 14:2c0bf576a0e7 134 double thetaflip = 0; //Angle of the flipping motor
EvaKrolis 14:2c0bf576a0e7 135 double prefx; //Desired x velocity
EvaKrolis 14:2c0bf576a0e7 136 double prefy; //Desired y velocity "
tverouden 41:5aecc1a27ce6 137 float iJ[2][2]; //inverse Jacobian matrix
EvaKrolis 14:2c0bf576a0e7 138
tverouden 44:ca74d11a2dac 139 //Kinematics parameters for x
tverouden 44:ca74d11a2dac 140 float xendsum;
tverouden 44:ca74d11a2dac 141 float xendsqrt1;
tverouden 44:ca74d11a2dac 142 float xendsqrt2;
tverouden 44:ca74d11a2dac 143 float xend;
tverouden 44:ca74d11a2dac 144
tverouden 44:ca74d11a2dac 145 //Kinematics parameters for y
tverouden 44:ca74d11a2dac 146 float yendsum;
tverouden 44:ca74d11a2dac 147 float yendsqrt1;
tverouden 44:ca74d11a2dac 148 float yendsqrt2;
tverouden 44:ca74d11a2dac 149 float yend;
tverouden 44:ca74d11a2dac 150
EvaKrolis 14:2c0bf576a0e7 151 // ---------------------- Parameters for the motors ---------------------------
efvanmarrewijk 26:247be0bea9b1 152 const float countsRad = 4200.0f;
efvanmarrewijk 26:247be0bea9b1 153 int countsL;
efvanmarrewijk 26:247be0bea9b1 154 int countsR;
efvanmarrewijk 26:247be0bea9b1 155 int countsF;
efvanmarrewijk 26:247be0bea9b1 156 int countsCalibratedL;
efvanmarrewijk 26:247be0bea9b1 157 int countsCalibratedR;
efvanmarrewijk 26:247be0bea9b1 158 int countsCalibratedF;
efvanmarrewijk 48:1eb0f9ed6cd9 159 int countsOffsetL;
efvanmarrewijk 48:1eb0f9ed6cd9 160 int countsOffsetR;
efvanmarrewijk 48:1eb0f9ed6cd9 161 int countsOffsetF;
efvanmarrewijk 26:247be0bea9b1 162 float angleCurrentL;
efvanmarrewijk 26:247be0bea9b1 163 float angleCurrentR;
efvanmarrewijk 26:247be0bea9b1 164 float angleCurrentF;
efvanmarrewijk 26:247be0bea9b1 165 float errorL;
efvanmarrewijk 26:247be0bea9b1 166 float errorR;
efvanmarrewijk 26:247be0bea9b1 167 float errorF;
efvanmarrewijk 26:247be0bea9b1 168 float errorCalibratedL;
efvanmarrewijk 26:247be0bea9b1 169 float errorCalibratedR;
efvanmarrewijk 26:247be0bea9b1 170 float errorCalibratedF;
EvaKrolis 14:2c0bf576a0e7 171
efvanmarrewijk 26:247be0bea9b1 172 int countsCalibration = 4200/4;
efvanmarrewijk 25:ac139331fe51 173
tverouden 4:5ce2c8864908 174 // ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡ FUNCTIONS ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡
tverouden 6:f32352bc5078 175 // ============================ GENERAL FUNCTIONS =============================
tverouden 6:f32352bc5078 176 void stopProgram(void)
tverouden 6:f32352bc5078 177 {
tverouden 6:f32352bc5078 178 // Error message
tverouden 6:f32352bc5078 179 pc.printf("[ERROR] emergency button pressed\r\n");
tverouden 6:f32352bc5078 180 currentState = failing; // change to state
tverouden 6:f32352bc5078 181 changeState = true; // next loop, switch states
tverouden 6:f32352bc5078 182 }
tverouden 8:8cef1050ebd9 183
tverouden 15:6566c5dedeeb 184 void blinkLedRed(void)
tverouden 15:6566c5dedeeb 185 {
tverouden 15:6566c5dedeeb 186 ledRed =! ledRed; // toggle LED
tverouden 10:584b7d2c2d63 187 }
tverouden 15:6566c5dedeeb 188 void blinkLedGreen(void)
tverouden 15:6566c5dedeeb 189 {
tverouden 15:6566c5dedeeb 190 ledGreen =! ledGreen; // toggle LED
tverouden 15:6566c5dedeeb 191 }
tverouden 15:6566c5dedeeb 192 void blinkLedBlue(void)
tverouden 15:6566c5dedeeb 193 {
tverouden 15:6566c5dedeeb 194 ledBlue =! ledBlue; // toggle LED
tverouden 15:6566c5dedeeb 195 }
tverouden 15:6566c5dedeeb 196
tverouden 4:5ce2c8864908 197 // ============================= EMG FUNCTIONS ===============================
tverouden 4:5ce2c8864908 198
EvaKrolis 13:4ba8f63e6ff4 199 //Function to read and filter the EMG
tverouden 41:5aecc1a27ce6 200 void ReadUseEMG0()
tverouden 41:5aecc1a27ce6 201 {
tverouden 41:5aecc1a27ce6 202 for(int i = Parts ; i > 0 ; i--) { //Make a first in, first out array
EvaKrolis 13:4ba8f63e6ff4 203 EMG0filtArray[i] = EMG0filtArray[i-1]; //Every value moves one up
EvaKrolis 13:4ba8f63e6ff4 204 }
tverouden 41:5aecc1a27ce6 205
EvaKrolis 13:4ba8f63e6ff4 206 Sum0 = 0;
EvaKrolis 13:4ba8f63e6ff4 207 EMG0 = EMG0In; //Save EMG input in float
EvaKrolis 13:4ba8f63e6ff4 208 EMG0filt = filter0.step(EMG0); //Filter the signal
EvaKrolis 13:4ba8f63e6ff4 209 EMG0filt = abs(EMG0filt); //Take the absolute value
EvaKrolis 13:4ba8f63e6ff4 210 EMG0filtArray[0] = EMG0filt; //Save the filtered signal on the first place in the array
tverouden 41:5aecc1a27ce6 211
tverouden 41:5aecc1a27ce6 212 for(int i = 0 ; i < Parts ; i++) { //Moving Average filter
EvaKrolis 13:4ba8f63e6ff4 213 Sum0 += EMG0filtArray[i]; //Sum the new value and the previous 49
EvaKrolis 13:4ba8f63e6ff4 214 }
EvaKrolis 13:4ba8f63e6ff4 215 EMG0Average = (float)Sum0/Parts; //Divide the sum by 50
tverouden 41:5aecc1a27ce6 216
tverouden 41:5aecc1a27ce6 217 if (EMG0Average > Threshold0) { //If the value is higher than the threshold value
EvaKrolis 13:4ba8f63e6ff4 218 xMove = true; //Set movement to true
tverouden 41:5aecc1a27ce6 219 } else {
tverouden 41:5aecc1a27ce6 220 xMove = false; //Otherwise set movement to false
EvaKrolis 13:4ba8f63e6ff4 221 }
EvaKrolis 13:4ba8f63e6ff4 222 }
EvaKrolis 13:4ba8f63e6ff4 223
EvaKrolis 13:4ba8f63e6ff4 224 //Function to read and filter the EMG
tverouden 41:5aecc1a27ce6 225 void ReadUseEMG1()
tverouden 41:5aecc1a27ce6 226 {
tverouden 41:5aecc1a27ce6 227 for(int i = Parts ; i > 0 ; i--) { //Make a first in, first out array
EvaKrolis 13:4ba8f63e6ff4 228 EMG1filtArray[i] = EMG1filtArray[i-1]; //Every value moves one up
EvaKrolis 13:4ba8f63e6ff4 229 }
tverouden 41:5aecc1a27ce6 230
EvaKrolis 13:4ba8f63e6ff4 231 Sum1 = 0;
EvaKrolis 13:4ba8f63e6ff4 232 EMG1 = EMG1In; //Save EMG input in float
EvaKrolis 13:4ba8f63e6ff4 233 EMG1filt = filter1.step(EMG1); //Filter the signal
EvaKrolis 13:4ba8f63e6ff4 234 EMG1filt = abs(EMG1filt); //Take the absolute value
EvaKrolis 13:4ba8f63e6ff4 235 EMG1filtArray[0] = EMG1filt; //Save the filtered signal on the first place in the array
tverouden 41:5aecc1a27ce6 236
tverouden 41:5aecc1a27ce6 237 for(int i = 0 ; i < Parts ; i++) { //Moving Average filter
EvaKrolis 13:4ba8f63e6ff4 238 Sum1 += EMG1filtArray[i]; //Sum the new value and the previous 49
EvaKrolis 13:4ba8f63e6ff4 239 }
EvaKrolis 13:4ba8f63e6ff4 240 EMG1Average = (float)Sum1/Parts; //Divide the sum by 50
tverouden 41:5aecc1a27ce6 241
tverouden 41:5aecc1a27ce6 242 if (EMG1Average > Threshold1) { //If the value is higher than the threshold value
EvaKrolis 13:4ba8f63e6ff4 243 yMove = true; //Set y movement to true
tverouden 41:5aecc1a27ce6 244 } else {
EvaKrolis 13:4ba8f63e6ff4 245 yMove = false; //Otherwise set y movement to false
EvaKrolis 13:4ba8f63e6ff4 246 }
EvaKrolis 13:4ba8f63e6ff4 247 }
EvaKrolis 13:4ba8f63e6ff4 248
EvaKrolis 13:4ba8f63e6ff4 249 //Function to make an array during the calibration
tverouden 41:5aecc1a27ce6 250 void CalibrateEMG0()
tverouden 41:5aecc1a27ce6 251 {
tverouden 41:5aecc1a27ce6 252 for(int i = Parts ; i > 0 ; i--) { //Make a first in, first out array
EvaKrolis 13:4ba8f63e6ff4 253 EMG0filtArray[i] = EMG0filtArray[i-1]; //Every value moves one up
EvaKrolis 13:4ba8f63e6ff4 254 }
tverouden 41:5aecc1a27ce6 255
EvaKrolis 13:4ba8f63e6ff4 256 Sum0 = 0;
EvaKrolis 13:4ba8f63e6ff4 257 EMG0 = EMG0In; //Save EMG input in float
EvaKrolis 13:4ba8f63e6ff4 258 EMG0filt = filter0.step(EMG0); //Filter the signal
EvaKrolis 13:4ba8f63e6ff4 259 EMG0filt = abs(EMG0filt); //Take the absolute value
EvaKrolis 13:4ba8f63e6ff4 260 EMG0filtArray[0] = EMG0filt; //Save the filtered signal on the first place in the array
tverouden 41:5aecc1a27ce6 261
tverouden 41:5aecc1a27ce6 262 for(int i = 0 ; i < Parts ; i++) { //Moving Average filter
EvaKrolis 13:4ba8f63e6ff4 263 Sum0 += EMG0filtArray[i]; //Sum the new value and the previous 49
EvaKrolis 13:4ba8f63e6ff4 264 }
EvaKrolis 13:4ba8f63e6ff4 265 EMG0Calibrate[ReadCal0] = (float)Sum0/Parts; //Divide the sum by 50
tverouden 41:5aecc1a27ce6 266
EvaKrolis 13:4ba8f63e6ff4 267 ReadCal0++;
EvaKrolis 13:4ba8f63e6ff4 268 }
EvaKrolis 13:4ba8f63e6ff4 269
EvaKrolis 13:4ba8f63e6ff4 270 //Function to make an array during the calibration
tverouden 41:5aecc1a27ce6 271 void CalibrateEMG1()
tverouden 41:5aecc1a27ce6 272 {
tverouden 41:5aecc1a27ce6 273 for(int i = Parts ; i > 0 ; i--) { //Make a first in, first out array
EvaKrolis 13:4ba8f63e6ff4 274 EMG1filtArray[i] = EMG1filtArray[i-1]; //Every value moves one up
EvaKrolis 13:4ba8f63e6ff4 275 }
tverouden 41:5aecc1a27ce6 276
EvaKrolis 13:4ba8f63e6ff4 277 Sum1 = 0;
EvaKrolis 13:4ba8f63e6ff4 278 EMG1 = EMG1In; //Save EMG input in float
EvaKrolis 13:4ba8f63e6ff4 279 EMG1filt = filter1.step(EMG1); //Filter the signal
EvaKrolis 13:4ba8f63e6ff4 280 EMG1filt = abs(EMG1filt); //Take the absolute value
EvaKrolis 13:4ba8f63e6ff4 281 EMG1filtArray[0] = EMG1filt; //Save the filtered signal on the first place in the array
tverouden 41:5aecc1a27ce6 282
tverouden 41:5aecc1a27ce6 283 for(int i = 0 ; i < Parts ; i++) { //Moving Average filter
EvaKrolis 13:4ba8f63e6ff4 284 Sum1 += EMG1filtArray[i]; //Sum the new value and the previous 49
EvaKrolis 13:4ba8f63e6ff4 285 }
EvaKrolis 13:4ba8f63e6ff4 286 EMG1Calibrate[ReadCal1] = (float)Sum1/Parts; //Divide the sum by 50
tverouden 41:5aecc1a27ce6 287
EvaKrolis 13:4ba8f63e6ff4 288 ReadCal1++;
EvaKrolis 13:4ba8f63e6ff4 289 }
EvaKrolis 13:4ba8f63e6ff4 290
EvaKrolis 13:4ba8f63e6ff4 291 //Function to find the max value during the calibration
tverouden 41:5aecc1a27ce6 292 void FindMax0()
tverouden 41:5aecc1a27ce6 293 {
EvaKrolis 13:4ba8f63e6ff4 294 MaxValue0 = *max_element(EMG0Calibrate+500,EMG0Calibrate+Length); //Find max value, but discard the first 100 values
EvaKrolis 13:4ba8f63e6ff4 295 Threshold0 = 0.30f*MaxValue0; //The threshold is a percentage of the max value
EvaKrolis 13:4ba8f63e6ff4 296 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 297 FindMax0_timer.detach(); //Detach the timer, so you only use this once
EvaKrolis 13:4ba8f63e6ff4 298 }
EvaKrolis 13:4ba8f63e6ff4 299
EvaKrolis 13:4ba8f63e6ff4 300 //Function to find the max value during the calibration
tverouden 41:5aecc1a27ce6 301 void FindMax1()
tverouden 41:5aecc1a27ce6 302 {
EvaKrolis 13:4ba8f63e6ff4 303 MaxValue1 = *max_element(EMG1Calibrate+500,EMG1Calibrate+Length); //Find max value, but discard the first 100 values
EvaKrolis 13:4ba8f63e6ff4 304 Threshold1 = 0.30f*MaxValue1; //The threshold is a percentage of the max value
EvaKrolis 13:4ba8f63e6ff4 305 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 306 FindMax1_timer.detach(); //Detach the timer, so you only use this once
EvaKrolis 13:4ba8f63e6ff4 307 }
tverouden 4:5ce2c8864908 308
tverouden 12:323ac4e27a0d 309 // ========================= KINEMATICS FUNCTIONS ============================
tverouden 12:323ac4e27a0d 310
tverouden 41:5aecc1a27ce6 311 //forward kinematics function , &xend and&yend are output.
tverouden 41:5aecc1a27ce6 312 void kinematicsForward(float &xend_, float &yend_, float theta1_, float theta4_)
EvaKrolis 14:2c0bf576a0e7 313 {
tverouden 41:5aecc1a27ce6 314
tverouden 41:5aecc1a27ce6 315 //Below we have the forward kinematics formula. Input should be the measured angles theta1 &theta4. Output
tverouden 41:5aecc1a27ce6 316
tverouden 41:5aecc1a27ce6 317 float xendsum_ = lb + xbase +ll*(cos(theta1_) - cos(theta4_));
tverouden 41:5aecc1a27ce6 318 float xendsqrt1_ = 2*sqrt(-xbase*xbase/4 + lu*lu + ll*(xbase*(cos(theta1_)+cos(theta4_))/2) -ll*(1+ cos(theta1_+theta4_)))*(-sin(theta1_)+sin(theta4_));
tverouden 41:5aecc1a27ce6 319 float xendsqrt2_ = sqrt(pow((-xbase/ll+cos(theta1_)+cos(theta4_)),2)+ pow(sin(theta1_) - sin(theta4_),2));
tverouden 41:5aecc1a27ce6 320 xend_ = (xendsum_ + xendsqrt1_/xendsqrt2_)/2;
EvaKrolis 14:2c0bf576a0e7 321
tverouden 41:5aecc1a27ce6 322 float yendsum_ = -le + ll/2*(sin(theta1_)+sin(theta4_));
tverouden 41:5aecc1a27ce6 323 float 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_))));
tverouden 41:5aecc1a27ce6 324 float yendsqrt2_ = sqrt(pow((-xbase/ll + cos(theta1_)+ cos(theta4_)),2)+ pow((sin(theta1_)-sin(theta4_)),2));
tverouden 41:5aecc1a27ce6 325 yend_ = (yendsum_ + yendsqrt1_/yendsqrt2_);
tverouden 41:5aecc1a27ce6 326 }
tverouden 41:5aecc1a27ce6 327
tverouden 41:5aecc1a27ce6 328 //Below we have the inverse kinematics function.
tverouden 41:5aecc1a27ce6 329 void kinematicsInverse(float prex, float prey)
tverouden 41:5aecc1a27ce6 330 {
tverouden 41:5aecc1a27ce6 331
tverouden 41:5aecc1a27ce6 332 theta1 += (prefx*(iJ[0][0])-iJ[0][1]*prey)*dt; //theta 1 is itself + the desired speeds in x and y direction, both
tverouden 41:5aecc1a27ce6 333 theta4 += (prefx*iJ[1][0]-iJ[1][1]*prey)*dt; //multiplied with a prefactor which comes out of the motor
tverouden 41:5aecc1a27ce6 334 //the iJ values are defined in the "kinematics" function
tverouden 41:5aecc1a27ce6 335
tverouden 41:5aecc1a27ce6 336 //Calling the forward kinematics, to calculate xend and yend
tverouden 41:5aecc1a27ce6 337 kinematicsForward(xend,yend,theta1,theta4);
EvaKrolis 14:2c0bf576a0e7 338
EvaKrolis 14:2c0bf576a0e7 339 }
EvaKrolis 14:2c0bf576a0e7 340
EvaKrolis 14:2c0bf576a0e7 341 void kinematics()
EvaKrolis 14:2c0bf576a0e7 342 {
tverouden 41:5aecc1a27ce6 343 float xend1,xend2,xend3,yend1,yend2,yend3;
EvaKrolis 14:2c0bf576a0e7 344
tverouden 41:5aecc1a27ce6 345 const float dq = 0.001; //benadering van 'delta' hoek
tverouden 41:5aecc1a27ce6 346
tverouden 41:5aecc1a27ce6 347 kinematicsForward(xend1,yend1,theta1,theta4);
tverouden 41:5aecc1a27ce6 348 kinematicsForward(xend2,yend2,theta1+dq,theta4);
tverouden 41:5aecc1a27ce6 349 kinematicsForward(xend3,yend3,theta1,theta4+dq);
EvaKrolis 14:2c0bf576a0e7 350
tverouden 41:5aecc1a27ce6 351 float a,b,c,d;
tverouden 41:5aecc1a27ce6 352 a = xend2-xend1;
tverouden 41:5aecc1a27ce6 353 b = xend3-xend1;
tverouden 41:5aecc1a27ce6 354 c = yend2-yend1;
tverouden 41:5aecc1a27ce6 355 d = yend3-yend1;
tverouden 41:5aecc1a27ce6 356
tverouden 41:5aecc1a27ce6 357 float Q = 1/(a*d-b*c)/dq;
EvaKrolis 14:2c0bf576a0e7 358
tverouden 41:5aecc1a27ce6 359 iJ[0][0] = d*Q;
tverouden 41:5aecc1a27ce6 360 iJ[0][1]= -c*Q;
tverouden 41:5aecc1a27ce6 361 iJ[1][0] = -b*Q;
tverouden 41:5aecc1a27ce6 362 iJ[1][1] = a*Q;
EvaKrolis 14:2c0bf576a0e7 363
tverouden 41:5aecc1a27ce6 364 prefx = 0.001*xMove; //sw3, Prefx has multiplier one, // Gain aanpassen eventueel??
tverouden 41:5aecc1a27ce6 365 // but that has to become a value
tverouden 41:5aecc1a27ce6 366 // dependant on the motor
tverouden 41:5aecc1a27ce6 367 prefy = 0.001*yMove; //sw2,
tverouden 41:5aecc1a27ce6 368 kinematicsInverse(prefx, prefy);
tverouden 41:5aecc1a27ce6 369 }
tverouden 41:5aecc1a27ce6 370
tverouden 41:5aecc1a27ce6 371 // these values are printed for controlling purposes (can later be removed)
tverouden 41:5aecc1a27ce6 372 void printvalue()
tverouden 41:5aecc1a27ce6 373 {
tverouden 41:5aecc1a27ce6 374 pc.printf("X-value: %f \t Y-value: %f \n\r \t theta 1 = %f \t theta4 = %f\n\r",xend, yend,theta1,theta4);
tverouden 41:5aecc1a27ce6 375 //pc.printf("%f\n\r",xend1);
EvaKrolis 14:2c0bf576a0e7 376 }
tverouden 12:323ac4e27a0d 377
efvanmarrewijk 25:ac139331fe51 378 // ============================ MOTOR FUNCTIONS ===============================
efvanmarrewijk 25:ac139331fe51 379 // angleDesired now defined as angle of potmeter and not the angle as output from the kinematics
efvanmarrewijk 25:ac139331fe51 380 // So, the angleDesired needs to be defined again and the part in which the potmeter value is calculated needs to be commented
efvanmarrewijk 25:ac139331fe51 381
efvanmarrewijk 25:ac139331fe51 382 // ------------------------ General motor functions ----------------------------
efvanmarrewijk 48:1eb0f9ed6cd9 383 int countsInputL() // Gets the counts from encoder 1
efvanmarrewijk 48:1eb0f9ed6cd9 384 { countsL = encoderL.getPulses();
efvanmarrewijk 48:1eb0f9ed6cd9 385 return countsL;
efvanmarrewijk 48:1eb0f9ed6cd9 386 }
efvanmarrewijk 48:1eb0f9ed6cd9 387 int countsInputR() // Gets the counts from encoder 2
efvanmarrewijk 48:1eb0f9ed6cd9 388 { countsR = encoderR.getPulses();
efvanmarrewijk 48:1eb0f9ed6cd9 389 return countsR;
efvanmarrewijk 48:1eb0f9ed6cd9 390 }
efvanmarrewijk 48:1eb0f9ed6cd9 391 int countsInputF() // Gets the counts from encoder 3
efvanmarrewijk 48:1eb0f9ed6cd9 392 { countsF = encoderF.getPulses();
efvanmarrewijk 48:1eb0f9ed6cd9 393 return countsF;
efvanmarrewijk 48:1eb0f9ed6cd9 394 }
efvanmarrewijk 25:ac139331fe51 395
efvanmarrewijk 48:1eb0f9ed6cd9 396 float angleCurrent(float counts) // Calculates the current angle of the motor (between -2*PI to 2*PI) based on the counts from the encoder
efvanmarrewijk 48:1eb0f9ed6cd9 397 { float angle = ((float)counts*2.0f*PI)/countsRad;
efvanmarrewijk 48:1eb0f9ed6cd9 398 while (angle > 2.0f*PI)
efvanmarrewijk 48:1eb0f9ed6cd9 399 { angle = angle-2.0f*PI;
efvanmarrewijk 48:1eb0f9ed6cd9 400 }
efvanmarrewijk 48:1eb0f9ed6cd9 401 while (angle < -2.0f*PI)
efvanmarrewijk 48:1eb0f9ed6cd9 402 { angle = angle+2.0f*PI;
efvanmarrewijk 48:1eb0f9ed6cd9 403 }
efvanmarrewijk 48:1eb0f9ed6cd9 404 return angle;
efvanmarrewijk 25:ac139331fe51 405 }
efvanmarrewijk 25:ac139331fe51 406
efvanmarrewijk 48:1eb0f9ed6cd9 407 float errorCalc(float angleReference,float angleCurrent) // Calculates the error of the system, based on the current angle and the reference value
efvanmarrewijk 48:1eb0f9ed6cd9 408 { float error = angleReference - angleCurrent;
efvanmarrewijk 48:1eb0f9ed6cd9 409 return error;
efvanmarrewijk 48:1eb0f9ed6cd9 410 }
efvanmarrewijk 25:ac139331fe51 411
efvanmarrewijk 25:ac139331fe51 412 // ------------------------- MOTOR FUNCTIONS FOR MOTOR LEFT --------------------
efvanmarrewijk 48:1eb0f9ed6cd9 413 float PIDControllerL(float angleReference,float angleCurrent) // PID controller for the motors, based on the reference value and the current angle of the motor
efvanmarrewijk 48:1eb0f9ed6cd9 414 { float Kp = 19.24f;
efvanmarrewijk 48:1eb0f9ed6cd9 415 float Ki = 1.02f;
efvanmarrewijk 48:1eb0f9ed6cd9 416 float Kd = 0.827f;
efvanmarrewijk 48:1eb0f9ed6cd9 417 float error = errorCalc(angleReference,angleCurrent);
efvanmarrewijk 48:1eb0f9ed6cd9 418 static float errorIntegralL = 0.0;
efvanmarrewijk 48:1eb0f9ed6cd9 419 static float errorPreviousL = error; // initialization with this value only done once!
efvanmarrewijk 48:1eb0f9ed6cd9 420 static BiQuad PIDFilterL(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
efvanmarrewijk 48:1eb0f9ed6cd9 421 // Proportional part:
efvanmarrewijk 48:1eb0f9ed6cd9 422 float u_k = Kp * error;
efvanmarrewijk 48:1eb0f9ed6cd9 423 // Integral part
efvanmarrewijk 48:1eb0f9ed6cd9 424 errorIntegralL = errorIntegralL + error * dt;
efvanmarrewijk 48:1eb0f9ed6cd9 425 float u_i = Ki * errorIntegralL;
efvanmarrewijk 48:1eb0f9ed6cd9 426 // Derivative part
efvanmarrewijk 48:1eb0f9ed6cd9 427 float errorDerivative = (error - errorPreviousL)/dt;
efvanmarrewijk 48:1eb0f9ed6cd9 428 float errorDerivativeFiltered = PIDFilterL.step(errorDerivative);
efvanmarrewijk 48:1eb0f9ed6cd9 429 float u_d = Kd * errorDerivativeFiltered;
efvanmarrewijk 48:1eb0f9ed6cd9 430 errorPreviousL = error;
efvanmarrewijk 48:1eb0f9ed6cd9 431 // Sum all parts and return it
efvanmarrewijk 48:1eb0f9ed6cd9 432 return u_k + u_i + u_d;
efvanmarrewijk 48:1eb0f9ed6cd9 433 }
efvanmarrewijk 25:ac139331fe51 434
efvanmarrewijk 48:1eb0f9ed6cd9 435 float angleDesiredL() // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
efvanmarrewijk 48:1eb0f9ed6cd9 436 { float angle = (pot1*2.0f*PI)-PI;
efvanmarrewijk 48:1eb0f9ed6cd9 437 return angle;
efvanmarrewijk 48:1eb0f9ed6cd9 438 }
efvanmarrewijk 48:1eb0f9ed6cd9 439
efvanmarrewijk 48:1eb0f9ed6cd9 440 int countsInputCalibratedL() // Gets the counts from encoder 1
efvanmarrewijk 48:1eb0f9ed6cd9 441 { countsL = encoderL.getPulses()- countsOffsetL + countsCalibration;
efvanmarrewijk 48:1eb0f9ed6cd9 442 return countsL;
efvanmarrewijk 48:1eb0f9ed6cd9 443 }
efvanmarrewijk 48:1eb0f9ed6cd9 444
efvanmarrewijk 48:1eb0f9ed6cd9 445 void motorTurnL() // main function for movement of motor 1, all above functions with an extra tab are called
efvanmarrewijk 48:1eb0f9ed6cd9 446 {
efvanmarrewijk 48:1eb0f9ed6cd9 447 float angleReferenceL = angleDesiredL(); // insert kinematics output here instead of angleDesiredL()
efvanmarrewijk 48:1eb0f9ed6cd9 448 angleReferenceL = -angleReferenceL; // different minus sign per motor
efvanmarrewijk 48:1eb0f9ed6cd9 449 countsL = countsInputCalibratedL(); // different encoder pins per motor
efvanmarrewijk 48:1eb0f9ed6cd9 450 angleCurrentL = angleCurrent(countsL); // different minus sign per motor
efvanmarrewijk 48:1eb0f9ed6cd9 451 errorL = errorCalc(angleReferenceL,angleCurrentL); // same for every motor
efvanmarrewijk 48:1eb0f9ed6cd9 452
efvanmarrewijk 48:1eb0f9ed6cd9 453 float PIDControlL = PIDControllerL(angleReferenceL,angleCurrentL); // same for every motor
efvanmarrewijk 48:1eb0f9ed6cd9 454 pin6 = fabs(PIDControlL); // different pins for every motor
efvanmarrewijk 48:1eb0f9ed6cd9 455 pin7 = PIDControlL > 0.0f; // different pins for every motor
tverouden 41:5aecc1a27ce6 456 }
EvaKrolis 13:4ba8f63e6ff4 457
efvanmarrewijk 25:ac139331fe51 458 void calibrationL() // Partially the same as motorTurnL, only with potmeter input
efvanmarrewijk 25:ac139331fe51 459 // How it works: manually turn motor using potmeters until the robot arm touches the bookholder.
efvanmarrewijk 25:ac139331fe51 460 // This program sets the counts from the motor to the reference counts (an angle of PI/4.0)
efvanmarrewijk 25:ac139331fe51 461 // Do this for every motor and after calibrated all motors, press a button
efvanmarrewijk 48:1eb0f9ed6cd9 462 { float angleReferenceL = angleDesiredL(); // insert kinematics output here instead of angleDesiredL()
efvanmarrewijk 25:ac139331fe51 463 angleReferenceL = -angleReferenceL; // different minus sign per motor
efvanmarrewijk 48:1eb0f9ed6cd9 464 countsL = countsInputL(); // different encoder pins per motor
efvanmarrewijk 25:ac139331fe51 465 angleCurrentL = angleCurrent(countsL); // different minus sign per motor
efvanmarrewijk 25:ac139331fe51 466 errorL = errorCalc(angleReferenceL,angleCurrentL); // same for every motor
efvanmarrewijk 48:1eb0f9ed6cd9 467
efvanmarrewijk 48:1eb0f9ed6cd9 468 if (fabs(errorL) >= 0.01f)
efvanmarrewijk 48:1eb0f9ed6cd9 469 { float PIDControlL = PIDControllerL(angleReferenceL,angleCurrentL); // same for every motor
efvanmarrewijk 25:ac139331fe51 470 pin6 = fabs(PIDControlL); // different pins for every motor
efvanmarrewijk 25:ac139331fe51 471 pin7 = PIDControlL > 0.0f; // different pins for every motor
tverouden 41:5aecc1a27ce6 472 }
efvanmarrewijk 48:1eb0f9ed6cd9 473 else if (fabs(errorL) < 0.01f)
efvanmarrewijk 48:1eb0f9ed6cd9 474 { countsOffsetL = countsL;
efvanmarrewijk 48:1eb0f9ed6cd9 475 countsL = countsL - countsOffsetL + countsCalibration;
efvanmarrewijk 48:1eb0f9ed6cd9 476 //countsL = 0;
efvanmarrewijk 48:1eb0f9ed6cd9 477 pin6 = 0.0f;
efvanmarrewijk 48:1eb0f9ed6cd9 478 // BUTTON PRESS: NAAR VOLGENDE STATE
efvanmarrewijk 48:1eb0f9ed6cd9 479 }
efvanmarrewijk 25:ac139331fe51 480 }
efvanmarrewijk 25:ac139331fe51 481
efvanmarrewijk 25:ac139331fe51 482 // ------------------------ MOTOR FUNCTIONS FOR MOTOR RIGHT --------------------
efvanmarrewijk 25:ac139331fe51 483
efvanmarrewijk 25:ac139331fe51 484
efvanmarrewijk 25:ac139331fe51 485 // ------------------------- MOTOR FUNCTIONS FOR MOTOR FLIP --------------------
EvaKrolis 13:4ba8f63e6ff4 486
tverouden 2:d70795e4e0bf 487 // ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡ STATE MACHINE ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡
tverouden 42:bb43f1b67787 488 void stateMachine(void) {
tverouden 42:bb43f1b67787 489 switch (currentState) { // determine which state Odin is in
tverouden 3:9c63fc5f157e 490
tverouden 4:5ce2c8864908 491 // ========================= MOTOR CALIBRATION MODE ==========================
tverouden 42:bb43f1b67787 492 case calibratingMotors:
tverouden 4:5ce2c8864908 493 // ------------------------- initialisation --------------------------
tverouden 42:bb43f1b67787 494 if (changeState) { // when entering the state
tverouden 42:bb43f1b67787 495 pc.printf("[MODE] calibrating motors...\r\n");
tverouden 42:bb43f1b67787 496 // print current state
tverouden 42:bb43f1b67787 497 changeState = false; // stay in this state
tverouden 4:5ce2c8864908 498
tverouden 42:bb43f1b67787 499 // Actions when entering state
tverouden 42:bb43f1b67787 500 ledRed = 1; // cyan-green blinking LED
tverouden 42:bb43f1b67787 501 ledGreen = 0;
tverouden 42:bb43f1b67787 502 ledBlue = 0;
tverouden 42:bb43f1b67787 503 blinkTimer.attach(&blinkLedBlue,0.5);
tverouden 4:5ce2c8864908 504
tverouden 42:bb43f1b67787 505 }
tverouden 4:5ce2c8864908 506 // ----------------------------- action ------------------------------
tverouden 42:bb43f1b67787 507 // Actions for each loop iteration
tverouden 42:bb43f1b67787 508 /* */
EvaKrolis 45:d405103e9625 509 calibrationL();
efvanmarrewijk 48:1eb0f9ed6cd9 510 //calibrationR();
tverouden 4:5ce2c8864908 511 // --------------------------- transition ----------------------------
tverouden 42:bb43f1b67787 512 // Transition condition: when all motor errors smaller than 0.01,
tverouden 42:bb43f1b67787 513 // start calibrating EMG
EvaKrolis 47:7919857587f8 514 if (errorL < 0.01f && errorR < 0.01f
EvaKrolis 47:7919857587f8 515 && errorF < 0.01f && buttonHome == false) {
tverouden 23:e282bdb9e9b7 516
tverouden 42:bb43f1b67787 517 // Actions when leaving state
tverouden 42:bb43f1b67787 518 blinkTimer.detach();
tverouden 5:04b26b2f536a 519
tverouden 42:bb43f1b67787 520 currentState = calibratingEMG; // change to state
tverouden 42:bb43f1b67787 521 changeState = true; // next loop, switch states
tverouden 42:bb43f1b67787 522 }
tverouden 5:04b26b2f536a 523
tverouden 42:bb43f1b67787 524 break; // end case
tverouden 4:5ce2c8864908 525
tverouden 7:ef5966469621 526 // =========================== EMG CALIBRATION MODE ===========================
tverouden 42:bb43f1b67787 527 case calibratingEMG:
tverouden 4:5ce2c8864908 528 // ------------------------- initialisation --------------------------
tverouden 42:bb43f1b67787 529 if (changeState) { // when entering the state
tverouden 42:bb43f1b67787 530 pc.printf("[MODE] calibrating EMG...\r\n");
tverouden 42:bb43f1b67787 531 // print current state
tverouden 42:bb43f1b67787 532 changeState = false; // stay in this state
tverouden 4:5ce2c8864908 533
tverouden 42:bb43f1b67787 534 // Actions when entering state
tverouden 42:bb43f1b67787 535 ledRed = 1; // cyan-blue blinking LED
tverouden 42:bb43f1b67787 536 ledGreen = 0;
tverouden 42:bb43f1b67787 537 ledBlue = 0;
tverouden 42:bb43f1b67787 538 blinkTimer.attach(&blinkLedGreen,0.5);
tverouden 4:5ce2c8864908 539
tverouden 42:bb43f1b67787 540 FindMax0_timer.attach(&FindMax0,15); //Find the maximum value after 15 seconds
tverouden 42:bb43f1b67787 541 FindMax1_timer.attach(&FindMax1,15); //Find the maximum value after 15 seconds
tverouden 41:5aecc1a27ce6 542
tverouden 42:bb43f1b67787 543 EMGtransition_timer.reset();
tverouden 42:bb43f1b67787 544 EMGtransition_timer.start();
tverouden 42:bb43f1b67787 545 }
tverouden 4:5ce2c8864908 546 // ----------------------------- action ------------------------------
tverouden 42:bb43f1b67787 547 // Actions for each loop iteration
tverouden 42:bb43f1b67787 548 CalibrateEMG0(); //start emg calibration every 0.005 seconds
tverouden 42:bb43f1b67787 549 CalibrateEMG1(); //Start EMG calibration every 0.005 seconds
tverouden 42:bb43f1b67787 550 /* */
tverouden 41:5aecc1a27ce6 551
tverouden 4:5ce2c8864908 552
tverouden 4:5ce2c8864908 553 // --------------------------- transition ----------------------------
tverouden 42:bb43f1b67787 554 // Transition condition: after 20 sec in state
EvaKrolis 46:f4dcfe6652ac 555 if (EMGtransition_timer.read() > 20) {
tverouden 42:bb43f1b67787 556 // Actions when leaving state
tverouden 42:bb43f1b67787 557 blinkTimer.detach();
tverouden 5:04b26b2f536a 558
tverouden 42:bb43f1b67787 559 currentState = homing; // change to state
tverouden 42:bb43f1b67787 560 changeState = true; // next loop, switch states
tverouden 42:bb43f1b67787 561 }
tverouden 42:bb43f1b67787 562 break; // end case
tverouden 4:5ce2c8864908 563
tverouden 4:5ce2c8864908 564 // ============================== HOMING MODE ================================
tverouden 42:bb43f1b67787 565 case homing:
tverouden 41:5aecc1a27ce6 566 // ------------------------- initialisation --------------------------
tverouden 42:bb43f1b67787 567 if (changeState) { // when entering the state
tverouden 42:bb43f1b67787 568 pc.printf("[MODE] homing...\r\n");
tverouden 42:bb43f1b67787 569 // print current state
tverouden 42:bb43f1b67787 570 changeState = false; // stay in this state
tverouden 4:5ce2c8864908 571
tverouden 4:5ce2c8864908 572
tverouden 42:bb43f1b67787 573 // Actions when entering state
tverouden 42:bb43f1b67787 574 ledRed = 1; // cyan LED on
tverouden 42:bb43f1b67787 575 ledGreen = 0;
tverouden 42:bb43f1b67787 576 ledBlue = 0;
tverouden 4:5ce2c8864908 577
tverouden 42:bb43f1b67787 578 }
tverouden 4:5ce2c8864908 579 // ----------------------------- action ------------------------------
tverouden 42:bb43f1b67787 580 // Actions for each loop iteration
tverouden 42:bb43f1b67787 581 /* */
tverouden 4:5ce2c8864908 582
tverouden 41:5aecc1a27ce6 583 // --------------------------- transition ----------------------------
EvaKrolis 45:d405103e9625 584 // Transition condition #1: with button press, enter reading mode,
tverouden 42:bb43f1b67787 585 // but only when velocity == 0
EvaKrolis 47:7919857587f8 586 if (errorCalibratedL < 0.01f && errorCalibratedR < 0.01f
EvaKrolis 47:7919857587f8 587 && errorCalibratedF < 0.01f && buttonBio1 == false) {
tverouden 42:bb43f1b67787 588 // Actions when leaving state
tverouden 42:bb43f1b67787 589 /* */
tverouden 24:0abc564349e1 590
tverouden 42:bb43f1b67787 591 currentState = reading; // change to state
tverouden 42:bb43f1b67787 592 changeState = true; // next loop, switch states
tverouden 42:bb43f1b67787 593 }
EvaKrolis 45:d405103e9625 594 // Transition condition #2: with button press, enter demo mode
tverouden 42:bb43f1b67787 595 // but only when velocity == 0
EvaKrolis 47:7919857587f8 596 if (errorCalibratedL < 0.01f && errorCalibratedR < 0.01f
EvaKrolis 47:7919857587f8 597 && errorCalibratedF < 0.01f && buttonBio2 == false) {
tverouden 42:bb43f1b67787 598 // Actions when leaving state
tverouden 42:bb43f1b67787 599 /* */
tverouden 5:04b26b2f536a 600
tverouden 42:bb43f1b67787 601 currentState = demoing; // change to state
tverouden 42:bb43f1b67787 602 changeState = true; // next loop, switch states
tverouden 42:bb43f1b67787 603 }
tverouden 42:bb43f1b67787 604 break; // end case
tverouden 4:5ce2c8864908 605
tverouden 41:5aecc1a27ce6 606 // ============================== READING MODE ===============================
tverouden 42:bb43f1b67787 607 case reading:
tverouden 17:b04e1938491a 608 // ------------------------- initialisation --------------------------
tverouden 42:bb43f1b67787 609 if (changeState) { // when entering the state
tverouden 42:bb43f1b67787 610 pc.printf("[MODE] reading...\r\n");
tverouden 42:bb43f1b67787 611 // print current state
tverouden 42:bb43f1b67787 612 changeState = false; // stay in this state
tverouden 17:b04e1938491a 613
tverouden 42:bb43f1b67787 614 // Actions when entering state
tverouden 42:bb43f1b67787 615 ledRed = 1; // blue LED on
tverouden 42:bb43f1b67787 616 ledGreen = 1;
tverouden 42:bb43f1b67787 617 ledBlue = 0;
tverouden 41:5aecc1a27ce6 618
tverouden 42:bb43f1b67787 619 // TERUGKLAPPEN
tverouden 17:b04e1938491a 620
tverouden 42:bb43f1b67787 621 }
tverouden 17:b04e1938491a 622 // ----------------------------- action ------------------------------
tverouden 42:bb43f1b67787 623 // Actions for each loop iteration
tverouden 42:bb43f1b67787 624 ReadUseEMG0(); //Start the use of EMG
tverouden 42:bb43f1b67787 625 ReadUseEMG1(); //Start the use of EMG
tverouden 42:bb43f1b67787 626 /* */
tverouden 17:b04e1938491a 627
tverouden 17:b04e1938491a 628 // --------------------------- transition ----------------------------
EvaKrolis 45:d405103e9625 629 // Transition condition #1: when EMG signal detected, enter operating
tverouden 42:bb43f1b67787 630 // mode
EvaKrolis 45:d405103e9625 631 if (xMove == true && yMove == true) {
tverouden 42:bb43f1b67787 632 // Actions when leaving state
tverouden 42:bb43f1b67787 633 /* */
tverouden 24:0abc564349e1 634
EvaKrolis 45:d405103e9625 635 currentState = operating; // change to state
tverouden 42:bb43f1b67787 636 changeState = true; // next loop, switch states
tverouden 42:bb43f1b67787 637 }
tverouden 42:bb43f1b67787 638 // Transition condition #2: with button press, back to homing mode
tverouden 42:bb43f1b67787 639 if (buttonHome == false) {
tverouden 42:bb43f1b67787 640 // Actions when leaving state
tverouden 42:bb43f1b67787 641 /* */
tverouden 17:b04e1938491a 642
tverouden 42:bb43f1b67787 643 currentState = homing; // change to state
tverouden 42:bb43f1b67787 644 changeState = true; // next loop, switch states
tverouden 42:bb43f1b67787 645 }
tverouden 42:bb43f1b67787 646 break; // end case
tverouden 17:b04e1938491a 647
tverouden 4:5ce2c8864908 648 // ============================= OPERATING MODE ==============================
tverouden 42:bb43f1b67787 649 case operating:
tverouden 4:5ce2c8864908 650 // ------------------------- initialisation --------------------------
tverouden 42:bb43f1b67787 651 if (changeState) { // when entering the state
tverouden 42:bb43f1b67787 652 pc.printf("[MODE] operating...\r\n");
tverouden 42:bb43f1b67787 653 // print current state
tverouden 42:bb43f1b67787 654 changeState = false; // stay in this state
tverouden 5:04b26b2f536a 655
tverouden 42:bb43f1b67787 656 // Actions when entering state
tverouden 42:bb43f1b67787 657 ledRed = 1; // blue fast blinking LED
tverouden 42:bb43f1b67787 658 ledGreen = 1;
tverouden 42:bb43f1b67787 659 ledBlue = 1;
tverouden 42:bb43f1b67787 660 blinkTimer.attach(&blinkLedBlue,0.25);
tverouden 5:04b26b2f536a 661
tverouden 42:bb43f1b67787 662 }
tverouden 5:04b26b2f536a 663 // ----------------------------- action ------------------------------
tverouden 42:bb43f1b67787 664 // Actions for each loop iteration
tverouden 42:bb43f1b67787 665 /* */
tverouden 5:04b26b2f536a 666
tverouden 5:04b26b2f536a 667 // --------------------------- transition ----------------------------
tverouden 42:bb43f1b67787 668 // Transition condition: when path is over, back to reading mode
EvaKrolis 47:7919857587f8 669 if (errorCalibratedL < 0.01f && errorCalibratedR < 0.01f) {
tverouden 42:bb43f1b67787 670 // Actions when leaving state
tverouden 42:bb43f1b67787 671 blinkTimer.detach();
tverouden 5:04b26b2f536a 672
tverouden 42:bb43f1b67787 673 currentState = reading; // change to state
tverouden 42:bb43f1b67787 674 changeState = true; // next loop, switch states
tverouden 42:bb43f1b67787 675 }
tverouden 42:bb43f1b67787 676 break; // end case
tverouden 5:04b26b2f536a 677
tverouden 5:04b26b2f536a 678 // ============================== DEMOING MODE ===============================
tverouden 42:bb43f1b67787 679 case demoing:
tverouden 5:04b26b2f536a 680 // ------------------------- initialisation --------------------------
tverouden 42:bb43f1b67787 681 if (changeState) { // when entering the state
tverouden 42:bb43f1b67787 682 pc.printf("[MODE] demoing...\r\n");
tverouden 42:bb43f1b67787 683 // print current state
tverouden 42:bb43f1b67787 684 changeState = false; // stay in this state
tverouden 5:04b26b2f536a 685
tverouden 42:bb43f1b67787 686 // Actions when entering state
tverouden 42:bb43f1b67787 687 ledRed = 0; // yellow LED on
tverouden 42:bb43f1b67787 688 ledGreen = 0;
tverouden 42:bb43f1b67787 689 ledBlue = 1;
tverouden 5:04b26b2f536a 690
tverouden 42:bb43f1b67787 691 }
tverouden 5:04b26b2f536a 692 // ----------------------------- action ------------------------------
tverouden 42:bb43f1b67787 693 // Actions for each loop iteration
tverouden 42:bb43f1b67787 694 /* */
tverouden 42:bb43f1b67787 695 ReadUseEMG0(); //Start the use of EMG
tverouden 42:bb43f1b67787 696 ReadUseEMG1(); //Start the use of EMG
EvaKrolis 45:d405103e9625 697 kinematics();
EvaKrolis 45:d405103e9625 698 motorTurnL();
efvanmarrewijk 48:1eb0f9ed6cd9 699 //motorTurnR();
tverouden 5:04b26b2f536a 700
tverouden 5:04b26b2f536a 701 // --------------------------- transition ----------------------------
tverouden 42:bb43f1b67787 702 // Transition condition: with button press, back to homing mode
tverouden 42:bb43f1b67787 703 if (buttonHome == false) {
tverouden 42:bb43f1b67787 704 // Actions when leaving state
tverouden 42:bb43f1b67787 705 /* */
tverouden 5:04b26b2f536a 706
tverouden 42:bb43f1b67787 707 currentState = homing; // change to state
tverouden 42:bb43f1b67787 708 changeState = true; // next loop, switch states
tverouden 42:bb43f1b67787 709 }
tverouden 42:bb43f1b67787 710 break; // end case
tverouden 5:04b26b2f536a 711
tverouden 5:04b26b2f536a 712 // =============================== FAILING MODE ================================
tverouden 42:bb43f1b67787 713 case failing:
tverouden 42:bb43f1b67787 714 changeState = false; // stay in this state
tverouden 3:9c63fc5f157e 715
tverouden 42:bb43f1b67787 716 // Actions when entering state
tverouden 42:bb43f1b67787 717 ledRed = 0; // red LED on
tverouden 42:bb43f1b67787 718 ledGreen = 1;
tverouden 42:bb43f1b67787 719 ledBlue = 1;
tverouden 4:5ce2c8864908 720
tverouden 42:bb43f1b67787 721 pin3 = 0; // all motor forces to zero
tverouden 42:bb43f1b67787 722 pin5 = 0;
tverouden 42:bb43f1b67787 723 pin6 = 0;
tverouden 42:bb43f1b67787 724 exit (0); // stop all current functions
tverouden 42:bb43f1b67787 725 break; // end case
tverouden 4:5ce2c8864908 726
tverouden 4:5ce2c8864908 727 // ============================== DEFAULT MODE =================================
tverouden 42:bb43f1b67787 728 default:
tverouden 4:5ce2c8864908 729 // ---------------------------- enter failing mode -----------------------------
tverouden 42:bb43f1b67787 730 currentState = failing; // change to state
tverouden 42:bb43f1b67787 731 changeState = true; // next loop, switch states
tverouden 42:bb43f1b67787 732 // print current state
tverouden 42:bb43f1b67787 733 pc.printf("[ERROR] unknown or unimplemented state reached\r\n");
tverouden 3:9c63fc5f157e 734
tverouden 42:bb43f1b67787 735 } // end switch
tverouden 42:bb43f1b67787 736 } // end stateMachine
tverouden 3:9c63fc5f157e 737
tverouden 3:9c63fc5f157e 738
tverouden 2:d70795e4e0bf 739
tverouden 2:d70795e4e0bf 740 // ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡ MAIN LOOP ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡
tverouden 2:d70795e4e0bf 741
tverouden 42:bb43f1b67787 742 int main() {
tverouden 8:8cef1050ebd9 743 // ================================ EMERGENCY ================================
tverouden 42:bb43f1b67787 744 //If the emergency button is pressed, stop program via failing state
tverouden 42:bb43f1b67787 745 buttonEmergency.rise(stopProgram); // Automatische triggers voor failure mode? -> ook error message in andere functies plaatsen!
tverouden 15:6566c5dedeeb 746
tverouden 43:d332aa9f49e0 747 // ============================= PC-COMMUNICATION ============================
tverouden 42:bb43f1b67787 748 pc.baud(115200); // communication with terminal
tverouden 42:bb43f1b67787 749 pc.printf("\n\n[START] starting O.D.I.N\r\n");
tverouden 6:f32352bc5078 750
tverouden 43:d332aa9f49e0 751 // ============================= PIN DEFINE PERIOD ===========================
tverouden 42:bb43f1b67787 752 // If you give a period on one pin, c++ gives all pins this period
tverouden 42:bb43f1b67787 753 pin3.period_us(15);
tverouden 41:5aecc1a27ce6 754
tverouden 43:d332aa9f49e0 755 // ==================================== LOOP ==================================
tverouden 42:bb43f1b67787 756 // run state machine at 500 Hz
tverouden 42:bb43f1b67787 757 stateTimer.attach(&stateMachine,dt);
tverouden 43:d332aa9f49e0 758
tverouden 43:d332aa9f49e0 759 // =============================== ADD FILTERS ===============================
tverouden 43:d332aa9f49e0 760 //Make filter chain for the first EMG
tverouden 43:d332aa9f49e0 761 filter0.add(&Notch50_0).add(&Notch100_0).add(&Notch150_0).add(&Notch200_0).add(&Low_0).add(&High_0);
tverouden 43:d332aa9f49e0 762 //Make filter chain for the second EMG
tverouden 43:d332aa9f49e0 763 filter1.add(&Notch50_1).add(&Notch100_1).add(&Notch150_1).add(&Notch200_1).add(&Low_1).add(&High_1);
tverouden 42:bb43f1b67787 764 }