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@57:099ebbb041b0, 2018-11-02 (annotated)

Committer:
efvanmarrewijk
Date:
Fri Nov 02 10:13:33 2018 +0000
Revision:
57:099ebbb041b0
Parent:
55:b297e064ebd9
Child:
58:6660127e5d34
Child:
59:c90348c81ac4
Motor works in calibration mode, kinematics+EMG works in demo mode, with output of theta1 and theta4

Who changed what in which revision?

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