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@56:e8d568d455a0, 2018-11-02 (annotated)

Committer:
efvanmarrewijk
Date:
Fri Nov 02 09:23:28 2018 +0000
Revision:
56:e8d568d455a0
Parent:
55:b297e064ebd9 
Values of theta do not change, motors move around and around

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 48:1eb0f9ed6cd9 509 countsL = countsInputCalibratedL(); // 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 25:ac139331fe51 526 angleCurrentL = angleCurrent(countsL); // different minus sign per motor
efvanmarrewijk 25:ac139331fe51 527 errorL = errorCalc(angleReferenceL,angleCurrentL); // same for every motor
tverouden 41:5aecc1a27ce6 528
tverouden 41:5aecc1a27ce6 529 if (fabs(errorL) >= 0.01f) {
tverouden 41:5aecc1a27ce6 530 float PIDControlL = PIDControllerL(angleReferenceL,angleCurrentL); // same for every motor
efvanmarrewijk 25:ac139331fe51 531 pin6 = fabs(PIDControlL); // different pins for every motor
efvanmarrewijk 25:ac139331fe51 532 pin7 = PIDControlL > 0.0f; // different pins for every motor
tverouden 41:5aecc1a27ce6 533 } else if (fabs(errorL) < 0.01f) {
tverouden 51:78c75cc72d17 534 countsOffsetL = countsL;
efvanmarrewijk 56:e8d568d455a0 535 //countsL = countsL - countsOffsetL + countsCalibration;
tverouden 51:78c75cc72d17 536 //countsL = 0;
tverouden 41:5aecc1a27ce6 537 pin6 = 0.0f;
tverouden 41:5aecc1a27ce6 538 // BUTTON PRESS: TO NEXT STATE
tverouden 41:5aecc1a27ce6 539 }
tverouden 41:5aecc1a27ce6 540 }
tverouden 41:5aecc1a27ce6 541
efvanmarrewijk 49:0ada5a721686 542 // ------------------------ MOTOR FUNCTIONS FOR MOTOR RIGHT --------------------
tverouden 41:5aecc1a27ce6 543 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 544 {
tverouden 41:5aecc1a27ce6 545 float Kp = 19.24f;
tverouden 41:5aecc1a27ce6 546 float Ki = 1.02f;
tverouden 41:5aecc1a27ce6 547 float Kd = 0.827f;
tverouden 41:5aecc1a27ce6 548 float error = errorCalc(angleReference,angleCurrent);
tverouden 41:5aecc1a27ce6 549 static float errorIntegralR = 0.0;
tverouden 41:5aecc1a27ce6 550 static float errorPreviousR = error; // initialization with this value only done once!
tverouden 41:5aecc1a27ce6 551 static BiQuad PIDFilterR(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
tverouden 41:5aecc1a27ce6 552 // Proportional part:
tverouden 41:5aecc1a27ce6 553 float u_k = Kp * error;
tverouden 41:5aecc1a27ce6 554 // Integral part
tverouden 41:5aecc1a27ce6 555 errorIntegralR = errorIntegralR + error * dt;
tverouden 41:5aecc1a27ce6 556 float u_i = Ki * errorIntegralR;
tverouden 41:5aecc1a27ce6 557 // Derivative part
tverouden 41:5aecc1a27ce6 558 float errorDerivative = (error - errorPreviousR)/dt;
tverouden 41:5aecc1a27ce6 559 float errorDerivativeFiltered = PIDFilterR.step(errorDerivative);
tverouden 41:5aecc1a27ce6 560 float u_d = Kd * errorDerivativeFiltered;
tverouden 41:5aecc1a27ce6 561 errorPreviousR = error;
tverouden 41:5aecc1a27ce6 562 // Sum all parts and return it
tverouden 41:5aecc1a27ce6 563 return u_k + u_i + u_d;
tverouden 41:5aecc1a27ce6 564 }
efvanmarrewijk 49:0ada5a721686 565
tverouden 51:78c75cc72d17 566 int countsInputCalibratedR() // Gets the counts from encoder 1
tverouden 51:78c75cc72d17 567 {
tverouden 51:78c75cc72d17 568 countsR = encoderR.getPulses()- countsOffsetR + countsCalibration;
tverouden 51:78c75cc72d17 569 return countsR;
tverouden 51:78c75cc72d17 570 }
tverouden 41:5aecc1a27ce6 571
efvanmarrewijk 49:0ada5a721686 572 void motorTurnR() // main function for movement of motor 1, all above functions with an extra tab are called
tverouden 41:5aecc1a27ce6 573 {
efvanmarrewijk 55:b297e064ebd9 574 float angleReferenceR = angleDesired(); // insert kinematics output here instead of angleDesired()
efvanmarrewijk 49:0ada5a721686 575 angleReferenceR = -angleReferenceR; // different minus sign per motor
efvanmarrewijk 49:0ada5a721686 576 countsR = countsInputCalibratedR(); // different encoder pins per motor
efvanmarrewijk 49:0ada5a721686 577 angleCurrentR = angleCurrent(countsR); // different minus sign per motor
efvanmarrewijk 49:0ada5a721686 578 errorR = errorCalc(angleReferenceR,angleCurrentR); // same for every motor
tverouden 51:78c75cc72d17 579
efvanmarrewijk 49:0ada5a721686 580 float PIDControlR = PIDControllerR(angleReferenceR,angleCurrentR); // same for every motor
efvanmarrewijk 49:0ada5a721686 581 pin5 = fabs(PIDControlR); // different pins for every motor
efvanmarrewijk 49:0ada5a721686 582 pin4 = PIDControlR > 0.0f; // different pins for every motor
efvanmarrewijk 49:0ada5a721686 583 }
efvanmarrewijk 49:0ada5a721686 584
efvanmarrewijk 49:0ada5a721686 585 void calibrationR() // Partially the same as motorTurnR, only with potmeter input
efvanmarrewijk 49:0ada5a721686 586 // How it works: manually turn motor using potmeters until the robot arm touches the bookholder.
efvanmarrewijk 49:0ada5a721686 587 // This program sets the counts from the motor to the reference counts (an angle of PI/4.0)
efvanmarrewijk 49:0ada5a721686 588 // Do this for every motor and after calibrated all motors, press a button
tverouden 41:5aecc1a27ce6 589 {
efvanmarrewijk 52:17b3aeacb643 590 float angleReferenceR = angleDesired(); // insert kinematics output here instead of angleDesired()
efvanmarrewijk 49:0ada5a721686 591 angleReferenceR = -angleReferenceR; // different minus sign per motor
efvanmarrewijk 49:0ada5a721686 592 countsR = countsInputR(); // different encoder pins per motor
efvanmarrewijk 49:0ada5a721686 593 angleCurrentR = angleCurrent(countsR); // different minus sign per motor
efvanmarrewijk 49:0ada5a721686 594 errorR = errorCalc(angleReferenceR,angleCurrentR); // same for every motor
tverouden 41:5aecc1a27ce6 595
tverouden 41:5aecc1a27ce6 596 if (fabs(errorR) >= 0.01f) {
tverouden 41:5aecc1a27ce6 597 float PIDControlR = PIDControllerR(angleReferenceR,angleCurrentR); // same for every motor
efvanmarrewijk 49:0ada5a721686 598 pin5 = fabs(PIDControlR); // different pins for every motor
efvanmarrewijk 49:0ada5a721686 599 pin4 = PIDControlR > 0.0f; // different pins for every motor
tverouden 41:5aecc1a27ce6 600 } else if (fabs(errorR) < 0.01f) {
tverouden 51:78c75cc72d17 601 countsOffsetR = countsR;
efvanmarrewijk 56:e8d568d455a0 602 //countsR = countsR - countsOffsetR + countsCalibration;
tverouden 51:78c75cc72d17 603 //countsR = 0;
tverouden 51:78c75cc72d17 604 pin5 = 0.0f;
tverouden 51:78c75cc72d17 605 // BUTTON PRESS: TO NEXT STATE
efvanmarrewijk 49:0ada5a721686 606 }
efvanmarrewijk 25:ac139331fe51 607 }
efvanmarrewijk 25:ac139331fe51 608
efvanmarrewijk 25:ac139331fe51 609 // ------------------------- MOTOR FUNCTIONS FOR MOTOR FLIP --------------------
EvaKrolis 13:4ba8f63e6ff4 610
efvanmarrewijk 52:17b3aeacb643 611 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 612 { float Kp = 19.24f;
efvanmarrewijk 52:17b3aeacb643 613 float Ki = 1.02f;
efvanmarrewijk 52:17b3aeacb643 614 float Kd = 0.827f;
efvanmarrewijk 52:17b3aeacb643 615 float error = errorCalc(angleReference,angleCurrent);
efvanmarrewijk 52:17b3aeacb643 616 static float errorIntegralF = 0.0;
efvanmarrewijk 52:17b3aeacb643 617 static float errorPreviousF = error; // initialization with this value only done once!
efvanmarrewijk 52:17b3aeacb643 618 static BiQuad PIDFilterF(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
efvanmarrewijk 52:17b3aeacb643 619 // Proportional part:
efvanmarrewijk 52:17b3aeacb643 620 float u_k = Kp * error;
efvanmarrewijk 52:17b3aeacb643 621 // Integral part
efvanmarrewijk 52:17b3aeacb643 622 errorIntegralF = errorIntegralF + error * dt;
efvanmarrewijk 52:17b3aeacb643 623 float u_i = Ki * errorIntegralF;
efvanmarrewijk 52:17b3aeacb643 624 // Derivative part
efvanmarrewijk 52:17b3aeacb643 625 float errorDerivative = (error - errorPreviousF)/dt;
efvanmarrewijk 52:17b3aeacb643 626 float errorDerivativeFiltered = PIDFilterF.step(errorDerivative);
efvanmarrewijk 52:17b3aeacb643 627 float u_d = Kd * errorDerivativeFiltered;
efvanmarrewijk 52:17b3aeacb643 628 errorPreviousF = error;
efvanmarrewijk 52:17b3aeacb643 629 // Sum all parts and return it
efvanmarrewijk 52:17b3aeacb643 630 return u_k + u_i + u_d;
efvanmarrewijk 52:17b3aeacb643 631 }
efvanmarrewijk 52:17b3aeacb643 632
efvanmarrewijk 52:17b3aeacb643 633 int countsInputCalibratedF() // Gets the counts from encoder 1
efvanmarrewijk 52:17b3aeacb643 634 { countsF = encoderF.getPulses()- countsOffsetF + countsCalibration;
efvanmarrewijk 52:17b3aeacb643 635 return countsF;
efvanmarrewijk 52:17b3aeacb643 636 }
efvanmarrewijk 52:17b3aeacb643 637
efvanmarrewijk 52:17b3aeacb643 638 void motorTurnF() // main function for movement of motor 1, all above functions with an extra tab are called
efvanmarrewijk 52:17b3aeacb643 639 {
efvanmarrewijk 52:17b3aeacb643 640 float angleReferenceF = 0.0f; // insert kinematics output here instead of angleDesired()
efvanmarrewijk 52:17b3aeacb643 641 angleReferenceF = -angleReferenceF; // different minus sign per motor
efvanmarrewijk 52:17b3aeacb643 642 countsF = countsInputCalibratedF(); // different encoder pins per motor
efvanmarrewijk 52:17b3aeacb643 643 angleCurrentF = angleCurrent(countsF); // different minus sign per motor
efvanmarrewijk 52:17b3aeacb643 644 errorF = errorCalc(angleReferenceF,angleCurrentF); // same for every motor
efvanmarrewijk 52:17b3aeacb643 645
efvanmarrewijk 52:17b3aeacb643 646 float PIDControlF = PIDControllerF(angleReferenceF,angleCurrentF); // same for every motor
efvanmarrewijk 52:17b3aeacb643 647 pin5 = fabs(PIDControlF); // different pins for every motor
efvanmarrewijk 52:17b3aeacb643 648 pin4 = PIDControlF > 0.0f; // different pins for every motor
efvanmarrewijk 52:17b3aeacb643 649 }
efvanmarrewijk 52:17b3aeacb643 650
efvanmarrewijk 52:17b3aeacb643 651 void calibrationF() // Partially the same as motorTurnF, only with potmeter input
efvanmarrewijk 52:17b3aeacb643 652 // How it works: manually turn motor using potmeters until the robot arm touches the bookholder.
efvanmarrewijk 52:17b3aeacb643 653 // This program sets the counts from the motor to the reference counts (an angle of PI/4.0)
efvanmarrewijk 52:17b3aeacb643 654 // Do this for every motor and after calibrated all motors, press a button
efvanmarrewijk 52:17b3aeacb643 655 { float angleReferenceF = 0.0f; // insert kinematics output here instead of angleDesired()
efvanmarrewijk 52:17b3aeacb643 656 angleReferenceF = -angleReferenceF; // different minus sign per motor
efvanmarrewijk 52:17b3aeacb643 657 countsF = countsInputF(); // different encoder pins per motor
efvanmarrewijk 52:17b3aeacb643 658 angleCurrentF = angleCurrent(countsF); // different minus sign per motor
efvanmarrewijk 52:17b3aeacb643 659 errorF = errorCalc(angleReferenceF,angleCurrentF); // same for every motor
efvanmarrewijk 52:17b3aeacb643 660
efvanmarrewijk 52:17b3aeacb643 661 if (fabs(errorF) >= 0.01f)
efvanmarrewijk 52:17b3aeacb643 662 { float PIDControlF = PIDControllerF(angleReferenceF,angleCurrentF); // same for every motor
efvanmarrewijk 52:17b3aeacb643 663 pin5 = fabs(PIDControlF); // different pins for every motor
efvanmarrewijk 52:17b3aeacb643 664 pin4 = PIDControlF > 0.0f; // different pins for every motor
efvanmarrewijk 52:17b3aeacb643 665 }
efvanmarrewijk 52:17b3aeacb643 666 else if (fabs(errorF) < 0.01f)
efvanmarrewijk 52:17b3aeacb643 667 { countsOffsetF = countsF;
efvanmarrewijk 52:17b3aeacb643 668 countsF = countsF - countsOffsetF + countsCalibration;
efvanmarrewijk 52:17b3aeacb643 669 //countsF = 0;
efvanmarrewijk 52:17b3aeacb643 670 pin5 = 0.0f;
efvanmarrewijk 52:17b3aeacb643 671 // BUTTON PRESS: TO NEXT STATE
efvanmarrewijk 52:17b3aeacb643 672 }
efvanmarrewijk 52:17b3aeacb643 673 }
efvanmarrewijk 52:17b3aeacb643 674
tverouden 2:d70795e4e0bf 675 // ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡ STATE MACHINE ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡
tverouden 51:78c75cc72d17 676 void stateMachine(void)
tverouden 51:78c75cc72d17 677 {
tverouden 51:78c75cc72d17 678 switch (currentState) { // determine which state Odin is in
tverouden 4:5ce2c8864908 679
tverouden 51:78c75cc72d17 680 // ======================== MOTOR L CALIBRATION MODE =========================
tverouden 51:78c75cc72d17 681 case calibratingMotorL:
tverouden 51:78c75cc72d17 682 // ------------------------- initialisation --------------------------
tverouden 51:78c75cc72d17 683 if (changeState) { // when entering the state
tverouden 51:78c75cc72d17 684 pc.printf("[MODE] calibrating motor L...\r\n");
tverouden 51:78c75cc72d17 685 // print current state
tverouden 51:78c75cc72d17 686 changeState = false; // stay in this state
tverouden 3:9c63fc5f157e 687
tverouden 51:78c75cc72d17 688 // Actions when entering state
tverouden 51:78c75cc72d17 689 ledRed = 1; // green blinking LED
tverouden 51:78c75cc72d17 690 ledGreen = 1;
tverouden 51:78c75cc72d17 691 ledBlue = 1;
tverouden 51:78c75cc72d17 692 blinkTimer.attach(&blinkLedGreen,0.5);
tverouden 51:78c75cc72d17 693
tverouden 51:78c75cc72d17 694 }
tverouden 4:5ce2c8864908 695 // ----------------------------- action ------------------------------
tverouden 51:78c75cc72d17 696 // Actions for each loop iteration
tverouden 51:78c75cc72d17 697 calibrationL();
tverouden 4:5ce2c8864908 698
tverouden 4:5ce2c8864908 699 // --------------------------- transition ----------------------------
tverouden 51:78c75cc72d17 700 // Transition condition: when all motor errors smaller than 0.01,
tverouden 51:78c75cc72d17 701 // start calibrating EMG
efvanmarrewijk 52:17b3aeacb643 702 if (errorL < 0.01f && buttonBio1 == false) {
tverouden 51:78c75cc72d17 703
tverouden 51:78c75cc72d17 704 // Actions when leaving state
tverouden 51:78c75cc72d17 705 blinkTimer.detach();
tverouden 4:5ce2c8864908 706
tverouden 51:78c75cc72d17 707 currentState = calibratingMotorR; // change to state
tverouden 51:78c75cc72d17 708 changeState = true; // next loop, switch states
tverouden 51:78c75cc72d17 709 }
tverouden 51:78c75cc72d17 710
tverouden 51:78c75cc72d17 711 break; // end case
tverouden 4:5ce2c8864908 712
tverouden 51:78c75cc72d17 713 // ======================== MOTOR R CALIBRATION MODE =========================
tverouden 51:78c75cc72d17 714 case calibratingMotorR:
tverouden 51:78c75cc72d17 715 // ------------------------- initialisation --------------------------
tverouden 51:78c75cc72d17 716 if (changeState) { // when entering the state
tverouden 51:78c75cc72d17 717 pc.printf("[MODE] calibrating motor R...\r\n");
tverouden 51:78c75cc72d17 718 // print current state
tverouden 51:78c75cc72d17 719 changeState = false; // stay in this state
tverouden 5:04b26b2f536a 720
tverouden 51:78c75cc72d17 721 // Actions when entering state
tverouden 51:78c75cc72d17 722 ledRed = 1; // cyan-green blinking LED
tverouden 51:78c75cc72d17 723 ledGreen = 0;
tverouden 51:78c75cc72d17 724 ledBlue = 1;
tverouden 51:78c75cc72d17 725 blinkTimer.attach(&blinkLedBlue,0.5);
tverouden 5:04b26b2f536a 726
tverouden 51:78c75cc72d17 727 }
tverouden 4:5ce2c8864908 728 // ----------------------------- action ------------------------------
tverouden 51:78c75cc72d17 729 // Actions for each loop iteration
tverouden 51:78c75cc72d17 730 calibrationR();
tverouden 4:5ce2c8864908 731 // --------------------------- transition ----------------------------
tverouden 51:78c75cc72d17 732 // Transition condition: when all motor errors smaller than 0.01,
tverouden 51:78c75cc72d17 733 // start calibrating EMG
efvanmarrewijk 52:17b3aeacb643 734 if (errorR < 0.01f && buttonBio2 == false) {
tverouden 23:e282bdb9e9b7 735
tverouden 51:78c75cc72d17 736 // Actions when leaving state
tverouden 51:78c75cc72d17 737 blinkTimer.detach();
tverouden 5:04b26b2f536a 738
tverouden 51:78c75cc72d17 739 currentState = calibratingMotorF; // change to state
tverouden 51:78c75cc72d17 740 changeState = true; // next loop, switch states
tverouden 51:78c75cc72d17 741 }
tverouden 5:04b26b2f536a 742
tverouden 51:78c75cc72d17 743 break; // end case
tverouden 51:78c75cc72d17 744
tverouden 51:78c75cc72d17 745 // ======================== MOTOR F CALIBRATION MODE =========================
tverouden 51:78c75cc72d17 746 case calibratingMotorF:
tverouden 4:5ce2c8864908 747 // ------------------------- initialisation --------------------------
tverouden 51:78c75cc72d17 748 if (changeState) { // when entering the state
tverouden 51:78c75cc72d17 749 pc.printf("[MODE] calibrating motor F...\r\n");
tverouden 51:78c75cc72d17 750 // print current state
tverouden 51:78c75cc72d17 751 changeState = false; // stay in this state
tverouden 4:5ce2c8864908 752
tverouden 51:78c75cc72d17 753 // Actions when entering state
tverouden 51:78c75cc72d17 754 ledRed = 1; // green blinking LED
tverouden 51:78c75cc72d17 755 ledGreen = 1;
tverouden 51:78c75cc72d17 756 ledBlue = 1;
tverouden 51:78c75cc72d17 757 blinkTimer.attach(&blinkLedGreen,0.5);
tverouden 4:5ce2c8864908 758
tverouden 51:78c75cc72d17 759 }
tverouden 51:78c75cc72d17 760 // ----------------------------- action ------------------------------
tverouden 51:78c75cc72d17 761 // Actions for each loop iteration
tverouden 51:78c75cc72d17 762 calibrationF();
tverouden 51:78c75cc72d17 763 // --------------------------- transition ----------------------------
tverouden 51:78c75cc72d17 764 // Transition condition: when all motor errors smaller than 0.01,
tverouden 51:78c75cc72d17 765 // start calibrating EMG
tverouden 51:78c75cc72d17 766 if (errorF < 0.01f && buttonHome == false) {
tverouden 51:78c75cc72d17 767
tverouden 51:78c75cc72d17 768 // Actions when leaving state
tverouden 51:78c75cc72d17 769 blinkTimer.detach();
tverouden 4:5ce2c8864908 770
tverouden 51:78c75cc72d17 771 currentState = calibratingEMG; // change to state
tverouden 51:78c75cc72d17 772 changeState = true; // next loop, switch states
tverouden 51:78c75cc72d17 773 }
tverouden 4:5ce2c8864908 774
tverouden 51:78c75cc72d17 775 break; // end case
tverouden 51:78c75cc72d17 776
tverouden 51:78c75cc72d17 777 // =========================== EMG CALIBRATION MODE ===========================
tverouden 51:78c75cc72d17 778 case calibratingEMG:
tverouden 51:78c75cc72d17 779 // ------------------------- initialisation --------------------------
tverouden 51:78c75cc72d17 780 if (changeState) { // when entering the state
tverouden 51:78c75cc72d17 781 pc.printf("[MODE] calibrating EMG...\r\n");
tverouden 51:78c75cc72d17 782 // print current state
tverouden 51:78c75cc72d17 783 changeState = false; // stay in this state
tverouden 41:5aecc1a27ce6 784
tverouden 51:78c75cc72d17 785 // Actions when entering state
tverouden 51:78c75cc72d17 786 ledRed = 1; // cyan-blue blinking LED
tverouden 51:78c75cc72d17 787 ledGreen = 0;
tverouden 51:78c75cc72d17 788 ledBlue = 0;
tverouden 51:78c75cc72d17 789 blinkTimer.attach(&blinkLedGreen,0.5);
tverouden 51:78c75cc72d17 790
tverouden 51:78c75cc72d17 791 FindMax0_timer.attach(&FindMax0,15); //Find the maximum value after 15 seconds
tverouden 51:78c75cc72d17 792 FindMax1_timer.attach(&FindMax1,15); //Find the maximum value after 15 seconds
tverouden 51:78c75cc72d17 793
tverouden 51:78c75cc72d17 794 EMGtransition_timer.reset();
tverouden 51:78c75cc72d17 795 EMGtransition_timer.start();
tverouden 51:78c75cc72d17 796 }
tverouden 4:5ce2c8864908 797 // ----------------------------- action ------------------------------
tverouden 51:78c75cc72d17 798 // Actions for each loop iteration
tverouden 51:78c75cc72d17 799 CalibrateEMG0(); //start emg calibration every 0.005 seconds
tverouden 51:78c75cc72d17 800 CalibrateEMG1(); //Start EMG calibration every 0.005 seconds
tverouden 51:78c75cc72d17 801 /* */
tverouden 41:5aecc1a27ce6 802
tverouden 4:5ce2c8864908 803
tverouden 4:5ce2c8864908 804 // --------------------------- transition ----------------------------
tverouden 51:78c75cc72d17 805 // Transition condition: after 20 sec in state
tverouden 51:78c75cc72d17 806 if (EMGtransition_timer.read() > 20) {
tverouden 51:78c75cc72d17 807 // Actions when leaving state
tverouden 51:78c75cc72d17 808 blinkTimer.detach();
tverouden 5:04b26b2f536a 809
tverouden 51:78c75cc72d17 810 currentState = homing; // change to state
tverouden 51:78c75cc72d17 811 changeState = true; // next loop, switch states
tverouden 51:78c75cc72d17 812 }
tverouden 51:78c75cc72d17 813 break; // end case
tverouden 4:5ce2c8864908 814
tverouden 4:5ce2c8864908 815 // ============================== HOMING MODE ================================
tverouden 51:78c75cc72d17 816 case homing:
tverouden 41:5aecc1a27ce6 817 // ------------------------- initialisation --------------------------
tverouden 51:78c75cc72d17 818 if (changeState) { // when entering the state
tverouden 51:78c75cc72d17 819 pc.printf("[MODE] homing...\r\n");
tverouden 51:78c75cc72d17 820 // print current state
tverouden 51:78c75cc72d17 821 changeState = false; // stay in this state
tverouden 4:5ce2c8864908 822
tverouden 4:5ce2c8864908 823
tverouden 51:78c75cc72d17 824 // Actions when entering state
tverouden 51:78c75cc72d17 825 ledRed = 1; // cyan LED on
tverouden 51:78c75cc72d17 826 ledGreen = 0;
tverouden 51:78c75cc72d17 827 ledBlue = 0;
tverouden 4:5ce2c8864908 828
tverouden 51:78c75cc72d17 829 }
tverouden 4:5ce2c8864908 830 // ----------------------------- action ------------------------------
tverouden 51:78c75cc72d17 831 // Actions for each loop iteration
tverouden 51:78c75cc72d17 832 /* */
tverouden 4:5ce2c8864908 833
tverouden 41:5aecc1a27ce6 834 // --------------------------- transition ----------------------------
tverouden 51:78c75cc72d17 835 // Transition condition #1: with button press, enter reading mode,
tverouden 51:78c75cc72d17 836 // but only when velocity == 0
tverouden 51:78c75cc72d17 837 if (errorCalibratedL < 0.01f && errorCalibratedR < 0.01f
tverouden 51:78c75cc72d17 838 && errorCalibratedF < 0.01f && buttonBio1 == false) {
tverouden 51:78c75cc72d17 839 // Actions when leaving state
tverouden 51:78c75cc72d17 840 /* */
tverouden 24:0abc564349e1 841
tverouden 51:78c75cc72d17 842 currentState = reading; // change to state
tverouden 51:78c75cc72d17 843 changeState = true; // next loop, switch states
tverouden 51:78c75cc72d17 844 }
tverouden 51:78c75cc72d17 845 // Transition condition #2: with button press, enter demo mode
tverouden 51:78c75cc72d17 846 // but only when velocity == 0
tverouden 51:78c75cc72d17 847 if (errorCalibratedL < 0.01f && errorCalibratedR < 0.01f
tverouden 51:78c75cc72d17 848 && errorCalibratedF < 0.01f && buttonBio2 == false) {
tverouden 51:78c75cc72d17 849 // Actions when leaving state
tverouden 51:78c75cc72d17 850 /* */
tverouden 5:04b26b2f536a 851
tverouden 51:78c75cc72d17 852 currentState = demoing; // change to state
tverouden 51:78c75cc72d17 853 changeState = true; // next loop, switch states
tverouden 51:78c75cc72d17 854 }
tverouden 51:78c75cc72d17 855 break; // end case
tverouden 4:5ce2c8864908 856
tverouden 41:5aecc1a27ce6 857 // ============================== READING MODE ===============================
tverouden 51:78c75cc72d17 858 case reading:
tverouden 17:b04e1938491a 859 // ------------------------- initialisation --------------------------
tverouden 51:78c75cc72d17 860 if (changeState) { // when entering the state
tverouden 51:78c75cc72d17 861 pc.printf("[MODE] reading...\r\n");
tverouden 51:78c75cc72d17 862 // print current state
tverouden 51:78c75cc72d17 863 changeState = false; // stay in this state
tverouden 17:b04e1938491a 864
tverouden 51:78c75cc72d17 865 // Actions when entering state
tverouden 51:78c75cc72d17 866 ledRed = 1; // blue LED on
tverouden 51:78c75cc72d17 867 ledGreen = 1;
tverouden 51:78c75cc72d17 868 ledBlue = 0;
tverouden 41:5aecc1a27ce6 869
tverouden 51:78c75cc72d17 870 // TERUGKLAPPEN
tverouden 17:b04e1938491a 871
tverouden 51:78c75cc72d17 872 }
tverouden 17:b04e1938491a 873 // ----------------------------- action ------------------------------
tverouden 51:78c75cc72d17 874 // Actions for each loop iteration
tverouden 51:78c75cc72d17 875 ReadUseEMG0(); //Start the use of EMG
tverouden 51:78c75cc72d17 876 ReadUseEMG1(); //Start the use of EMG
tverouden 51:78c75cc72d17 877 /* */
tverouden 17:b04e1938491a 878
tverouden 17:b04e1938491a 879 // --------------------------- transition ----------------------------
tverouden 51:78c75cc72d17 880 // Transition condition #1: when EMG signal detected, enter operating
tverouden 51:78c75cc72d17 881 // mode
tverouden 51:78c75cc72d17 882 if (xMove == true && yMove == true) {
tverouden 51:78c75cc72d17 883 // Actions when leaving state
tverouden 51:78c75cc72d17 884 /* */
tverouden 24:0abc564349e1 885
tverouden 51:78c75cc72d17 886 currentState = operating; // change to state
tverouden 51:78c75cc72d17 887 changeState = true; // next loop, switch states
tverouden 51:78c75cc72d17 888 }
tverouden 51:78c75cc72d17 889 // Transition condition #2: with button press, back to homing mode
tverouden 51:78c75cc72d17 890 if (buttonHome == false) {
tverouden 51:78c75cc72d17 891 // Actions when leaving state
tverouden 51:78c75cc72d17 892 /* */
tverouden 17:b04e1938491a 893
tverouden 51:78c75cc72d17 894 currentState = homing; // change to state
tverouden 51:78c75cc72d17 895 changeState = true; // next loop, switch states
tverouden 51:78c75cc72d17 896 }
tverouden 51:78c75cc72d17 897 break; // end case
tverouden 17:b04e1938491a 898
tverouden 4:5ce2c8864908 899 // ============================= OPERATING MODE ==============================
tverouden 51:78c75cc72d17 900 case operating:
tverouden 4:5ce2c8864908 901 // ------------------------- initialisation --------------------------
tverouden 51:78c75cc72d17 902 if (changeState) { // when entering the state
tverouden 51:78c75cc72d17 903 pc.printf("[MODE] operating...\r\n");
tverouden 51:78c75cc72d17 904 // print current state
tverouden 51:78c75cc72d17 905 changeState = false; // stay in this state
tverouden 5:04b26b2f536a 906
tverouden 51:78c75cc72d17 907 // Actions when entering state
tverouden 51:78c75cc72d17 908 ledRed = 1; // blue fast blinking LED
tverouden 51:78c75cc72d17 909 ledGreen = 1;
tverouden 51:78c75cc72d17 910 ledBlue = 1;
tverouden 51:78c75cc72d17 911 blinkTimer.attach(&blinkLedBlue,0.25);
tverouden 5:04b26b2f536a 912
tverouden 51:78c75cc72d17 913 }
tverouden 5:04b26b2f536a 914 // ----------------------------- action ------------------------------
tverouden 51:78c75cc72d17 915 // Actions for each loop iteration
tverouden 51:78c75cc72d17 916 /* */
tverouden 5:04b26b2f536a 917
tverouden 5:04b26b2f536a 918 // --------------------------- transition ----------------------------
tverouden 51:78c75cc72d17 919 // Transition condition: when path is over, back to reading mode
tverouden 51:78c75cc72d17 920 if (errorCalibratedL < 0.01f && errorCalibratedR < 0.01f) {
tverouden 51:78c75cc72d17 921 // Actions when leaving state
tverouden 51:78c75cc72d17 922 blinkTimer.detach();
tverouden 5:04b26b2f536a 923
tverouden 51:78c75cc72d17 924 currentState = reading; // change to state
tverouden 51:78c75cc72d17 925 changeState = true; // next loop, switch states
tverouden 51:78c75cc72d17 926 }
tverouden 51:78c75cc72d17 927 break; // end case
tverouden 5:04b26b2f536a 928
tverouden 5:04b26b2f536a 929 // ============================== DEMOING MODE ===============================
tverouden 51:78c75cc72d17 930 case demoing:
tverouden 5:04b26b2f536a 931 // ------------------------- initialisation --------------------------
tverouden 51:78c75cc72d17 932 if (changeState) { // when entering the state
tverouden 51:78c75cc72d17 933 pc.printf("[MODE] demoing...\r\n");
tverouden 51:78c75cc72d17 934 // print current state
tverouden 51:78c75cc72d17 935 changeState = false; // stay in this state
tverouden 5:04b26b2f536a 936
tverouden 51:78c75cc72d17 937 // Actions when entering state
tverouden 51:78c75cc72d17 938 ledRed = 0; // yellow LED on
tverouden 51:78c75cc72d17 939 ledGreen = 0;
tverouden 51:78c75cc72d17 940 ledBlue = 1;
tverouden 5:04b26b2f536a 941
tverouden 51:78c75cc72d17 942 }
tverouden 5:04b26b2f536a 943 // ----------------------------- action ------------------------------
tverouden 51:78c75cc72d17 944 // Actions for each loop iteration
tverouden 51:78c75cc72d17 945 /* */
tverouden 51:78c75cc72d17 946 ReadUseEMG0(); //Start the use of EMG
tverouden 51:78c75cc72d17 947 ReadUseEMG1(); //Start the use of EMG
efvanmarrewijk 56:e8d568d455a0 948 pc.printf("theta1 and 4: %f \t\t %f \n\r",theta1,theta4);
efvanmarrewijk 55:b297e064ebd9 949 //kinematics();
tverouden 51:78c75cc72d17 950 motorTurnL();
tverouden 51:78c75cc72d17 951 motorTurnR();
efvanmarrewijk 52:17b3aeacb643 952
tverouden 5:04b26b2f536a 953
tverouden 5:04b26b2f536a 954 // --------------------------- transition ----------------------------
tverouden 51:78c75cc72d17 955 // Transition condition: with button press, back to homing mode
tverouden 51:78c75cc72d17 956 if (buttonHome == false) {
tverouden 51:78c75cc72d17 957 // Actions when leaving state
tverouden 51:78c75cc72d17 958 /* */
tverouden 5:04b26b2f536a 959
tverouden 51:78c75cc72d17 960 currentState = homing; // change to state
tverouden 51:78c75cc72d17 961 changeState = true; // next loop, switch states
tverouden 51:78c75cc72d17 962 }
tverouden 51:78c75cc72d17 963 break; // end case
tverouden 5:04b26b2f536a 964
tverouden 5:04b26b2f536a 965 // =============================== FAILING MODE ================================
tverouden 51:78c75cc72d17 966 case failing:
tverouden 51:78c75cc72d17 967 changeState = false; // stay in this state
tverouden 3:9c63fc5f157e 968
tverouden 51:78c75cc72d17 969 // Actions when entering state
tverouden 51:78c75cc72d17 970 ledRed = 0; // red LED on
tverouden 51:78c75cc72d17 971 ledGreen = 1;
tverouden 51:78c75cc72d17 972 ledBlue = 1;
tverouden 4:5ce2c8864908 973
tverouden 51:78c75cc72d17 974 pin3 = 0; // all motor forces to zero
tverouden 51:78c75cc72d17 975 pin5 = 0;
tverouden 51:78c75cc72d17 976 pin6 = 0;
tverouden 51:78c75cc72d17 977 exit (0); // stop all current functions
tverouden 51:78c75cc72d17 978 break; // end case
tverouden 4:5ce2c8864908 979
tverouden 4:5ce2c8864908 980 // ============================== DEFAULT MODE =================================
tverouden 51:78c75cc72d17 981 default:
tverouden 4:5ce2c8864908 982 // ---------------------------- enter failing mode -----------------------------
tverouden 51:78c75cc72d17 983 currentState = failing; // change to state
tverouden 51:78c75cc72d17 984 changeState = true; // next loop, switch states
tverouden 51:78c75cc72d17 985 // print current state
tverouden 51:78c75cc72d17 986 pc.printf("[ERROR] unknown or unimplemented state reached\r\n");
tverouden 3:9c63fc5f157e 987
tverouden 51:78c75cc72d17 988 } // end switch
tverouden 51:78c75cc72d17 989 } // end stateMachine
tverouden 3:9c63fc5f157e 990
tverouden 3:9c63fc5f157e 991
tverouden 2:d70795e4e0bf 992
tverouden 2:d70795e4e0bf 993 // ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡ MAIN LOOP ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡
tverouden 2:d70795e4e0bf 994
tverouden 51:78c75cc72d17 995 int main()
tverouden 51:78c75cc72d17 996 {
tverouden 8:8cef1050ebd9 997 // ================================ EMERGENCY ================================
tverouden 51:78c75cc72d17 998 //If the emergency button is pressed, stop program via failing state
tverouden 51:78c75cc72d17 999 buttonEmergency.rise(stopProgram); // Automatische triggers voor failure mode? -> ook error message in andere functies plaatsen!
tverouden 15:6566c5dedeeb 1000
tverouden 43:d332aa9f49e0 1001 // ============================= PC-COMMUNICATION ============================
tverouden 51:78c75cc72d17 1002 pc.baud(115200); // communication with terminal
tverouden 51:78c75cc72d17 1003 pc.printf("\n\n[START] starting O.D.I.N\r\n");
tverouden 6:f32352bc5078 1004
tverouden 43:d332aa9f49e0 1005 // ============================= PIN DEFINE PERIOD ===========================
tverouden 51:78c75cc72d17 1006 // If you give a period on one pin, c++ gives all pins this period
tverouden 51:78c75cc72d17 1007 pin3.period_us(15);
tverouden 41:5aecc1a27ce6 1008
tverouden 43:d332aa9f49e0 1009 // ==================================== LOOP ==================================
tverouden 51:78c75cc72d17 1010 // run state machine at 500 Hz
tverouden 51:78c75cc72d17 1011 stateTimer.attach(&stateMachine,dt);
tverouden 43:d332aa9f49e0 1012
tverouden 43:d332aa9f49e0 1013 // =============================== ADD FILTERS ===============================
tverouden 51:78c75cc72d17 1014 //Make filter chain for the first EMG
tverouden 51:78c75cc72d17 1015 filter0.add(&Notch50_0).add(&Notch100_0).add(&Notch150_0).add(&Notch200_0).add(&Low_0).add(&High_0);
tverouden 51:78c75cc72d17 1016 //Make filter chain for the second EMG
tverouden 51:78c75cc72d17 1017 filter1.add(&Notch50_1).add(&Notch100_1).add(&Notch150_1).add(&Notch200_1).add(&Low_1).add(&High_1);
tverouden 51:78c75cc72d17 1018 }