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@41:5aecc1a27ce6, 2018-11-01 (annotated)

Committer:
tverouden
Date:
Thu Nov 01 14:51:25 2018 +0000
Revision:
41:5aecc1a27ce6
Parent:
37:317e14b9d551
Child:
42:bb43f1b67787
Added kinematics functions & parameters

Who changed what in which revision?

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