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@53:75076f9705dc, 2018-11-01 (annotated)

Committer:
efvanmarrewijk
Date:
Thu Nov 01 21:35:38 2018 +0000
Revision:
53:75076f9705dc
Parent:
52:17b3aeacb643
Child:
54:31957e8d6a73
kinematics: output of motors added

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