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@58:6660127e5d34, 2018-11-02 (annotated)

Committer:
efvanmarrewijk
Date:
Fri Nov 02 10:20:02 2018 +0000
Revision:
58:6660127e5d34
Parent:
57:099ebbb041b0 
Kinematics deleted, tried to implement EMG to motor --> doesnt work

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 pot2(A2);
efvanmarrewijk 25:ac139331fe51 29
efvanmarrewijk 25:ac139331fe51 30 // Motor pins input
efvanmarrewijk 25:ac139331fe51 31 DigitalIn pin8(D8); // Encoder L B
efvanmarrewijk 25:ac139331fe51 32 DigitalIn pin9(D9); // Encoder L A
efvanmarrewijk 25:ac139331fe51 33 DigitalIn pin10(D10); // Encoder R B
efvanmarrewijk 25:ac139331fe51 34 DigitalIn pin11(D11); // Encoder R A
efvanmarrewijk 25:ac139331fe51 35 DigitalIn pin12(D12); // Encoder F B
efvanmarrewijk 25:ac139331fe51 36 DigitalIn pin13(D13); // Encoder F A
tverouden 8:8cef1050ebd9 37
efvanmarrewijk 25:ac139331fe51 38 // Motor pins output
efvanmarrewijk 25:ac139331fe51 39 DigitalOut pin2(D2); // Motor F direction = motor flip
efvanmarrewijk 25:ac139331fe51 40 FastPWM pin3(A5); // Motor F pwm = motor flip
efvanmarrewijk 25:ac139331fe51 41 DigitalOut pin4(D4); // Motor R direction = motor right
efvanmarrewijk 25:ac139331fe51 42 FastPWM pin5(D5); // Motor R pwm = motor right
efvanmarrewijk 25:ac139331fe51 43 FastPWM pin6(D6); // Motor L pwm = motor left
tverouden 32:a00ff9898574 44 DigitalOut pin7(D7); // Motor L direction = motor left
efvanmarrewijk 25:ac139331fe51 45
efvanmarrewijk 25:ac139331fe51 46 // Utilisation of libraries
tverouden 32:a00ff9898574 47 MODSERIAL pc(USBTX, USBRX); // communication with pc
efvanmarrewijk 25:ac139331fe51 48 QEI encoderL(D9,D8,NC,4200); // Encoder input of motor L
efvanmarrewijk 25:ac139331fe51 49 QEI encoderR(D10,D11,NC,4200); // Encoder input of motor R
efvanmarrewijk 25:ac139331fe51 50 QEI encoderF(D12,D13,NC,4200); // Encoder input of motor F
efvanmarrewijk 25:ac139331fe51 51 Ticker motorL;
efvanmarrewijk 25:ac139331fe51 52 Ticker motorR;
efvanmarrewijk 25:ac139331fe51 53 Ticker motorF;
tverouden 4:5ce2c8864908 54
tverouden 0:c0c35b95765f 55 // Define & initialise state machine
efvanmarrewijk 26:247be0bea9b1 56 const float dt = 0.002f;
tverouden 50:4a7b0a3f64cb 57 enum states { calibratingMotorL, calibratingMotorR, calibratingMotorF,
tverouden 51:78c75cc72d17 58 calibratingEMG, homing, reading, operating, failing, demoing
tverouden 2:d70795e4e0bf 59 };
tverouden 51:78c75cc72d17 60
tverouden 50:4a7b0a3f64cb 61 states currentState = calibratingMotorL; // start in motor L mode
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
efvanmarrewijk 53:75076f9705dc 132 double theta1 = PI*0.40; //Angle of the left motor
efvanmarrewijk 53:75076f9705dc 133 double theta4 = PI*0.40; //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
efvanmarrewijk 55:b297e064ebd9 151 float a;
efvanmarrewijk 55:b297e064ebd9 152 float b;
efvanmarrewijk 55:b297e064ebd9 153 float c;
efvanmarrewijk 55:b297e064ebd9 154 float d;
efvanmarrewijk 55:b297e064ebd9 155
efvanmarrewijk 55:b297e064ebd9 156 float xend1;
efvanmarrewijk 55:b297e064ebd9 157 float yend1;
efvanmarrewijk 55:b297e064ebd9 158 float xend2;
efvanmarrewijk 55:b297e064ebd9 159 float yend2;
efvanmarrewijk 55:b297e064ebd9 160 float xend3;
efvanmarrewijk 55:b297e064ebd9 161 float yend3;
efvanmarrewijk 55:b297e064ebd9 162
EvaKrolis 14:2c0bf576a0e7 163 // ---------------------- Parameters for the motors ---------------------------
efvanmarrewijk 26:247be0bea9b1 164 const float countsRad = 4200.0f;
efvanmarrewijk 26:247be0bea9b1 165 int countsL;
efvanmarrewijk 26:247be0bea9b1 166 int countsR;
efvanmarrewijk 26:247be0bea9b1 167 int countsF;
efvanmarrewijk 26:247be0bea9b1 168 int countsCalibratedL;
efvanmarrewijk 26:247be0bea9b1 169 int countsCalibratedR;
efvanmarrewijk 26:247be0bea9b1 170 int countsCalibratedF;
efvanmarrewijk 48:1eb0f9ed6cd9 171 int countsOffsetL;
efvanmarrewijk 48:1eb0f9ed6cd9 172 int countsOffsetR;
efvanmarrewijk 48:1eb0f9ed6cd9 173 int countsOffsetF;
efvanmarrewijk 26:247be0bea9b1 174 float angleCurrentL;
efvanmarrewijk 26:247be0bea9b1 175 float angleCurrentR;
efvanmarrewijk 26:247be0bea9b1 176 float angleCurrentF;
efvanmarrewijk 26:247be0bea9b1 177 float errorL;
efvanmarrewijk 26:247be0bea9b1 178 float errorR;
efvanmarrewijk 26:247be0bea9b1 179 float errorF;
efvanmarrewijk 26:247be0bea9b1 180 float errorCalibratedL;
efvanmarrewijk 26:247be0bea9b1 181 float errorCalibratedR;
efvanmarrewijk 26:247be0bea9b1 182 float errorCalibratedF;
EvaKrolis 14:2c0bf576a0e7 183
efvanmarrewijk 26:247be0bea9b1 184 int countsCalibration = 4200/4;
efvanmarrewijk 25:ac139331fe51 185
tverouden 4:5ce2c8864908 186 // ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡ FUNCTIONS ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡
tverouden 6:f32352bc5078 187 // ============================ GENERAL FUNCTIONS =============================
tverouden 6:f32352bc5078 188 void stopProgram(void)
tverouden 6:f32352bc5078 189 {
tverouden 6:f32352bc5078 190 // Error message
tverouden 6:f32352bc5078 191 pc.printf("[ERROR] emergency button pressed\r\n");
tverouden 6:f32352bc5078 192 currentState = failing; // change to state
tverouden 6:f32352bc5078 193 changeState = true; // next loop, switch states
tverouden 6:f32352bc5078 194 }
tverouden 8:8cef1050ebd9 195
tverouden 15:6566c5dedeeb 196 void blinkLedRed(void)
tverouden 15:6566c5dedeeb 197 {
tverouden 15:6566c5dedeeb 198 ledRed =! ledRed; // toggle LED
tverouden 10:584b7d2c2d63 199 }
tverouden 15:6566c5dedeeb 200 void blinkLedGreen(void)
tverouden 15:6566c5dedeeb 201 {
tverouden 15:6566c5dedeeb 202 ledGreen =! ledGreen; // toggle LED
tverouden 15:6566c5dedeeb 203 }
tverouden 15:6566c5dedeeb 204 void blinkLedBlue(void)
tverouden 15:6566c5dedeeb 205 {
tverouden 15:6566c5dedeeb 206 ledBlue =! ledBlue; // toggle LED
tverouden 15:6566c5dedeeb 207 }
tverouden 15:6566c5dedeeb 208
efvanmarrewijk 53:75076f9705dc 209 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 53:75076f9705dc 210 {
efvanmarrewijk 53:75076f9705dc 211 float angle = ((float)counts*2.0f*PI)/countsRad;
efvanmarrewijk 53:75076f9705dc 212 while (angle > 2.0f*PI) {
efvanmarrewijk 53:75076f9705dc 213 angle = angle-2.0f*PI;
efvanmarrewijk 53:75076f9705dc 214 }
efvanmarrewijk 53:75076f9705dc 215 while (angle < -2.0f*PI) {
efvanmarrewijk 53:75076f9705dc 216 angle = angle+2.0f*PI;
efvanmarrewijk 53:75076f9705dc 217 }
efvanmarrewijk 53:75076f9705dc 218 return angle;
efvanmarrewijk 53:75076f9705dc 219 }
efvanmarrewijk 53:75076f9705dc 220
tverouden 4:5ce2c8864908 221 // ============================= EMG FUNCTIONS ===============================
tverouden 4:5ce2c8864908 222
EvaKrolis 13:4ba8f63e6ff4 223 //Function to read and filter the EMG
tverouden 41:5aecc1a27ce6 224 void ReadUseEMG0()
tverouden 41:5aecc1a27ce6 225 {
tverouden 41:5aecc1a27ce6 226 for(int i = Parts ; i > 0 ; i--) { //Make a first in, first out array
EvaKrolis 13:4ba8f63e6ff4 227 EMG0filtArray[i] = EMG0filtArray[i-1]; //Every value moves one up
EvaKrolis 13:4ba8f63e6ff4 228 }
tverouden 41:5aecc1a27ce6 229
EvaKrolis 13:4ba8f63e6ff4 230 Sum0 = 0;
EvaKrolis 13:4ba8f63e6ff4 231 EMG0 = EMG0In; //Save EMG input in float
EvaKrolis 13:4ba8f63e6ff4 232 EMG0filt = filter0.step(EMG0); //Filter the signal
EvaKrolis 13:4ba8f63e6ff4 233 EMG0filt = abs(EMG0filt); //Take the absolute value
EvaKrolis 13:4ba8f63e6ff4 234 EMG0filtArray[0] = EMG0filt; //Save the filtered signal on the first place in the array
tverouden 41:5aecc1a27ce6 235
tverouden 41:5aecc1a27ce6 236 for(int i = 0 ; i < Parts ; i++) { //Moving Average filter
EvaKrolis 13:4ba8f63e6ff4 237 Sum0 += EMG0filtArray[i]; //Sum the new value and the previous 49
EvaKrolis 13:4ba8f63e6ff4 238 }
EvaKrolis 13:4ba8f63e6ff4 239 EMG0Average = (float)Sum0/Parts; //Divide the sum by 50
tverouden 41:5aecc1a27ce6 240
tverouden 41:5aecc1a27ce6 241 if (EMG0Average > Threshold0) { //If the value is higher than the threshold value
EvaKrolis 13:4ba8f63e6ff4 242 xMove = true; //Set movement to true
tverouden 41:5aecc1a27ce6 243 } else {
tverouden 41:5aecc1a27ce6 244 xMove = false; //Otherwise set movement to false
EvaKrolis 13:4ba8f63e6ff4 245 }
EvaKrolis 13:4ba8f63e6ff4 246 }
EvaKrolis 13:4ba8f63e6ff4 247
EvaKrolis 13:4ba8f63e6ff4 248 //Function to read and filter the EMG
tverouden 41:5aecc1a27ce6 249 void ReadUseEMG1()
tverouden 41:5aecc1a27ce6 250 {
tverouden 41:5aecc1a27ce6 251 for(int i = Parts ; i > 0 ; i--) { //Make a first in, first out array
EvaKrolis 13:4ba8f63e6ff4 252 EMG1filtArray[i] = EMG1filtArray[i-1]; //Every value moves one up
EvaKrolis 13:4ba8f63e6ff4 253 }
tverouden 41:5aecc1a27ce6 254
EvaKrolis 13:4ba8f63e6ff4 255 Sum1 = 0;
EvaKrolis 13:4ba8f63e6ff4 256 EMG1 = EMG1In; //Save EMG input in float
EvaKrolis 13:4ba8f63e6ff4 257 EMG1filt = filter1.step(EMG1); //Filter the signal
EvaKrolis 13:4ba8f63e6ff4 258 EMG1filt = abs(EMG1filt); //Take the absolute value
EvaKrolis 13:4ba8f63e6ff4 259 EMG1filtArray[0] = EMG1filt; //Save the filtered signal on the first place in the array
tverouden 41:5aecc1a27ce6 260
tverouden 41:5aecc1a27ce6 261 for(int i = 0 ; i < Parts ; i++) { //Moving Average filter
EvaKrolis 13:4ba8f63e6ff4 262 Sum1 += EMG1filtArray[i]; //Sum the new value and the previous 49
EvaKrolis 13:4ba8f63e6ff4 263 }
EvaKrolis 13:4ba8f63e6ff4 264 EMG1Average = (float)Sum1/Parts; //Divide the sum by 50
tverouden 41:5aecc1a27ce6 265
tverouden 41:5aecc1a27ce6 266 if (EMG1Average > Threshold1) { //If the value is higher than the threshold value
EvaKrolis 13:4ba8f63e6ff4 267 yMove = true; //Set y movement to true
tverouden 41:5aecc1a27ce6 268 } else {
EvaKrolis 13:4ba8f63e6ff4 269 yMove = false; //Otherwise set y movement to false
EvaKrolis 13:4ba8f63e6ff4 270 }
EvaKrolis 13:4ba8f63e6ff4 271 }
EvaKrolis 13:4ba8f63e6ff4 272
EvaKrolis 13:4ba8f63e6ff4 273 //Function to make an array during the calibration
tverouden 41:5aecc1a27ce6 274 void CalibrateEMG0()
tverouden 41:5aecc1a27ce6 275 {
tverouden 41:5aecc1a27ce6 276 for(int i = Parts ; i > 0 ; i--) { //Make a first in, first out array
EvaKrolis 13:4ba8f63e6ff4 277 EMG0filtArray[i] = EMG0filtArray[i-1]; //Every value moves one up
EvaKrolis 13:4ba8f63e6ff4 278 }
tverouden 41:5aecc1a27ce6 279
EvaKrolis 13:4ba8f63e6ff4 280 Sum0 = 0;
EvaKrolis 13:4ba8f63e6ff4 281 EMG0 = EMG0In; //Save EMG input in float
EvaKrolis 13:4ba8f63e6ff4 282 EMG0filt = filter0.step(EMG0); //Filter the signal
EvaKrolis 13:4ba8f63e6ff4 283 EMG0filt = abs(EMG0filt); //Take the absolute value
EvaKrolis 13:4ba8f63e6ff4 284 EMG0filtArray[0] = EMG0filt; //Save the filtered signal on the first place in the array
tverouden 41:5aecc1a27ce6 285
tverouden 41:5aecc1a27ce6 286 for(int i = 0 ; i < Parts ; i++) { //Moving Average filter
EvaKrolis 13:4ba8f63e6ff4 287 Sum0 += EMG0filtArray[i]; //Sum the new value and the previous 49
EvaKrolis 13:4ba8f63e6ff4 288 }
EvaKrolis 13:4ba8f63e6ff4 289 EMG0Calibrate[ReadCal0] = (float)Sum0/Parts; //Divide the sum by 50
tverouden 41:5aecc1a27ce6 290
EvaKrolis 13:4ba8f63e6ff4 291 ReadCal0++;
EvaKrolis 13:4ba8f63e6ff4 292 }
EvaKrolis 13:4ba8f63e6ff4 293
EvaKrolis 13:4ba8f63e6ff4 294 //Function to make an array during the calibration
tverouden 41:5aecc1a27ce6 295 void CalibrateEMG1()
tverouden 41:5aecc1a27ce6 296 {
tverouden 41:5aecc1a27ce6 297 for(int i = Parts ; i > 0 ; i--) { //Make a first in, first out array
EvaKrolis 13:4ba8f63e6ff4 298 EMG1filtArray[i] = EMG1filtArray[i-1]; //Every value moves one up
EvaKrolis 13:4ba8f63e6ff4 299 }
tverouden 41:5aecc1a27ce6 300
EvaKrolis 13:4ba8f63e6ff4 301 Sum1 = 0;
EvaKrolis 13:4ba8f63e6ff4 302 EMG1 = EMG1In; //Save EMG input in float
EvaKrolis 13:4ba8f63e6ff4 303 EMG1filt = filter1.step(EMG1); //Filter the signal
EvaKrolis 13:4ba8f63e6ff4 304 EMG1filt = abs(EMG1filt); //Take the absolute value
EvaKrolis 13:4ba8f63e6ff4 305 EMG1filtArray[0] = EMG1filt; //Save the filtered signal on the first place in the array
tverouden 41:5aecc1a27ce6 306
tverouden 41:5aecc1a27ce6 307 for(int i = 0 ; i < Parts ; i++) { //Moving Average filter
EvaKrolis 13:4ba8f63e6ff4 308 Sum1 += EMG1filtArray[i]; //Sum the new value and the previous 49
EvaKrolis 13:4ba8f63e6ff4 309 }
EvaKrolis 13:4ba8f63e6ff4 310 EMG1Calibrate[ReadCal1] = (float)Sum1/Parts; //Divide the sum by 50
tverouden 41:5aecc1a27ce6 311
EvaKrolis 13:4ba8f63e6ff4 312 ReadCal1++;
EvaKrolis 13:4ba8f63e6ff4 313 }
EvaKrolis 13:4ba8f63e6ff4 314
EvaKrolis 13:4ba8f63e6ff4 315 //Function to find the max value during the calibration
tverouden 41:5aecc1a27ce6 316 void FindMax0()
tverouden 41:5aecc1a27ce6 317 {
EvaKrolis 13:4ba8f63e6ff4 318 MaxValue0 = *max_element(EMG0Calibrate+500,EMG0Calibrate+Length); //Find max value, but discard the first 100 values
EvaKrolis 13:4ba8f63e6ff4 319 Threshold0 = 0.30f*MaxValue0; //The threshold is a percentage of the max value
EvaKrolis 13:4ba8f63e6ff4 320 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 321 FindMax0_timer.detach(); //Detach the timer, so you only use this once
EvaKrolis 13:4ba8f63e6ff4 322 }
EvaKrolis 13:4ba8f63e6ff4 323
EvaKrolis 13:4ba8f63e6ff4 324 //Function to find the max value during the calibration
tverouden 41:5aecc1a27ce6 325 void FindMax1()
tverouden 41:5aecc1a27ce6 326 {
EvaKrolis 13:4ba8f63e6ff4 327 MaxValue1 = *max_element(EMG1Calibrate+500,EMG1Calibrate+Length); //Find max value, but discard the first 100 values
EvaKrolis 13:4ba8f63e6ff4 328 Threshold1 = 0.30f*MaxValue1; //The threshold is a percentage of the max value
EvaKrolis 13:4ba8f63e6ff4 329 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 330 FindMax1_timer.detach(); //Detach the timer, so you only use this once
EvaKrolis 13:4ba8f63e6ff4 331 }
tverouden 4:5ce2c8864908 332
tverouden 12:323ac4e27a0d 333 // ========================= KINEMATICS FUNCTIONS ============================
tverouden 12:323ac4e27a0d 334
efvanmarrewijk 58:6660127e5d34 335 void EMGMotorLink(){
efvanmarrewijk 58:6660127e5d34 336 if (xMove == true){
efvanmarrewijk 58:6660127e5d34 337 theta1 = theta1*0.1;
efvanmarrewijk 58:6660127e5d34 338 theta4 = -theta4*0.1;
efvanmarrewijk 58:6660127e5d34 339 }
efvanmarrewijk 58:6660127e5d34 340 if(yMove == true){
efvanmarrewijk 58:6660127e5d34 341 theta1 = -theta1*0.1;
efvanmarrewijk 58:6660127e5d34 342 theta4 = theta4*0.1;
efvanmarrewijk 58:6660127e5d34 343 }
EvaKrolis 14:2c0bf576a0e7 344 }
tverouden 12:323ac4e27a0d 345
efvanmarrewijk 25:ac139331fe51 346 // ============================ MOTOR FUNCTIONS ===============================
efvanmarrewijk 25:ac139331fe51 347 // angleDesired now defined as angle of potmeter and not the angle as output from the kinematics
efvanmarrewijk 25:ac139331fe51 348 // 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 349
efvanmarrewijk 25:ac139331fe51 350 // ------------------------ General motor functions ----------------------------
tverouden 41:5aecc1a27ce6 351 int countsInputL() // Gets the counts from encoder 1
tverouden 41:5aecc1a27ce6 352 {
tverouden 41:5aecc1a27ce6 353 countsL = encoderL.getPulses();
tverouden 41:5aecc1a27ce6 354 return countsL;
tverouden 41:5aecc1a27ce6 355 }
tverouden 41:5aecc1a27ce6 356 int countsInputR() // Gets the counts from encoder 2
tverouden 41:5aecc1a27ce6 357 {
tverouden 41:5aecc1a27ce6 358 countsR = encoderR.getPulses();
tverouden 41:5aecc1a27ce6 359 return countsR;
tverouden 41:5aecc1a27ce6 360 }
tverouden 41:5aecc1a27ce6 361 int countsInputF() // Gets the counts from encoder 3
tverouden 41:5aecc1a27ce6 362 {
tverouden 41:5aecc1a27ce6 363 countsF = encoderF.getPulses();
tverouden 41:5aecc1a27ce6 364 return countsF;
tverouden 41:5aecc1a27ce6 365 }
efvanmarrewijk 25:ac139331fe51 366
tverouden 41:5aecc1a27ce6 367 float errorCalc(float angleReference,float angleCurrent) // Calculates the error of the system, based on the current angle and the reference value
tverouden 41:5aecc1a27ce6 368 {
tverouden 41:5aecc1a27ce6 369 float error = angleReference - angleCurrent;
tverouden 41:5aecc1a27ce6 370 return error;
tverouden 41:5aecc1a27ce6 371 }
efvanmarrewijk 25:ac139331fe51 372
efvanmarrewijk 52:17b3aeacb643 373 float angleDesired() // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
efvanmarrewijk 52:17b3aeacb643 374 {
efvanmarrewijk 52:17b3aeacb643 375 float angle = (pot2*2.0f*PI)-PI;
efvanmarrewijk 52:17b3aeacb643 376 return angle;
efvanmarrewijk 52:17b3aeacb643 377 }
efvanmarrewijk 52:17b3aeacb643 378
efvanmarrewijk 25:ac139331fe51 379 // ------------------------- MOTOR FUNCTIONS FOR MOTOR LEFT --------------------
efvanmarrewijk 49:0ada5a721686 380 float PIDControllerL(float angleReference,float angleCurrent) // PID controller for the motors, based on the reference value and the current angle of the motor
tverouden 41:5aecc1a27ce6 381 {
tverouden 41:5aecc1a27ce6 382 float Kp = 19.24f;
tverouden 41:5aecc1a27ce6 383 float Ki = 1.02f;
tverouden 41:5aecc1a27ce6 384 float Kd = 0.827f;
tverouden 41:5aecc1a27ce6 385 float error = errorCalc(angleReference,angleCurrent);
tverouden 41:5aecc1a27ce6 386 static float errorIntegralL = 0.0;
tverouden 41:5aecc1a27ce6 387 static float errorPreviousL = error; // initialization with this value only done once!
tverouden 41:5aecc1a27ce6 388 static BiQuad PIDFilterL(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
tverouden 41:5aecc1a27ce6 389 // Proportional part:
tverouden 41:5aecc1a27ce6 390 float u_k = Kp * error;
tverouden 41:5aecc1a27ce6 391 // Integral part
tverouden 41:5aecc1a27ce6 392 errorIntegralL = errorIntegralL + error * dt;
tverouden 41:5aecc1a27ce6 393 float u_i = Ki * errorIntegralL;
tverouden 41:5aecc1a27ce6 394 // Derivative part
tverouden 41:5aecc1a27ce6 395 float errorDerivative = (error - errorPreviousL)/dt;
tverouden 41:5aecc1a27ce6 396 float errorDerivativeFiltered = PIDFilterL.step(errorDerivative);
tverouden 41:5aecc1a27ce6 397 float u_d = Kd * errorDerivativeFiltered;
tverouden 41:5aecc1a27ce6 398 errorPreviousL = error;
tverouden 41:5aecc1a27ce6 399 // Sum all parts and return it
tverouden 41:5aecc1a27ce6 400 return u_k + u_i + u_d;
tverouden 41:5aecc1a27ce6 401 }
efvanmarrewijk 25:ac139331fe51 402
tverouden 51:78c75cc72d17 403 int countsInputCalibratedL() // Gets the counts from encoder 1
tverouden 51:78c75cc72d17 404 {
tverouden 51:78c75cc72d17 405 countsL = encoderL.getPulses()- countsOffsetL + countsCalibration;
tverouden 51:78c75cc72d17 406 return countsL;
tverouden 51:78c75cc72d17 407 }
tverouden 41:5aecc1a27ce6 408
efvanmarrewijk 48:1eb0f9ed6cd9 409 void motorTurnL() // main function for movement of motor 1, all above functions with an extra tab are called
tverouden 41:5aecc1a27ce6 410 {
efvanmarrewijk 58:6660127e5d34 411 float angleReferenceL = theta4; // insert kinematics output here instead of angleDesired()
efvanmarrewijk 48:1eb0f9ed6cd9 412 angleReferenceL = -angleReferenceL; // different minus sign per motor
efvanmarrewijk 57:099ebbb041b0 413 countsL = countsInputL(); // different encoder pins per motor
efvanmarrewijk 48:1eb0f9ed6cd9 414 angleCurrentL = angleCurrent(countsL); // different minus sign per motor
efvanmarrewijk 48:1eb0f9ed6cd9 415 errorL = errorCalc(angleReferenceL,angleCurrentL); // same for every motor
tverouden 51:78c75cc72d17 416
efvanmarrewijk 48:1eb0f9ed6cd9 417 float PIDControlL = PIDControllerL(angleReferenceL,angleCurrentL); // same for every motor
efvanmarrewijk 48:1eb0f9ed6cd9 418 pin6 = fabs(PIDControlL); // different pins for every motor
efvanmarrewijk 48:1eb0f9ed6cd9 419 pin7 = PIDControlL > 0.0f; // different pins for every motor
tverouden 41:5aecc1a27ce6 420 }
EvaKrolis 13:4ba8f63e6ff4 421
efvanmarrewijk 25:ac139331fe51 422 void calibrationL() // Partially the same as motorTurnL, only with potmeter input
efvanmarrewijk 25:ac139331fe51 423 // How it works: manually turn motor using potmeters until the robot arm touches the bookholder.
efvanmarrewijk 25:ac139331fe51 424 // This program sets the counts from the motor to the reference counts (an angle of PI/4.0)
efvanmarrewijk 25:ac139331fe51 425 // Do this for every motor and after calibrated all motors, press a button
tverouden 41:5aecc1a27ce6 426 {
efvanmarrewijk 52:17b3aeacb643 427 float angleReferenceL = angleDesired(); // insert kinematics output here instead of angleDesired()
efvanmarrewijk 25:ac139331fe51 428 angleReferenceL = -angleReferenceL; // different minus sign per motor
efvanmarrewijk 48:1eb0f9ed6cd9 429 countsL = countsInputL(); // different encoder pins per motor
efvanmarrewijk 57:099ebbb041b0 430 countsCalibratedL = countsL;
efvanmarrewijk 25:ac139331fe51 431 angleCurrentL = angleCurrent(countsL); // different minus sign per motor
efvanmarrewijk 25:ac139331fe51 432 errorL = errorCalc(angleReferenceL,angleCurrentL); // same for every motor
tverouden 41:5aecc1a27ce6 433
efvanmarrewijk 57:099ebbb041b0 434 //if (fabs(errorL) >= 0.01f) {
tverouden 41:5aecc1a27ce6 435 float PIDControlL = PIDControllerL(angleReferenceL,angleCurrentL); // same for every motor
efvanmarrewijk 25:ac139331fe51 436 pin6 = fabs(PIDControlL); // different pins for every motor
efvanmarrewijk 25:ac139331fe51 437 pin7 = PIDControlL > 0.0f; // different pins for every motor
efvanmarrewijk 57:099ebbb041b0 438 /* } else if (fabs(errorL) < 0.01f) {
tverouden 51:78c75cc72d17 439 countsOffsetL = countsL;
tverouden 51:78c75cc72d17 440 countsL = countsL - countsOffsetL + countsCalibration;
tverouden 51:78c75cc72d17 441 //countsL = 0;
tverouden 41:5aecc1a27ce6 442 pin6 = 0.0f;
tverouden 41:5aecc1a27ce6 443 // BUTTON PRESS: TO NEXT STATE
efvanmarrewijk 57:099ebbb041b0 444 }*/
tverouden 41:5aecc1a27ce6 445 }
tverouden 41:5aecc1a27ce6 446
efvanmarrewijk 49:0ada5a721686 447 // ------------------------ MOTOR FUNCTIONS FOR MOTOR RIGHT --------------------
tverouden 41:5aecc1a27ce6 448 float PIDControllerR(float angleReference,float angleCurrent) // PID controller for the motors, based on the reference value and the current angle of the motor
tverouden 41:5aecc1a27ce6 449 {
tverouden 41:5aecc1a27ce6 450 float Kp = 19.24f;
tverouden 41:5aecc1a27ce6 451 float Ki = 1.02f;
tverouden 41:5aecc1a27ce6 452 float Kd = 0.827f;
tverouden 41:5aecc1a27ce6 453 float error = errorCalc(angleReference,angleCurrent);
tverouden 41:5aecc1a27ce6 454 static float errorIntegralR = 0.0;
tverouden 41:5aecc1a27ce6 455 static float errorPreviousR = error; // initialization with this value only done once!
tverouden 41:5aecc1a27ce6 456 static BiQuad PIDFilterR(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
tverouden 41:5aecc1a27ce6 457 // Proportional part:
tverouden 41:5aecc1a27ce6 458 float u_k = Kp * error;
tverouden 41:5aecc1a27ce6 459 // Integral part
tverouden 41:5aecc1a27ce6 460 errorIntegralR = errorIntegralR + error * dt;
tverouden 41:5aecc1a27ce6 461 float u_i = Ki * errorIntegralR;
tverouden 41:5aecc1a27ce6 462 // Derivative part
tverouden 41:5aecc1a27ce6 463 float errorDerivative = (error - errorPreviousR)/dt;
tverouden 41:5aecc1a27ce6 464 float errorDerivativeFiltered = PIDFilterR.step(errorDerivative);
tverouden 41:5aecc1a27ce6 465 float u_d = Kd * errorDerivativeFiltered;
tverouden 41:5aecc1a27ce6 466 errorPreviousR = error;
tverouden 41:5aecc1a27ce6 467 // Sum all parts and return it
tverouden 41:5aecc1a27ce6 468 return u_k + u_i + u_d;
tverouden 41:5aecc1a27ce6 469 }
efvanmarrewijk 49:0ada5a721686 470
tverouden 51:78c75cc72d17 471 int countsInputCalibratedR() // Gets the counts from encoder 1
tverouden 51:78c75cc72d17 472 {
tverouden 51:78c75cc72d17 473 countsR = encoderR.getPulses()- countsOffsetR + countsCalibration;
tverouden 51:78c75cc72d17 474 return countsR;
tverouden 51:78c75cc72d17 475 }
tverouden 41:5aecc1a27ce6 476
efvanmarrewijk 49:0ada5a721686 477 void motorTurnR() // main function for movement of motor 1, all above functions with an extra tab are called
tverouden 41:5aecc1a27ce6 478 {
efvanmarrewijk 57:099ebbb041b0 479 float angleReferenceR = theta1; // insert kinematics output here instead of angleDesired()
efvanmarrewijk 49:0ada5a721686 480 angleReferenceR = -angleReferenceR; // different minus sign per motor
efvanmarrewijk 57:099ebbb041b0 481 countsR = countsInputR(); // different encoder pins per motor
efvanmarrewijk 57:099ebbb041b0 482 countsCalibratedR = countsR;
efvanmarrewijk 49:0ada5a721686 483 angleCurrentR = angleCurrent(countsR); // different minus sign per motor
efvanmarrewijk 49:0ada5a721686 484 errorR = errorCalc(angleReferenceR,angleCurrentR); // same for every motor
tverouden 51:78c75cc72d17 485
efvanmarrewijk 49:0ada5a721686 486 float PIDControlR = PIDControllerR(angleReferenceR,angleCurrentR); // same for every motor
efvanmarrewijk 49:0ada5a721686 487 pin5 = fabs(PIDControlR); // different pins for every motor
efvanmarrewijk 49:0ada5a721686 488 pin4 = PIDControlR > 0.0f; // different pins for every motor
efvanmarrewijk 49:0ada5a721686 489 }
efvanmarrewijk 49:0ada5a721686 490
efvanmarrewijk 49:0ada5a721686 491 void calibrationR() // Partially the same as motorTurnR, only with potmeter input
efvanmarrewijk 49:0ada5a721686 492 // How it works: manually turn motor using potmeters until the robot arm touches the bookholder.
efvanmarrewijk 49:0ada5a721686 493 // This program sets the counts from the motor to the reference counts (an angle of PI/4.0)
efvanmarrewijk 49:0ada5a721686 494 // Do this for every motor and after calibrated all motors, press a button
tverouden 41:5aecc1a27ce6 495 {
efvanmarrewijk 52:17b3aeacb643 496 float angleReferenceR = angleDesired(); // insert kinematics output here instead of angleDesired()
efvanmarrewijk 49:0ada5a721686 497 angleReferenceR = -angleReferenceR; // different minus sign per motor
efvanmarrewijk 49:0ada5a721686 498 countsR = countsInputR(); // different encoder pins per motor
efvanmarrewijk 49:0ada5a721686 499 angleCurrentR = angleCurrent(countsR); // different minus sign per motor
efvanmarrewijk 49:0ada5a721686 500 errorR = errorCalc(angleReferenceR,angleCurrentR); // same for every motor
tverouden 41:5aecc1a27ce6 501
efvanmarrewijk 57:099ebbb041b0 502 //if (fabs(errorR) >= 0.01f) {
tverouden 41:5aecc1a27ce6 503 float PIDControlR = PIDControllerR(angleReferenceR,angleCurrentR); // same for every motor
efvanmarrewijk 49:0ada5a721686 504 pin5 = fabs(PIDControlR); // different pins for every motor
efvanmarrewijk 49:0ada5a721686 505 pin4 = PIDControlR > 0.0f; // different pins for every motor
efvanmarrewijk 57:099ebbb041b0 506 /*} else if (fabs(errorR) < 0.01f) {
tverouden 51:78c75cc72d17 507 countsOffsetR = countsR;
tverouden 51:78c75cc72d17 508 countsR = countsR - countsOffsetR + countsCalibration;
tverouden 51:78c75cc72d17 509 //countsR = 0;
tverouden 51:78c75cc72d17 510 pin5 = 0.0f;
tverouden 51:78c75cc72d17 511 // BUTTON PRESS: TO NEXT STATE
efvanmarrewijk 57:099ebbb041b0 512 }*/
efvanmarrewijk 25:ac139331fe51 513 }
efvanmarrewijk 25:ac139331fe51 514
efvanmarrewijk 25:ac139331fe51 515 // ------------------------- MOTOR FUNCTIONS FOR MOTOR FLIP --------------------
EvaKrolis 13:4ba8f63e6ff4 516
efvanmarrewijk 52:17b3aeacb643 517 float PIDControllerF(float angleReference,float angleCurrent) // PID controller for the motors, based on the reference value and the current angle of the motor
efvanmarrewijk 52:17b3aeacb643 518 { float Kp = 19.24f;
efvanmarrewijk 52:17b3aeacb643 519 float Ki = 1.02f;
efvanmarrewijk 52:17b3aeacb643 520 float Kd = 0.827f;
efvanmarrewijk 52:17b3aeacb643 521 float error = errorCalc(angleReference,angleCurrent);
efvanmarrewijk 52:17b3aeacb643 522 static float errorIntegralF = 0.0;
efvanmarrewijk 52:17b3aeacb643 523 static float errorPreviousF = error; // initialization with this value only done once!
efvanmarrewijk 52:17b3aeacb643 524 static BiQuad PIDFilterF(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
efvanmarrewijk 52:17b3aeacb643 525 // Proportional part:
efvanmarrewijk 52:17b3aeacb643 526 float u_k = Kp * error;
efvanmarrewijk 52:17b3aeacb643 527 // Integral part
efvanmarrewijk 52:17b3aeacb643 528 errorIntegralF = errorIntegralF + error * dt;
efvanmarrewijk 52:17b3aeacb643 529 float u_i = Ki * errorIntegralF;
efvanmarrewijk 52:17b3aeacb643 530 // Derivative part
efvanmarrewijk 52:17b3aeacb643 531 float errorDerivative = (error - errorPreviousF)/dt;
efvanmarrewijk 52:17b3aeacb643 532 float errorDerivativeFiltered = PIDFilterF.step(errorDerivative);
efvanmarrewijk 52:17b3aeacb643 533 float u_d = Kd * errorDerivativeFiltered;
efvanmarrewijk 52:17b3aeacb643 534 errorPreviousF = error;
efvanmarrewijk 52:17b3aeacb643 535 // Sum all parts and return it
efvanmarrewijk 52:17b3aeacb643 536 return u_k + u_i + u_d;
efvanmarrewijk 52:17b3aeacb643 537 }
efvanmarrewijk 52:17b3aeacb643 538
efvanmarrewijk 52:17b3aeacb643 539 int countsInputCalibratedF() // Gets the counts from encoder 1
efvanmarrewijk 52:17b3aeacb643 540 { countsF = encoderF.getPulses()- countsOffsetF + countsCalibration;
efvanmarrewijk 52:17b3aeacb643 541 return countsF;
efvanmarrewijk 52:17b3aeacb643 542 }
efvanmarrewijk 52:17b3aeacb643 543
efvanmarrewijk 52:17b3aeacb643 544 void motorTurnF() // main function for movement of motor 1, all above functions with an extra tab are called
efvanmarrewijk 52:17b3aeacb643 545 {
efvanmarrewijk 52:17b3aeacb643 546 float angleReferenceF = 0.0f; // insert kinematics output here instead of angleDesired()
efvanmarrewijk 52:17b3aeacb643 547 angleReferenceF = -angleReferenceF; // different minus sign per motor
efvanmarrewijk 52:17b3aeacb643 548 countsF = countsInputCalibratedF(); // different encoder pins per motor
efvanmarrewijk 52:17b3aeacb643 549 angleCurrentF = angleCurrent(countsF); // different minus sign per motor
efvanmarrewijk 52:17b3aeacb643 550 errorF = errorCalc(angleReferenceF,angleCurrentF); // same for every motor
efvanmarrewijk 52:17b3aeacb643 551
efvanmarrewijk 52:17b3aeacb643 552 float PIDControlF = PIDControllerF(angleReferenceF,angleCurrentF); // same for every motor
efvanmarrewijk 52:17b3aeacb643 553 pin5 = fabs(PIDControlF); // different pins for every motor
efvanmarrewijk 52:17b3aeacb643 554 pin4 = PIDControlF > 0.0f; // different pins for every motor
efvanmarrewijk 52:17b3aeacb643 555 }
efvanmarrewijk 52:17b3aeacb643 556
efvanmarrewijk 52:17b3aeacb643 557 void calibrationF() // Partially the same as motorTurnF, only with potmeter input
efvanmarrewijk 52:17b3aeacb643 558 // How it works: manually turn motor using potmeters until the robot arm touches the bookholder.
efvanmarrewijk 52:17b3aeacb643 559 // This program sets the counts from the motor to the reference counts (an angle of PI/4.0)
efvanmarrewijk 52:17b3aeacb643 560 // Do this for every motor and after calibrated all motors, press a button
efvanmarrewijk 52:17b3aeacb643 561 { float angleReferenceF = 0.0f; // insert kinematics output here instead of angleDesired()
efvanmarrewijk 52:17b3aeacb643 562 angleReferenceF = -angleReferenceF; // different minus sign per motor
efvanmarrewijk 52:17b3aeacb643 563 countsF = countsInputF(); // different encoder pins per motor
efvanmarrewijk 52:17b3aeacb643 564 angleCurrentF = angleCurrent(countsF); // different minus sign per motor
efvanmarrewijk 52:17b3aeacb643 565 errorF = errorCalc(angleReferenceF,angleCurrentF); // same for every motor
efvanmarrewijk 52:17b3aeacb643 566
efvanmarrewijk 52:17b3aeacb643 567 if (fabs(errorF) >= 0.01f)
efvanmarrewijk 52:17b3aeacb643 568 { float PIDControlF = PIDControllerF(angleReferenceF,angleCurrentF); // same for every motor
efvanmarrewijk 52:17b3aeacb643 569 pin5 = fabs(PIDControlF); // different pins for every motor
efvanmarrewijk 52:17b3aeacb643 570 pin4 = PIDControlF > 0.0f; // different pins for every motor
efvanmarrewijk 52:17b3aeacb643 571 }
efvanmarrewijk 52:17b3aeacb643 572 else if (fabs(errorF) < 0.01f)
efvanmarrewijk 52:17b3aeacb643 573 { countsOffsetF = countsF;
efvanmarrewijk 52:17b3aeacb643 574 countsF = countsF - countsOffsetF + countsCalibration;
efvanmarrewijk 52:17b3aeacb643 575 //countsF = 0;
efvanmarrewijk 52:17b3aeacb643 576 pin5 = 0.0f;
efvanmarrewijk 52:17b3aeacb643 577 // BUTTON PRESS: TO NEXT STATE
efvanmarrewijk 52:17b3aeacb643 578 }
efvanmarrewijk 52:17b3aeacb643 579 }
efvanmarrewijk 52:17b3aeacb643 580
tverouden 2:d70795e4e0bf 581 // ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡ STATE MACHINE ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡
tverouden 51:78c75cc72d17 582 void stateMachine(void)
tverouden 51:78c75cc72d17 583 {
tverouden 51:78c75cc72d17 584 switch (currentState) { // determine which state Odin is in
tverouden 4:5ce2c8864908 585
tverouden 51:78c75cc72d17 586 // ======================== MOTOR L CALIBRATION MODE =========================
tverouden 51:78c75cc72d17 587 case calibratingMotorL:
tverouden 51:78c75cc72d17 588 // ------------------------- initialisation --------------------------
tverouden 51:78c75cc72d17 589 if (changeState) { // when entering the state
tverouden 51:78c75cc72d17 590 pc.printf("[MODE] calibrating motor L...\r\n");
tverouden 51:78c75cc72d17 591 // print current state
tverouden 51:78c75cc72d17 592 changeState = false; // stay in this state
tverouden 3:9c63fc5f157e 593
tverouden 51:78c75cc72d17 594 // Actions when entering state
tverouden 51:78c75cc72d17 595 ledRed = 1; // green blinking LED
tverouden 51:78c75cc72d17 596 ledGreen = 1;
tverouden 51:78c75cc72d17 597 ledBlue = 1;
tverouden 51:78c75cc72d17 598 blinkTimer.attach(&blinkLedGreen,0.5);
tverouden 51:78c75cc72d17 599
tverouden 51:78c75cc72d17 600 }
tverouden 4:5ce2c8864908 601 // ----------------------------- action ------------------------------
tverouden 51:78c75cc72d17 602 // Actions for each loop iteration
tverouden 51:78c75cc72d17 603 calibrationL();
tverouden 4:5ce2c8864908 604
tverouden 4:5ce2c8864908 605 // --------------------------- transition ----------------------------
tverouden 51:78c75cc72d17 606 // Transition condition: when all motor errors smaller than 0.01,
tverouden 51:78c75cc72d17 607 // start calibrating EMG
efvanmarrewijk 52:17b3aeacb643 608 if (errorL < 0.01f && buttonBio1 == false) {
tverouden 51:78c75cc72d17 609
tverouden 51:78c75cc72d17 610 // Actions when leaving state
tverouden 51:78c75cc72d17 611 blinkTimer.detach();
tverouden 4:5ce2c8864908 612
tverouden 51:78c75cc72d17 613 currentState = calibratingMotorR; // change to state
tverouden 51:78c75cc72d17 614 changeState = true; // next loop, switch states
tverouden 51:78c75cc72d17 615 }
tverouden 51:78c75cc72d17 616
tverouden 51:78c75cc72d17 617 break; // end case
tverouden 4:5ce2c8864908 618
tverouden 51:78c75cc72d17 619 // ======================== MOTOR R CALIBRATION MODE =========================
tverouden 51:78c75cc72d17 620 case calibratingMotorR:
tverouden 51:78c75cc72d17 621 // ------------------------- initialisation --------------------------
tverouden 51:78c75cc72d17 622 if (changeState) { // when entering the state
tverouden 51:78c75cc72d17 623 pc.printf("[MODE] calibrating motor R...\r\n");
tverouden 51:78c75cc72d17 624 // print current state
tverouden 51:78c75cc72d17 625 changeState = false; // stay in this state
tverouden 5:04b26b2f536a 626
tverouden 51:78c75cc72d17 627 // Actions when entering state
tverouden 51:78c75cc72d17 628 ledRed = 1; // cyan-green blinking LED
tverouden 51:78c75cc72d17 629 ledGreen = 0;
tverouden 51:78c75cc72d17 630 ledBlue = 1;
tverouden 51:78c75cc72d17 631 blinkTimer.attach(&blinkLedBlue,0.5);
tverouden 5:04b26b2f536a 632
tverouden 51:78c75cc72d17 633 }
tverouden 4:5ce2c8864908 634 // ----------------------------- action ------------------------------
tverouden 51:78c75cc72d17 635 // Actions for each loop iteration
tverouden 51:78c75cc72d17 636 calibrationR();
tverouden 4:5ce2c8864908 637 // --------------------------- transition ----------------------------
tverouden 51:78c75cc72d17 638 // Transition condition: when all motor errors smaller than 0.01,
tverouden 51:78c75cc72d17 639 // start calibrating EMG
efvanmarrewijk 52:17b3aeacb643 640 if (errorR < 0.01f && buttonBio2 == false) {
tverouden 23:e282bdb9e9b7 641
tverouden 51:78c75cc72d17 642 // Actions when leaving state
tverouden 51:78c75cc72d17 643 blinkTimer.detach();
tverouden 5:04b26b2f536a 644
tverouden 51:78c75cc72d17 645 currentState = calibratingMotorF; // change to state
tverouden 51:78c75cc72d17 646 changeState = true; // next loop, switch states
tverouden 51:78c75cc72d17 647 }
tverouden 5:04b26b2f536a 648
tverouden 51:78c75cc72d17 649 break; // end case
tverouden 51:78c75cc72d17 650
tverouden 51:78c75cc72d17 651 // ======================== MOTOR F CALIBRATION MODE =========================
tverouden 51:78c75cc72d17 652 case calibratingMotorF:
tverouden 4:5ce2c8864908 653 // ------------------------- initialisation --------------------------
tverouden 51:78c75cc72d17 654 if (changeState) { // when entering the state
tverouden 51:78c75cc72d17 655 pc.printf("[MODE] calibrating motor F...\r\n");
tverouden 51:78c75cc72d17 656 // print current state
tverouden 51:78c75cc72d17 657 changeState = false; // stay in this state
tverouden 4:5ce2c8864908 658
tverouden 51:78c75cc72d17 659 // Actions when entering state
tverouden 51:78c75cc72d17 660 ledRed = 1; // green blinking LED
tverouden 51:78c75cc72d17 661 ledGreen = 1;
tverouden 51:78c75cc72d17 662 ledBlue = 1;
tverouden 51:78c75cc72d17 663 blinkTimer.attach(&blinkLedGreen,0.5);
tverouden 4:5ce2c8864908 664
tverouden 51:78c75cc72d17 665 }
tverouden 51:78c75cc72d17 666 // ----------------------------- action ------------------------------
tverouden 51:78c75cc72d17 667 // Actions for each loop iteration
tverouden 51:78c75cc72d17 668 calibrationF();
tverouden 51:78c75cc72d17 669 // --------------------------- transition ----------------------------
tverouden 51:78c75cc72d17 670 // Transition condition: when all motor errors smaller than 0.01,
tverouden 51:78c75cc72d17 671 // start calibrating EMG
tverouden 51:78c75cc72d17 672 if (errorF < 0.01f && buttonHome == false) {
tverouden 51:78c75cc72d17 673
tverouden 51:78c75cc72d17 674 // Actions when leaving state
tverouden 51:78c75cc72d17 675 blinkTimer.detach();
tverouden 4:5ce2c8864908 676
tverouden 51:78c75cc72d17 677 currentState = calibratingEMG; // change to state
tverouden 51:78c75cc72d17 678 changeState = true; // next loop, switch states
tverouden 51:78c75cc72d17 679 }
tverouden 4:5ce2c8864908 680
tverouden 51:78c75cc72d17 681 break; // end case
tverouden 51:78c75cc72d17 682
tverouden 51:78c75cc72d17 683 // =========================== EMG CALIBRATION MODE ===========================
tverouden 51:78c75cc72d17 684 case calibratingEMG:
tverouden 51:78c75cc72d17 685 // ------------------------- initialisation --------------------------
tverouden 51:78c75cc72d17 686 if (changeState) { // when entering the state
tverouden 51:78c75cc72d17 687 pc.printf("[MODE] calibrating EMG...\r\n");
tverouden 51:78c75cc72d17 688 // print current state
tverouden 51:78c75cc72d17 689 changeState = false; // stay in this state
tverouden 41:5aecc1a27ce6 690
tverouden 51:78c75cc72d17 691 // Actions when entering state
tverouden 51:78c75cc72d17 692 ledRed = 1; // cyan-blue blinking LED
tverouden 51:78c75cc72d17 693 ledGreen = 0;
tverouden 51:78c75cc72d17 694 ledBlue = 0;
tverouden 51:78c75cc72d17 695 blinkTimer.attach(&blinkLedGreen,0.5);
tverouden 51:78c75cc72d17 696
tverouden 51:78c75cc72d17 697 FindMax0_timer.attach(&FindMax0,15); //Find the maximum value after 15 seconds
tverouden 51:78c75cc72d17 698 FindMax1_timer.attach(&FindMax1,15); //Find the maximum value after 15 seconds
tverouden 51:78c75cc72d17 699
tverouden 51:78c75cc72d17 700 EMGtransition_timer.reset();
tverouden 51:78c75cc72d17 701 EMGtransition_timer.start();
tverouden 51:78c75cc72d17 702 }
tverouden 4:5ce2c8864908 703 // ----------------------------- action ------------------------------
tverouden 51:78c75cc72d17 704 // Actions for each loop iteration
tverouden 51:78c75cc72d17 705 CalibrateEMG0(); //start emg calibration every 0.005 seconds
tverouden 51:78c75cc72d17 706 CalibrateEMG1(); //Start EMG calibration every 0.005 seconds
tverouden 51:78c75cc72d17 707 /* */
tverouden 41:5aecc1a27ce6 708
tverouden 4:5ce2c8864908 709
tverouden 4:5ce2c8864908 710 // --------------------------- transition ----------------------------
tverouden 51:78c75cc72d17 711 // Transition condition: after 20 sec in state
tverouden 51:78c75cc72d17 712 if (EMGtransition_timer.read() > 20) {
tverouden 51:78c75cc72d17 713 // Actions when leaving state
tverouden 51:78c75cc72d17 714 blinkTimer.detach();
tverouden 5:04b26b2f536a 715
tverouden 51:78c75cc72d17 716 currentState = homing; // change to state
tverouden 51:78c75cc72d17 717 changeState = true; // next loop, switch states
tverouden 51:78c75cc72d17 718 }
tverouden 51:78c75cc72d17 719 break; // end case
tverouden 4:5ce2c8864908 720
tverouden 4:5ce2c8864908 721 // ============================== HOMING MODE ================================
tverouden 51:78c75cc72d17 722 case homing:
tverouden 41:5aecc1a27ce6 723 // ------------------------- initialisation --------------------------
tverouden 51:78c75cc72d17 724 if (changeState) { // when entering the state
tverouden 51:78c75cc72d17 725 pc.printf("[MODE] homing...\r\n");
tverouden 51:78c75cc72d17 726 // print current state
tverouden 51:78c75cc72d17 727 changeState = false; // stay in this state
tverouden 4:5ce2c8864908 728
tverouden 4:5ce2c8864908 729
tverouden 51:78c75cc72d17 730 // Actions when entering state
tverouden 51:78c75cc72d17 731 ledRed = 1; // cyan LED on
tverouden 51:78c75cc72d17 732 ledGreen = 0;
tverouden 51:78c75cc72d17 733 ledBlue = 0;
tverouden 4:5ce2c8864908 734
tverouden 51:78c75cc72d17 735 }
tverouden 4:5ce2c8864908 736 // ----------------------------- action ------------------------------
tverouden 51:78c75cc72d17 737 // Actions for each loop iteration
tverouden 51:78c75cc72d17 738 /* */
tverouden 4:5ce2c8864908 739
tverouden 41:5aecc1a27ce6 740 // --------------------------- transition ----------------------------
tverouden 51:78c75cc72d17 741 // Transition condition #1: with button press, enter reading mode,
tverouden 51:78c75cc72d17 742 // but only when velocity == 0
tverouden 51:78c75cc72d17 743 if (errorCalibratedL < 0.01f && errorCalibratedR < 0.01f
tverouden 51:78c75cc72d17 744 && errorCalibratedF < 0.01f && buttonBio1 == false) {
tverouden 51:78c75cc72d17 745 // Actions when leaving state
tverouden 51:78c75cc72d17 746 /* */
tverouden 24:0abc564349e1 747
tverouden 51:78c75cc72d17 748 currentState = reading; // change to state
tverouden 51:78c75cc72d17 749 changeState = true; // next loop, switch states
tverouden 51:78c75cc72d17 750 }
tverouden 51:78c75cc72d17 751 // Transition condition #2: with button press, enter demo mode
tverouden 51:78c75cc72d17 752 // but only when velocity == 0
tverouden 51:78c75cc72d17 753 if (errorCalibratedL < 0.01f && errorCalibratedR < 0.01f
tverouden 51:78c75cc72d17 754 && errorCalibratedF < 0.01f && buttonBio2 == false) {
tverouden 51:78c75cc72d17 755 // Actions when leaving state
tverouden 51:78c75cc72d17 756 /* */
tverouden 5:04b26b2f536a 757
tverouden 51:78c75cc72d17 758 currentState = demoing; // change to state
tverouden 51:78c75cc72d17 759 changeState = true; // next loop, switch states
tverouden 51:78c75cc72d17 760 }
tverouden 51:78c75cc72d17 761 break; // end case
tverouden 4:5ce2c8864908 762
tverouden 41:5aecc1a27ce6 763 // ============================== READING MODE ===============================
tverouden 51:78c75cc72d17 764 case reading:
tverouden 17:b04e1938491a 765 // ------------------------- initialisation --------------------------
tverouden 51:78c75cc72d17 766 if (changeState) { // when entering the state
tverouden 51:78c75cc72d17 767 pc.printf("[MODE] reading...\r\n");
tverouden 51:78c75cc72d17 768 // print current state
tverouden 51:78c75cc72d17 769 changeState = false; // stay in this state
tverouden 17:b04e1938491a 770
tverouden 51:78c75cc72d17 771 // Actions when entering state
tverouden 51:78c75cc72d17 772 ledRed = 1; // blue LED on
tverouden 51:78c75cc72d17 773 ledGreen = 1;
tverouden 51:78c75cc72d17 774 ledBlue = 0;
tverouden 41:5aecc1a27ce6 775
tverouden 51:78c75cc72d17 776 // TERUGKLAPPEN
tverouden 17:b04e1938491a 777
tverouden 51:78c75cc72d17 778 }
tverouden 17:b04e1938491a 779 // ----------------------------- action ------------------------------
tverouden 51:78c75cc72d17 780 // Actions for each loop iteration
tverouden 51:78c75cc72d17 781 ReadUseEMG0(); //Start the use of EMG
tverouden 51:78c75cc72d17 782 ReadUseEMG1(); //Start the use of EMG
tverouden 51:78c75cc72d17 783 /* */
tverouden 17:b04e1938491a 784
tverouden 17:b04e1938491a 785 // --------------------------- transition ----------------------------
tverouden 51:78c75cc72d17 786 // Transition condition #1: when EMG signal detected, enter operating
tverouden 51:78c75cc72d17 787 // mode
tverouden 51:78c75cc72d17 788 if (xMove == true && yMove == true) {
tverouden 51:78c75cc72d17 789 // Actions when leaving state
tverouden 51:78c75cc72d17 790 /* */
tverouden 24:0abc564349e1 791
tverouden 51:78c75cc72d17 792 currentState = operating; // change to state
tverouden 51:78c75cc72d17 793 changeState = true; // next loop, switch states
tverouden 51:78c75cc72d17 794 }
tverouden 51:78c75cc72d17 795 // Transition condition #2: with button press, back to homing mode
tverouden 51:78c75cc72d17 796 if (buttonHome == false) {
tverouden 51:78c75cc72d17 797 // Actions when leaving state
tverouden 51:78c75cc72d17 798 /* */
tverouden 17:b04e1938491a 799
tverouden 51:78c75cc72d17 800 currentState = homing; // change to state
tverouden 51:78c75cc72d17 801 changeState = true; // next loop, switch states
tverouden 51:78c75cc72d17 802 }
tverouden 51:78c75cc72d17 803 break; // end case
tverouden 17:b04e1938491a 804
tverouden 4:5ce2c8864908 805 // ============================= OPERATING MODE ==============================
tverouden 51:78c75cc72d17 806 case operating:
tverouden 4:5ce2c8864908 807 // ------------------------- initialisation --------------------------
tverouden 51:78c75cc72d17 808 if (changeState) { // when entering the state
tverouden 51:78c75cc72d17 809 pc.printf("[MODE] operating...\r\n");
tverouden 51:78c75cc72d17 810 // print current state
tverouden 51:78c75cc72d17 811 changeState = false; // stay in this state
tverouden 5:04b26b2f536a 812
tverouden 51:78c75cc72d17 813 // Actions when entering state
tverouden 51:78c75cc72d17 814 ledRed = 1; // blue fast blinking LED
tverouden 51:78c75cc72d17 815 ledGreen = 1;
tverouden 51:78c75cc72d17 816 ledBlue = 1;
tverouden 51:78c75cc72d17 817 blinkTimer.attach(&blinkLedBlue,0.25);
tverouden 5:04b26b2f536a 818
tverouden 51:78c75cc72d17 819 }
tverouden 5:04b26b2f536a 820 // ----------------------------- action ------------------------------
tverouden 51:78c75cc72d17 821 // Actions for each loop iteration
tverouden 51:78c75cc72d17 822 /* */
tverouden 5:04b26b2f536a 823
tverouden 5:04b26b2f536a 824 // --------------------------- transition ----------------------------
tverouden 51:78c75cc72d17 825 // Transition condition: when path is over, back to reading mode
tverouden 51:78c75cc72d17 826 if (errorCalibratedL < 0.01f && errorCalibratedR < 0.01f) {
tverouden 51:78c75cc72d17 827 // Actions when leaving state
tverouden 51:78c75cc72d17 828 blinkTimer.detach();
tverouden 5:04b26b2f536a 829
tverouden 51:78c75cc72d17 830 currentState = reading; // change to state
tverouden 51:78c75cc72d17 831 changeState = true; // next loop, switch states
tverouden 51:78c75cc72d17 832 }
tverouden 51:78c75cc72d17 833 break; // end case
tverouden 5:04b26b2f536a 834
tverouden 5:04b26b2f536a 835 // ============================== DEMOING MODE ===============================
tverouden 51:78c75cc72d17 836 case demoing:
tverouden 5:04b26b2f536a 837 // ------------------------- initialisation --------------------------
tverouden 51:78c75cc72d17 838 if (changeState) { // when entering the state
tverouden 51:78c75cc72d17 839 pc.printf("[MODE] demoing...\r\n");
tverouden 51:78c75cc72d17 840 // print current state
tverouden 51:78c75cc72d17 841 changeState = false; // stay in this state
tverouden 5:04b26b2f536a 842
tverouden 51:78c75cc72d17 843 // Actions when entering state
tverouden 51:78c75cc72d17 844 ledRed = 0; // yellow LED on
tverouden 51:78c75cc72d17 845 ledGreen = 0;
tverouden 51:78c75cc72d17 846 ledBlue = 1;
tverouden 5:04b26b2f536a 847
tverouden 51:78c75cc72d17 848 }
tverouden 5:04b26b2f536a 849 // ----------------------------- action ------------------------------
tverouden 51:78c75cc72d17 850 // Actions for each loop iteration
tverouden 51:78c75cc72d17 851 /* */
efvanmarrewijk 57:099ebbb041b0 852
tverouden 51:78c75cc72d17 853 ReadUseEMG0(); //Start the use of EMG
tverouden 51:78c75cc72d17 854 ReadUseEMG1(); //Start the use of EMG
efvanmarrewijk 58:6660127e5d34 855 //pc.printf("theta1 and 4: %f \t\t %f \n\r",theta1,theta4);
efvanmarrewijk 58:6660127e5d34 856 //kinematics();
efvanmarrewijk 58:6660127e5d34 857 motorTurnR();
efvanmarrewijk 58:6660127e5d34 858 motorTurnL();
efvanmarrewijk 58:6660127e5d34 859 EMGMotorLink();
efvanmarrewijk 52:17b3aeacb643 860
tverouden 5:04b26b2f536a 861
tverouden 5:04b26b2f536a 862 // --------------------------- transition ----------------------------
tverouden 51:78c75cc72d17 863 // Transition condition: with button press, back to homing mode
tverouden 51:78c75cc72d17 864 if (buttonHome == false) {
tverouden 51:78c75cc72d17 865 // Actions when leaving state
tverouden 51:78c75cc72d17 866 /* */
tverouden 5:04b26b2f536a 867
tverouden 51:78c75cc72d17 868 currentState = homing; // change to state
tverouden 51:78c75cc72d17 869 changeState = true; // next loop, switch states
tverouden 51:78c75cc72d17 870 }
tverouden 51:78c75cc72d17 871 break; // end case
tverouden 5:04b26b2f536a 872
tverouden 5:04b26b2f536a 873 // =============================== FAILING MODE ================================
tverouden 51:78c75cc72d17 874 case failing:
tverouden 51:78c75cc72d17 875 changeState = false; // stay in this state
tverouden 3:9c63fc5f157e 876
tverouden 51:78c75cc72d17 877 // Actions when entering state
tverouden 51:78c75cc72d17 878 ledRed = 0; // red LED on
tverouden 51:78c75cc72d17 879 ledGreen = 1;
tverouden 51:78c75cc72d17 880 ledBlue = 1;
tverouden 4:5ce2c8864908 881
tverouden 51:78c75cc72d17 882 pin3 = 0; // all motor forces to zero
tverouden 51:78c75cc72d17 883 pin5 = 0;
tverouden 51:78c75cc72d17 884 pin6 = 0;
tverouden 51:78c75cc72d17 885 exit (0); // stop all current functions
tverouden 51:78c75cc72d17 886 break; // end case
tverouden 4:5ce2c8864908 887
tverouden 4:5ce2c8864908 888 // ============================== DEFAULT MODE =================================
tverouden 51:78c75cc72d17 889 default:
tverouden 4:5ce2c8864908 890 // ---------------------------- enter failing mode -----------------------------
tverouden 51:78c75cc72d17 891 currentState = failing; // change to state
tverouden 51:78c75cc72d17 892 changeState = true; // next loop, switch states
tverouden 51:78c75cc72d17 893 // print current state
tverouden 51:78c75cc72d17 894 pc.printf("[ERROR] unknown or unimplemented state reached\r\n");
tverouden 3:9c63fc5f157e 895
tverouden 51:78c75cc72d17 896 } // end switch
tverouden 51:78c75cc72d17 897 } // end stateMachine
tverouden 3:9c63fc5f157e 898
tverouden 3:9c63fc5f157e 899
tverouden 2:d70795e4e0bf 900
tverouden 2:d70795e4e0bf 901 // ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡ MAIN LOOP ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡
tverouden 2:d70795e4e0bf 902
tverouden 51:78c75cc72d17 903 int main()
tverouden 51:78c75cc72d17 904 {
tverouden 8:8cef1050ebd9 905 // ================================ EMERGENCY ================================
tverouden 51:78c75cc72d17 906 //If the emergency button is pressed, stop program via failing state
tverouden 51:78c75cc72d17 907 buttonEmergency.rise(stopProgram); // Automatische triggers voor failure mode? -> ook error message in andere functies plaatsen!
tverouden 15:6566c5dedeeb 908
tverouden 43:d332aa9f49e0 909 // ============================= PC-COMMUNICATION ============================
tverouden 51:78c75cc72d17 910 pc.baud(115200); // communication with terminal
tverouden 51:78c75cc72d17 911 pc.printf("\n\n[START] starting O.D.I.N\r\n");
tverouden 6:f32352bc5078 912
tverouden 43:d332aa9f49e0 913 // ============================= PIN DEFINE PERIOD ===========================
tverouden 51:78c75cc72d17 914 // If you give a period on one pin, c++ gives all pins this period
tverouden 51:78c75cc72d17 915 pin3.period_us(15);
tverouden 41:5aecc1a27ce6 916
tverouden 43:d332aa9f49e0 917 // ==================================== LOOP ==================================
tverouden 51:78c75cc72d17 918 // run state machine at 500 Hz
tverouden 51:78c75cc72d17 919 stateTimer.attach(&stateMachine,dt);
tverouden 43:d332aa9f49e0 920
tverouden 43:d332aa9f49e0 921 // =============================== ADD FILTERS ===============================
tverouden 51:78c75cc72d17 922 //Make filter chain for the first EMG
tverouden 51:78c75cc72d17 923 filter0.add(&Notch50_0).add(&Notch100_0).add(&Notch150_0).add(&Notch200_0).add(&Low_0).add(&High_0);
tverouden 51:78c75cc72d17 924 //Make filter chain for the second EMG
tverouden 51:78c75cc72d17 925 filter1.add(&Notch50_1).add(&Notch100_1).add(&Notch150_1).add(&Notch200_1).add(&Low_1).add(&High_1);
tverouden 51:78c75cc72d17 926 }