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@38:317b8eaaf1a2, 2018-11-01 (annotated)

Committer:
efvanmarrewijk
Date:
Thu Nov 01 14:46:50 2018 +0000
Revision:
38:317b8eaaf1a2
Parent:
37:317e14b9d551
Child:
42:bb43f1b67787
Kinematics connected to motor movement

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