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@50:4a7b0a3f64cb, 2018-11-01 (annotated)

Committer:
tverouden
Date:
Thu Nov 01 20:22:46 2018 +0000
Revision:
50:4a7b0a3f64cb
Parent:
44:ca74d11a2dac
Child:
51:78c75cc72d17
Merge

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