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@43:d332aa9f49e0, 2018-11-01 (annotated)

Committer:
tverouden
Date:
Thu Nov 01 15:02:32 2018 +0000
Revision:
43:d332aa9f49e0
Parent:
42:bb43f1b67787
Child:
44:ca74d11a2dac
Added filter chains in main

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 42:bb43f1b67787 429 //float angleDesiredL() // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
tverouden 42:bb43f1b67787 430 //{ float angle = (pot2*2.0f*PI)-PI;
tverouden 42:bb43f1b67787 431 // return angle;
tverouden 42:bb43f1b67787 432 //}
tverouden 41:5aecc1a27ce6 433
tverouden 41:5aecc1a27ce6 434 float countsCalibrCalcL(int countsOffsetL)
tverouden 41:5aecc1a27ce6 435 {
tverouden 41:5aecc1a27ce6 436 countsCalibratedL = countsL - countsOffsetL + countsCalibration;
tverouden 41:5aecc1a27ce6 437 return countsCalibratedL;
tverouden 41:5aecc1a27ce6 438 }
EvaKrolis 13:4ba8f63e6ff4 439
efvanmarrewijk 25:ac139331fe51 440 void calibrationL() // Partially the same as motorTurnL, only with potmeter input
efvanmarrewijk 25:ac139331fe51 441 // How it works: manually turn motor using potmeters until the robot arm touches the bookholder.
efvanmarrewijk 25:ac139331fe51 442 // This program sets the counts from the motor to the reference counts (an angle of PI/4.0)
efvanmarrewijk 25:ac139331fe51 443 // Do this for every motor and after calibrated all motors, press a button
tverouden 41:5aecc1a27ce6 444 {
tverouden 41:5aecc1a27ce6 445 float angleReferenceL = angleDesiredL(); // insert kinematics output here instead of angleDesiredL()
efvanmarrewijk 25:ac139331fe51 446 angleReferenceL = -angleReferenceL; // different minus sign per motor
efvanmarrewijk 25:ac139331fe51 447 angleCurrentL = angleCurrent(countsL); // different minus sign per motor
efvanmarrewijk 25:ac139331fe51 448 errorL = errorCalc(angleReferenceL,angleCurrentL); // same for every motor
tverouden 41:5aecc1a27ce6 449
tverouden 41:5aecc1a27ce6 450 if (fabs(errorL) >= 0.01f) {
tverouden 41:5aecc1a27ce6 451 float PIDControlL = PIDControllerL(angleReferenceL,angleCurrentL); // same for every motor
efvanmarrewijk 25:ac139331fe51 452 pin6 = fabs(PIDControlL); // different pins for every motor
efvanmarrewijk 25:ac139331fe51 453 pin7 = PIDControlL > 0.0f; // different pins for every motor
tverouden 41:5aecc1a27ce6 454 } else if (fabs(errorL) < 0.01f) {
tverouden 41:5aecc1a27ce6 455 int countsOffsetL = countsL;
efvanmarrewijk 25:ac139331fe51 456 countsCalibrCalcL(countsOffsetL);
tverouden 41:5aecc1a27ce6 457 pin6 = 0.0f;
tverouden 41:5aecc1a27ce6 458 // BUTTON PRESS: TO NEXT STATE
tverouden 41:5aecc1a27ce6 459 }
tverouden 41:5aecc1a27ce6 460 }
tverouden 41:5aecc1a27ce6 461
efvanmarrewijk 25:ac139331fe51 462 void motorTurnL() // main function for movement of motor 1, all above functions with an extra tab are called
tverouden 41:5aecc1a27ce6 463 {
efvanmarrewijk 38:317b8eaaf1a2 464 float angleReferenceL = theta1; // insert kinematics output here instead of angleDesiredL()
efvanmarrewijk 25:ac139331fe51 465 angleReferenceL = -angleReferenceL; // different minus sign per motor
efvanmarrewijk 25:ac139331fe51 466 int countsL = countsInputL(); // different encoder pins per motor
efvanmarrewijk 25:ac139331fe51 467 angleCurrentL = angleCurrent(countsCalibratedL); // different minus sign per motor
efvanmarrewijk 25:ac139331fe51 468 errorCalibratedL = errorCalc(angleReferenceL,angleCurrentL); // same for every motor
tverouden 41:5aecc1a27ce6 469
efvanmarrewijk 25:ac139331fe51 470 float PIDControlL = PIDControllerL(angleReferenceL,angleCurrentL); // same for every motor
efvanmarrewijk 25:ac139331fe51 471 pin6 = fabs(PIDControlL); // different pins for every motor
efvanmarrewijk 25:ac139331fe51 472 pin7 = PIDControlL > 0.0f; // different pins for every motor
efvanmarrewijk 25:ac139331fe51 473 }
efvanmarrewijk 25:ac139331fe51 474
efvanmarrewijk 25:ac139331fe51 475 // ------------------------ MOTOR FUNCTIONS FOR MOTOR RIGHT --------------------
tverouden 41:5aecc1a27ce6 476 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 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 errorIntegralR = 0.0;
tverouden 41:5aecc1a27ce6 483 static float errorPreviousR = error; // initialization with this value only done once!
tverouden 41:5aecc1a27ce6 484 static BiQuad PIDFilterR(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 errorIntegralR = errorIntegralR + error * dt;
tverouden 41:5aecc1a27ce6 489 float u_i = Ki * errorIntegralR;
tverouden 41:5aecc1a27ce6 490 // Derivative part
tverouden 41:5aecc1a27ce6 491 float errorDerivative = (error - errorPreviousR)/dt;
tverouden 41:5aecc1a27ce6 492 float errorDerivativeFiltered = PIDFilterR.step(errorDerivative);
tverouden 41:5aecc1a27ce6 493 float u_d = Kd * errorDerivativeFiltered;
tverouden 41:5aecc1a27ce6 494 errorPreviousR = 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 42:bb43f1b67787 499 //float angleDesiredR() // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
tverouden 42:bb43f1b67787 500 //{ float angle = (pot2*2.0f*PI)-PI;
tverouden 42:bb43f1b67787 501 // return angle;
tverouden 42:bb43f1b67787 502 //}
tverouden 41:5aecc1a27ce6 503
tverouden 41:5aecc1a27ce6 504 float countsCalibrCalcR(int countsOffsetR)
tverouden 41:5aecc1a27ce6 505 {
tverouden 41:5aecc1a27ce6 506 countsCalibratedR = countsR - countsOffsetR + countsCalibration;
tverouden 41:5aecc1a27ce6 507 return countsCalibratedR;
tverouden 41:5aecc1a27ce6 508 }
efvanmarrewijk 25:ac139331fe51 509
efvanmarrewijk 25:ac139331fe51 510 void calibrationR() // Partially the same as motorTurnR, only with potmeter input
efvanmarrewijk 25:ac139331fe51 511 // How it works: manually turn motor using potmeters until the robot arm touches the bookholder.
efvanmarrewijk 25:ac139331fe51 512 // This program sets the counts from the motor to the reference counts (an angle of PI/4.0)
efvanmarrewijk 25:ac139331fe51 513 // Do this for every motor and after calibrated all motors, press a button
tverouden 41:5aecc1a27ce6 514 {
tverouden 41:5aecc1a27ce6 515 float angleReferenceR = angleDesiredR(); // insert kinematics output here instead of angleDesiredR()
efvanmarrewijk 25:ac139331fe51 516 angleReferenceR = -angleReferenceR; // different minus sign per motor
efvanmarrewijk 25:ac139331fe51 517 angleCurrentR = angleCurrent(countsR); // different minus sign per motor
efvanmarrewijk 25:ac139331fe51 518 errorR = errorCalc(angleReferenceR,angleCurrentR); // same for every motor
tverouden 41:5aecc1a27ce6 519
tverouden 41:5aecc1a27ce6 520 if (fabs(errorR) >= 0.01f) {
tverouden 41:5aecc1a27ce6 521 float PIDControlR = PIDControllerR(angleReferenceR,angleCurrentR); // same for every motor
efvanmarrewijk 25:ac139331fe51 522 pin6 = fabs(PIDControlR); // different pins for every motor
efvanmarrewijk 25:ac139331fe51 523 pin7 = PIDControlR > 0.0f; // different pins for every motor
tverouden 41:5aecc1a27ce6 524 } else if (fabs(errorR) < 0.01f) {
tverouden 41:5aecc1a27ce6 525 int countsOffsetR = countsR;
efvanmarrewijk 25:ac139331fe51 526 countsCalibrCalcR(countsOffsetR);
tverouden 41:5aecc1a27ce6 527 pin6 = 0.0f;
tverouden 41:5aecc1a27ce6 528 // BUTTON PRESS: NAAR VOLGENDE STATE
tverouden 41:5aecc1a27ce6 529 }
tverouden 41:5aecc1a27ce6 530 }
tverouden 41:5aecc1a27ce6 531
efvanmarrewijk 25:ac139331fe51 532 void motorTurnR() // main function for movement of motor 1, all above functions with an extra tab are called
tverouden 41:5aecc1a27ce6 533 {
efvanmarrewijk 38:317b8eaaf1a2 534 float angleReferenceR = theta4; // insert kinematics output here instead of angleDesiredR()
efvanmarrewijk 25:ac139331fe51 535 angleReferenceR = -angleReferenceR; // different minus sign per motor
efvanmarrewijk 25:ac139331fe51 536 int countsR = countsInputR(); // different encoder pins per motor
efvanmarrewijk 25:ac139331fe51 537 angleCurrentR = angleCurrent(countsCalibratedR); // different minus sign per motor
efvanmarrewijk 25:ac139331fe51 538 errorCalibratedR = errorCalc(angleReferenceR,angleCurrentR); // same for every motor
tverouden 41:5aecc1a27ce6 539
efvanmarrewijk 25:ac139331fe51 540 float PIDControlR = PIDControllerR(angleReferenceR,angleCurrentR); // same for every motor
efvanmarrewijk 25:ac139331fe51 541 pin6 = fabs(PIDControlR); // different pins for every motor
efvanmarrewijk 25:ac139331fe51 542 pin7 = PIDControlR > 0.0f; // different pins for every motor
efvanmarrewijk 25:ac139331fe51 543 }
efvanmarrewijk 25:ac139331fe51 544
efvanmarrewijk 25:ac139331fe51 545 // ------------------------- MOTOR FUNCTIONS FOR MOTOR FLIP --------------------
tverouden 41:5aecc1a27ce6 546 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 547 {
tverouden 41:5aecc1a27ce6 548 float Kp = 19.24f;
tverouden 41:5aecc1a27ce6 549 float Ki = 1.02f;
tverouden 41:5aecc1a27ce6 550 float Kd = 0.827f;
tverouden 41:5aecc1a27ce6 551 float error = errorCalc(angleReference,angleCurrent);
tverouden 41:5aecc1a27ce6 552 static float errorIntegralF = 0.0;
tverouden 41:5aecc1a27ce6 553 static float errorPreviousF = error; // initialization with this value only done once!
tverouden 41:5aecc1a27ce6 554 static BiQuad PIDFilterF(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
tverouden 41:5aecc1a27ce6 555 // Proportional part:
tverouden 41:5aecc1a27ce6 556 float u_k = Kp * error;
tverouden 41:5aecc1a27ce6 557 // Integral part
tverouden 41:5aecc1a27ce6 558 errorIntegralF = errorIntegralF + error * dt;
tverouden 41:5aecc1a27ce6 559 float u_i = Ki * errorIntegralF;
tverouden 41:5aecc1a27ce6 560 // Derivative part
tverouden 41:5aecc1a27ce6 561 float errorDerivative = (error - errorPreviousF)/dt;
tverouden 41:5aecc1a27ce6 562 float errorDerivativeFiltered = PIDFilterF.step(errorDerivative);
tverouden 41:5aecc1a27ce6 563 float u_d = Kd * errorDerivativeFiltered;
tverouden 41:5aecc1a27ce6 564 errorPreviousF = error;
tverouden 41:5aecc1a27ce6 565 // Sum all parts and return it
tverouden 41:5aecc1a27ce6 566 return u_k + u_i + u_d;
tverouden 41:5aecc1a27ce6 567 }
efvanmarrewijk 35:3c937770aa41 568
tverouden 41:5aecc1a27ce6 569 float angleDesiredF() // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
tverouden 41:5aecc1a27ce6 570 {
tverouden 41:5aecc1a27ce6 571 float angle = (pot1*2.0f*PI)-PI;
tverouden 41:5aecc1a27ce6 572 return angle;
tverouden 41:5aecc1a27ce6 573 }
tverouden 41:5aecc1a27ce6 574
tverouden 41:5aecc1a27ce6 575 float countsCalibrCalcF(int countsOffsetF)
tverouden 41:5aecc1a27ce6 576 {
tverouden 41:5aecc1a27ce6 577 countsCalibratedF = countsF - countsOffsetF + countsCalibration;
tverouden 41:5aecc1a27ce6 578 return countsCalibratedF;
tverouden 41:5aecc1a27ce6 579 }
efvanmarrewijk 35:3c937770aa41 580
efvanmarrewijk 35:3c937770aa41 581 void calibrationF() // Partially the same as motorTurnF, only with potmeter input
efvanmarrewijk 35:3c937770aa41 582 // How it works: manually turn motor using potmeters until the robot arm touches the bookholder.
efvanmarrewijk 35:3c937770aa41 583 // This program sets the counts from the motor to the reference counts (an angle of PI/4.0)
efvanmarrewijk 35:3c937770aa41 584 // Do this for every motor and after calibrated all motors, press a button
tverouden 42:bb43f1b67787 585 {
tverouden 42:bb43f1b67787 586 float angleReferenceF = 0.0f;
tverouden 42:bb43f1b67787 587 //float angleReferenceF = angleDesiredF(); // insert kinematics output here instead of angleDesiredF()
tverouden 42:bb43f1b67787 588 angleReferenceF = -angleReferenceF; // different minus sign per motor
tverouden 42:bb43f1b67787 589 angleCurrentF = angleCurrent(countsF); // different minus sign per motor
tverouden 42:bb43f1b67787 590 errorF = errorCalc(angleReferenceF,angleCurrentF); // same for every motor
tverouden 41:5aecc1a27ce6 591
tverouden 42:bb43f1b67787 592 if (fabs(errorF) >= 0.01f) {
tverouden 42:bb43f1b67787 593 float PIDControlF = PIDControllerF(angleReferenceF,angleCurrentF); // same for every motor
tverouden 42:bb43f1b67787 594 pin6 = fabs(PIDControlF); // different pins for every motor
tverouden 42:bb43f1b67787 595 pin7 = PIDControlF > 0.0f; // different pins for every motor
tverouden 42:bb43f1b67787 596 } else if (fabs(errorF) < 0.01f) {
tverouden 42:bb43f1b67787 597 int countsOffsetF = countsF;
tverouden 42:bb43f1b67787 598 countsCalibrCalcF(countsOffsetF);
tverouden 42:bb43f1b67787 599 pin6 = 0.0f;
tverouden 42:bb43f1b67787 600 // BUTTON PRESS: TO NEXT STATE
tverouden 42:bb43f1b67787 601 }
efvanmarrewijk 35:3c937770aa41 602 }
tverouden 41:5aecc1a27ce6 603
tverouden 42:bb43f1b67787 604 void motorTurnF() // main function for movement of motor 1, all above functions with an extra tab are called
tverouden 42:bb43f1b67787 605 {
tverouden 42:bb43f1b67787 606 float angleReferenceF = 0.0f;
tverouden 42:bb43f1b67787 607 //float angleReferenceF = angleDesiredF(); // insert kinematics output here instead of angleDesiredF()
tverouden 42:bb43f1b67787 608 angleReferenceF = -angleReferenceF; // different minus sign per motor
tverouden 42:bb43f1b67787 609 int countsF = countsInputF(); // different encoder pins per motor
tverouden 42:bb43f1b67787 610 angleCurrentF = angleCurrent(countsCalibratedF); // different minus sign per motor
tverouden 42:bb43f1b67787 611 errorCalibratedF = errorCalc(angleReferenceF,angleCurrentF); // same for every motor
tverouden 41:5aecc1a27ce6 612
tverouden 42:bb43f1b67787 613 float PIDControlF = PIDControllerF(angleReferenceF,angleCurrentF); // same for every motor
tverouden 42:bb43f1b67787 614 pin6 = fabs(PIDControlF); // different pins for every motor
tverouden 42:bb43f1b67787 615 pin7 = PIDControlF > 0.0f; // different pins for every motor
tverouden 42:bb43f1b67787 616 }
EvaKrolis 13:4ba8f63e6ff4 617
tverouden 2:d70795e4e0bf 618 // ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡ STATE MACHINE ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡
tverouden 42:bb43f1b67787 619 void stateMachine(void) {
tverouden 42:bb43f1b67787 620 switch (currentState) { // determine which state Odin is in
tverouden 3:9c63fc5f157e 621
tverouden 4:5ce2c8864908 622 // ========================= MOTOR CALIBRATION MODE ==========================
tverouden 42:bb43f1b67787 623 case calibratingMotors:
tverouden 4:5ce2c8864908 624 // ------------------------- initialisation --------------------------
tverouden 42:bb43f1b67787 625 if (changeState) { // when entering the state
tverouden 42:bb43f1b67787 626 pc.printf("[MODE] calibrating motors...\r\n");
tverouden 42:bb43f1b67787 627 // print current state
tverouden 42:bb43f1b67787 628 changeState = false; // stay in this state
tverouden 4:5ce2c8864908 629
tverouden 42:bb43f1b67787 630 // Actions when entering state
tverouden 42:bb43f1b67787 631 ledRed = 1; // cyan-green blinking LED
tverouden 42:bb43f1b67787 632 ledGreen = 0;
tverouden 42:bb43f1b67787 633 ledBlue = 0;
tverouden 42:bb43f1b67787 634 blinkTimer.attach(&blinkLedBlue,0.5);
tverouden 4:5ce2c8864908 635
tverouden 42:bb43f1b67787 636 }
tverouden 4:5ce2c8864908 637 // ----------------------------- action ------------------------------
tverouden 42:bb43f1b67787 638 // Actions for each loop iteration
tverouden 42:bb43f1b67787 639 /* */
tverouden 4:5ce2c8864908 640
tverouden 4:5ce2c8864908 641 // --------------------------- transition ----------------------------
tverouden 42:bb43f1b67787 642 // Transition condition: when all motor errors smaller than 0.01,
tverouden 42:bb43f1b67787 643 // start calibrating EMG
tverouden 42:bb43f1b67787 644 if (errorMotorL < 0.01 && errorMotorR < 0.01
tverouden 42:bb43f1b67787 645 && errorMotorF < 0.01 && buttonHome == false) {
tverouden 23:e282bdb9e9b7 646
tverouden 42:bb43f1b67787 647 // Actions when leaving state
tverouden 42:bb43f1b67787 648 blinkTimer.detach();
tverouden 5:04b26b2f536a 649
tverouden 42:bb43f1b67787 650 currentState = calibratingEMG; // change to state
tverouden 42:bb43f1b67787 651 changeState = true; // next loop, switch states
tverouden 42:bb43f1b67787 652 }
tverouden 5:04b26b2f536a 653
tverouden 42:bb43f1b67787 654 break; // end case
tverouden 4:5ce2c8864908 655
tverouden 7:ef5966469621 656 // =========================== EMG CALIBRATION MODE ===========================
tverouden 42:bb43f1b67787 657 case calibratingEMG:
tverouden 4:5ce2c8864908 658 // ------------------------- initialisation --------------------------
tverouden 42:bb43f1b67787 659 if (changeState) { // when entering the state
tverouden 42:bb43f1b67787 660 pc.printf("[MODE] calibrating EMG...\r\n");
tverouden 42:bb43f1b67787 661 // print current state
tverouden 42:bb43f1b67787 662 changeState = false; // stay in this state
tverouden 4:5ce2c8864908 663
tverouden 42:bb43f1b67787 664 // Actions when entering state
tverouden 42:bb43f1b67787 665 ledRed = 1; // cyan-blue blinking LED
tverouden 42:bb43f1b67787 666 ledGreen = 0;
tverouden 42:bb43f1b67787 667 ledBlue = 0;
tverouden 42:bb43f1b67787 668 blinkTimer.attach(&blinkLedGreen,0.5);
tverouden 4:5ce2c8864908 669
tverouden 42:bb43f1b67787 670 FindMax0_timer.attach(&FindMax0,15); //Find the maximum value after 15 seconds
tverouden 42:bb43f1b67787 671 FindMax1_timer.attach(&FindMax1,15); //Find the maximum value after 15 seconds
tverouden 41:5aecc1a27ce6 672
tverouden 42:bb43f1b67787 673 EMGtransition_timer.reset();
tverouden 42:bb43f1b67787 674 EMGtransition_timer.start();
tverouden 42:bb43f1b67787 675 }
tverouden 4:5ce2c8864908 676 // ----------------------------- action ------------------------------
tverouden 42:bb43f1b67787 677 // Actions for each loop iteration
tverouden 42:bb43f1b67787 678 CalibrateEMG0(); //start emg calibration every 0.005 seconds
tverouden 42:bb43f1b67787 679 CalibrateEMG1(); //Start EMG calibration every 0.005 seconds
tverouden 42:bb43f1b67787 680 /* */
tverouden 41:5aecc1a27ce6 681
tverouden 4:5ce2c8864908 682
tverouden 4:5ce2c8864908 683 // --------------------------- transition ----------------------------
tverouden 42:bb43f1b67787 684 // Transition condition: after 20 sec in state
tverouden 42:bb43f1b67787 685 if (local_timer.read() > 20) {
tverouden 42:bb43f1b67787 686 // Actions when leaving state
tverouden 42:bb43f1b67787 687 blinkTimer.detach();
tverouden 5:04b26b2f536a 688
tverouden 42:bb43f1b67787 689 currentState = homing; // change to state
tverouden 42:bb43f1b67787 690 changeState = true; // next loop, switch states
tverouden 42:bb43f1b67787 691 }
tverouden 42:bb43f1b67787 692 break; // end case
tverouden 4:5ce2c8864908 693
tverouden 4:5ce2c8864908 694 // ============================== HOMING MODE ================================
tverouden 42:bb43f1b67787 695 case homing:
tverouden 41:5aecc1a27ce6 696 // ------------------------- initialisation --------------------------
tverouden 42:bb43f1b67787 697 if (changeState) { // when entering the state
tverouden 42:bb43f1b67787 698 pc.printf("[MODE] homing...\r\n");
tverouden 42:bb43f1b67787 699 // print current state
tverouden 42:bb43f1b67787 700 changeState = false; // stay in this state
tverouden 4:5ce2c8864908 701
tverouden 4:5ce2c8864908 702
tverouden 42:bb43f1b67787 703 // Actions when entering state
tverouden 42:bb43f1b67787 704 ledRed = 1; // cyan LED on
tverouden 42:bb43f1b67787 705 ledGreen = 0;
tverouden 42:bb43f1b67787 706 ledBlue = 0;
tverouden 4:5ce2c8864908 707
tverouden 42:bb43f1b67787 708 }
tverouden 4:5ce2c8864908 709 // ----------------------------- action ------------------------------
tverouden 42:bb43f1b67787 710 // Actions for each loop iteration
tverouden 42:bb43f1b67787 711 /* */
tverouden 4:5ce2c8864908 712
tverouden 41:5aecc1a27ce6 713 // --------------------------- transition ----------------------------
tverouden 42:bb43f1b67787 714 // Transition condition #1: with button press, enter demo mode,
tverouden 42:bb43f1b67787 715 // but only when velocity == 0
tverouden 42:bb43f1b67787 716 if (errorMotorL < 0.01 && errorMotorR < 0.01
tverouden 42:bb43f1b67787 717 && errorMotorF < 0.01 && buttonBio1 == true) {
tverouden 42:bb43f1b67787 718 // Actions when leaving state
tverouden 42:bb43f1b67787 719 /* */
tverouden 24:0abc564349e1 720
tverouden 42:bb43f1b67787 721 currentState = reading; // change to state
tverouden 42:bb43f1b67787 722 changeState = true; // next loop, switch states
tverouden 42:bb43f1b67787 723 }
tverouden 42:bb43f1b67787 724 // Transition condition #2: with button press, enter operation mode
tverouden 42:bb43f1b67787 725 // but only when velocity == 0
tverouden 42:bb43f1b67787 726 if (errorMotorL < 0.01 && errorMotorR < 0.01
tverouden 42:bb43f1b67787 727 && errorMotorF < 0.01 && buttonBio2 == true) {
tverouden 42:bb43f1b67787 728 // Actions when leaving state
tverouden 42:bb43f1b67787 729 /* */
tverouden 5:04b26b2f536a 730
tverouden 42:bb43f1b67787 731 currentState = demoing; // change to state
tverouden 42:bb43f1b67787 732 changeState = true; // next loop, switch states
tverouden 42:bb43f1b67787 733 }
tverouden 42:bb43f1b67787 734 break; // end case
tverouden 4:5ce2c8864908 735
tverouden 41:5aecc1a27ce6 736 // ============================== READING MODE ===============================
tverouden 42:bb43f1b67787 737 case reading:
tverouden 17:b04e1938491a 738 // ------------------------- initialisation --------------------------
tverouden 42:bb43f1b67787 739 if (changeState) { // when entering the state
tverouden 42:bb43f1b67787 740 pc.printf("[MODE] reading...\r\n");
tverouden 42:bb43f1b67787 741 // print current state
tverouden 42:bb43f1b67787 742 changeState = false; // stay in this state
tverouden 17:b04e1938491a 743
tverouden 42:bb43f1b67787 744 // Actions when entering state
tverouden 42:bb43f1b67787 745 ledRed = 1; // blue LED on
tverouden 42:bb43f1b67787 746 ledGreen = 1;
tverouden 42:bb43f1b67787 747 ledBlue = 0;
tverouden 41:5aecc1a27ce6 748
tverouden 42:bb43f1b67787 749 // TERUGKLAPPEN
tverouden 17:b04e1938491a 750
tverouden 42:bb43f1b67787 751 }
tverouden 17:b04e1938491a 752 // ----------------------------- action ------------------------------
tverouden 42:bb43f1b67787 753 // Actions for each loop iteration
tverouden 42:bb43f1b67787 754 ReadUseEMG0(); //Start the use of EMG
tverouden 42:bb43f1b67787 755 ReadUseEMG1(); //Start the use of EMG
tverouden 42:bb43f1b67787 756 /* */
tverouden 17:b04e1938491a 757
tverouden 17:b04e1938491a 758 // --------------------------- transition ----------------------------
tverouden 42:bb43f1b67787 759 // Transition condition #1: when EMG signal detected, enter reading
tverouden 42:bb43f1b67787 760 // mode
tverouden 42:bb43f1b67787 761 if (xMove == true || yMove == true) {
tverouden 42:bb43f1b67787 762 // Actions when leaving state
tverouden 42:bb43f1b67787 763 /* */
tverouden 24:0abc564349e1 764
tverouden 42:bb43f1b67787 765 currentState = reading; // change to state
tverouden 42:bb43f1b67787 766 changeState = true; // next loop, switch states
tverouden 42:bb43f1b67787 767 }
tverouden 42:bb43f1b67787 768 // Transition condition #2: with button press, back to homing mode
tverouden 42:bb43f1b67787 769 if (buttonHome == false) {
tverouden 42:bb43f1b67787 770 // Actions when leaving state
tverouden 42:bb43f1b67787 771 /* */
tverouden 17:b04e1938491a 772
tverouden 42:bb43f1b67787 773 currentState = homing; // change to state
tverouden 42:bb43f1b67787 774 changeState = true; // next loop, switch states
tverouden 42:bb43f1b67787 775 }
tverouden 42:bb43f1b67787 776 break; // end case
tverouden 17:b04e1938491a 777
tverouden 4:5ce2c8864908 778 // ============================= OPERATING MODE ==============================
tverouden 42:bb43f1b67787 779 case operating:
tverouden 4:5ce2c8864908 780 // ------------------------- initialisation --------------------------
tverouden 42:bb43f1b67787 781 if (changeState) { // when entering the state
tverouden 42:bb43f1b67787 782 pc.printf("[MODE] operating...\r\n");
tverouden 42:bb43f1b67787 783 // print current state
tverouden 42:bb43f1b67787 784 changeState = false; // stay in this state
tverouden 5:04b26b2f536a 785
tverouden 42:bb43f1b67787 786 // Actions when entering state
tverouden 42:bb43f1b67787 787 ledRed = 1; // blue fast blinking LED
tverouden 42:bb43f1b67787 788 ledGreen = 1;
tverouden 42:bb43f1b67787 789 ledBlue = 1;
tverouden 42:bb43f1b67787 790 blinkTimer.attach(&blinkLedBlue,0.25);
tverouden 5:04b26b2f536a 791
tverouden 42:bb43f1b67787 792 }
tverouden 5:04b26b2f536a 793 // ----------------------------- action ------------------------------
tverouden 42:bb43f1b67787 794 // Actions for each loop iteration
tverouden 42:bb43f1b67787 795 /* */
tverouden 5:04b26b2f536a 796
tverouden 5:04b26b2f536a 797 // --------------------------- transition ----------------------------
tverouden 42:bb43f1b67787 798 // Transition condition: when path is over, back to reading mode
tverouden 42:bb43f1b67787 799 if (errorMotorL < 0.01 && errorMotorR < 0.01) {
tverouden 42:bb43f1b67787 800 // Actions when leaving state
tverouden 42:bb43f1b67787 801 blinkTimer.detach();
tverouden 5:04b26b2f536a 802
tverouden 42:bb43f1b67787 803 currentState = reading; // change to state
tverouden 42:bb43f1b67787 804 changeState = true; // next loop, switch states
tverouden 42:bb43f1b67787 805 }
tverouden 42:bb43f1b67787 806 break; // end case
tverouden 5:04b26b2f536a 807
tverouden 5:04b26b2f536a 808 // ============================== DEMOING MODE ===============================
tverouden 42:bb43f1b67787 809 case demoing:
tverouden 5:04b26b2f536a 810 // ------------------------- initialisation --------------------------
tverouden 42:bb43f1b67787 811 if (changeState) { // when entering the state
tverouden 42:bb43f1b67787 812 pc.printf("[MODE] demoing...\r\n");
tverouden 42:bb43f1b67787 813 // print current state
tverouden 42:bb43f1b67787 814 changeState = false; // stay in this state
tverouden 5:04b26b2f536a 815
tverouden 42:bb43f1b67787 816 // Actions when entering state
tverouden 42:bb43f1b67787 817 ledRed = 0; // yellow LED on
tverouden 42:bb43f1b67787 818 ledGreen = 0;
tverouden 42:bb43f1b67787 819 ledBlue = 1;
tverouden 5:04b26b2f536a 820
tverouden 42:bb43f1b67787 821 }
tverouden 5:04b26b2f536a 822 // ----------------------------- action ------------------------------
tverouden 42:bb43f1b67787 823 // Actions for each loop iteration
tverouden 42:bb43f1b67787 824 /* */
tverouden 42:bb43f1b67787 825 ReadUseEMG0(); //Start the use of EMG
tverouden 42:bb43f1b67787 826 ReadUseEMG1(); //Start the use of EMG
tverouden 5:04b26b2f536a 827
tverouden 5:04b26b2f536a 828 // --------------------------- transition ----------------------------
tverouden 42:bb43f1b67787 829 // Transition condition: with button press, back to homing mode
tverouden 42:bb43f1b67787 830 if (buttonHome == false) {
tverouden 42:bb43f1b67787 831 // Actions when leaving state
tverouden 42:bb43f1b67787 832 /* */
tverouden 5:04b26b2f536a 833
tverouden 42:bb43f1b67787 834 currentState = homing; // change to state
tverouden 42:bb43f1b67787 835 changeState = true; // next loop, switch states
tverouden 42:bb43f1b67787 836 }
tverouden 42:bb43f1b67787 837 break; // end case
tverouden 5:04b26b2f536a 838
tverouden 5:04b26b2f536a 839 // =============================== FAILING MODE ================================
tverouden 42:bb43f1b67787 840 case failing:
tverouden 42:bb43f1b67787 841 changeState = false; // stay in this state
tverouden 3:9c63fc5f157e 842
tverouden 42:bb43f1b67787 843 // Actions when entering state
tverouden 42:bb43f1b67787 844 ledRed = 0; // red LED on
tverouden 42:bb43f1b67787 845 ledGreen = 1;
tverouden 42:bb43f1b67787 846 ledBlue = 1;
tverouden 4:5ce2c8864908 847
tverouden 42:bb43f1b67787 848 pin3 = 0; // all motor forces to zero
tverouden 42:bb43f1b67787 849 pin5 = 0;
tverouden 42:bb43f1b67787 850 pin6 = 0;
tverouden 42:bb43f1b67787 851 exit (0); // stop all current functions
tverouden 42:bb43f1b67787 852 break; // end case
tverouden 4:5ce2c8864908 853
tverouden 4:5ce2c8864908 854 // ============================== DEFAULT MODE =================================
tverouden 42:bb43f1b67787 855 default:
tverouden 4:5ce2c8864908 856 // ---------------------------- enter failing mode -----------------------------
tverouden 42:bb43f1b67787 857 currentState = failing; // change to state
tverouden 42:bb43f1b67787 858 changeState = true; // next loop, switch states
tverouden 42:bb43f1b67787 859 // print current state
tverouden 42:bb43f1b67787 860 pc.printf("[ERROR] unknown or unimplemented state reached\r\n");
tverouden 3:9c63fc5f157e 861
tverouden 42:bb43f1b67787 862 } // end switch
tverouden 42:bb43f1b67787 863 } // end stateMachine
tverouden 3:9c63fc5f157e 864
tverouden 3:9c63fc5f157e 865
tverouden 2:d70795e4e0bf 866
tverouden 2:d70795e4e0bf 867 // ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡ MAIN LOOP ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡
tverouden 2:d70795e4e0bf 868
tverouden 42:bb43f1b67787 869 int main() {
tverouden 8:8cef1050ebd9 870 // ================================ EMERGENCY ================================
tverouden 42:bb43f1b67787 871 //If the emergency button is pressed, stop program via failing state
tverouden 42:bb43f1b67787 872 buttonEmergency.rise(stopProgram); // Automatische triggers voor failure mode? -> ook error message in andere functies plaatsen!
tverouden 15:6566c5dedeeb 873
tverouden 43:d332aa9f49e0 874 // ============================= PC-COMMUNICATION ============================
tverouden 42:bb43f1b67787 875 pc.baud(115200); // communication with terminal
tverouden 42:bb43f1b67787 876 pc.printf("\n\n[START] starting O.D.I.N\r\n");
tverouden 6:f32352bc5078 877
tverouden 43:d332aa9f49e0 878 // ============================= PIN DEFINE PERIOD ===========================
tverouden 42:bb43f1b67787 879 // If you give a period on one pin, c++ gives all pins this period
tverouden 42:bb43f1b67787 880 pin3.period_us(15);
tverouden 41:5aecc1a27ce6 881
tverouden 43:d332aa9f49e0 882 // ==================================== LOOP ==================================
tverouden 42:bb43f1b67787 883 // run state machine at 500 Hz
tverouden 42:bb43f1b67787 884 stateTimer.attach(&stateMachine,dt);
tverouden 43:d332aa9f49e0 885
tverouden 43:d332aa9f49e0 886 // =============================== ADD FILTERS ===============================
tverouden 43:d332aa9f49e0 887 //Make filter chain for the first EMG
tverouden 43:d332aa9f49e0 888 filter0.add(&Notch50_0).add(&Notch100_0).add(&Notch150_0).add(&Notch200_0).add(&Low_0).add(&High_0);
tverouden 43:d332aa9f49e0 889 //Make filter chain for the second EMG
tverouden 43:d332aa9f49e0 890 filter1.add(&Notch50_1).add(&Notch100_1).add(&Notch150_1).add(&Notch200_1).add(&Low_1).add(&High_1);
tverouden 42:bb43f1b67787 891 }