Jesse Lohman / Mbed 2 deprecated state_program1

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:   FastPWM MODSERIAL QEI biquadFilter mbed

Fork of state_program by Fabrice Tshilumba

main.cpp@8:5896424eb539, 2018-11-06 (annotated)

Committer:
JesseLohman
Date:
Tue Nov 06 14:45:19 2018 +0000
Revision:
8:5896424eb539
Parent:
7:1906d404cd1e 
Alles netjes gemaakt met comments. Inverse kinematics en EMG gedeelte moeten nog gecontroleerd worden

Who changed what in which revision?

UserRevisionLine numberNew contents of line
JesseLohman 0:2a5dd6cc0008 1 #include "mbed.h"
JesseLohman 0:2a5dd6cc0008 2 #include "FastPWM.h"
JesseLohman 0:2a5dd6cc0008 3 #include "QEI.h"
JesseLohman 0:2a5dd6cc0008 4 #include "BiQuad.h"
JesseLohman 3:be922ea2415f 5 #include <iostream>
JesseLohman 3:be922ea2415f 6 #include <string>
JesseLohman 0:2a5dd6cc0008 7
JesseLohman 8:5896424eb539 8 enum States {WaitState, MotorCalState, EMGCalState, HomingState, MovingState, GrippingState, ScrewingState, FailureState};
JesseLohman 0:2a5dd6cc0008 9 States currentState = WaitState;
JesseLohman 0:2a5dd6cc0008 10
JesseLohman 0:2a5dd6cc0008 11 DigitalIn startButton(D0);
JesseLohman 0:2a5dd6cc0008 12 InterruptIn failureButton(D1);
KingMufasa 7:1906d404cd1e 13 DigitalIn gripperButton(D2);
KingMufasa 7:1906d404cd1e 14 DigitalIn directionSwitch(D3);
KingMufasa 7:1906d404cd1e 15 DigitalIn gripperMotorButton(D14);
KingMufasa 7:1906d404cd1e 16
KingMufasa 5:a1843b698d0d 17
KingMufasa 5:a1843b698d0d 18 Serial pc(USBTX, USBRX);
KingMufasa 5:a1843b698d0d 19
JesseLohman 1:4cb9af313c26 20 DigitalOut led1(LED1); // Red led
JesseLohman 1:4cb9af313c26 21 DigitalOut led2(LED2); // Green led
JesseLohman 1:4cb9af313c26 22 DigitalOut led3(LED3); // Blue led
KingMufasa 6:e67250b8b100 23 QEI encoder1(D10, D11, NC, 8400, QEI::X4_ENCODING);
JesseLohman 3:be922ea2415f 24 QEI encoder2(D12, D13, NC, 8400, QEI::X4_ENCODING);
KingMufasa 5:a1843b698d0d 25
KingMufasa 5:a1843b698d0d 26 AnalogIn emg0(A0);
KingMufasa 5:a1843b698d0d 27 AnalogIn emg1(A1);
KingMufasa 5:a1843b698d0d 28 AnalogIn emg2(A2);
KingMufasa 5:a1843b698d0d 29 AnalogIn emg3(A3);
JesseLohman 0:2a5dd6cc0008 30
JesseLohman 0:2a5dd6cc0008 31 bool stateChanged = true;
JesseLohman 0:2a5dd6cc0008 32
JesseLohman 0:2a5dd6cc0008 33 Ticker mainTicker;
JesseLohman 0:2a5dd6cc0008 34 Timer stateTimer;
JesseLohman 8:5896424eb539 35 Ticker calTimer; //Ticker that waits (to prepare the person)
JesseLohman 8:5896424eb539 36 Ticker EMGsampler; //Ticker that samples the EMG
JesseLohman 2:5cace74299e7 37
JesseLohman 2:5cace74299e7 38 const double sampleTime = 0.001;
JesseLohman 2:5cace74299e7 39 const double PI = 3.141592653589793238463;
JesseLohman 3:be922ea2415f 40 const double L1 = 0.328;
JesseLohman 3:be922ea2415f 41 const double L2 = 0.218;
KingMufasa 7:1906d404cd1e 42 double T1[3][3] {
JesseLohman 3:be922ea2415f 43 {0, -1, 0},
JesseLohman 3:be922ea2415f 44 {1, 0, 0,},
JesseLohman 3:be922ea2415f 45 {0, 0, 0,}
JesseLohman 3:be922ea2415f 46 };
KingMufasa 7:1906d404cd1e 47 double T20[3][3] {
JesseLohman 3:be922ea2415f 48 {0, -1, 0},
JesseLohman 3:be922ea2415f 49 {1, 0, -L1,},
JesseLohman 3:be922ea2415f 50 {0, 0, 0,}
JesseLohman 3:be922ea2415f 51 };
KingMufasa 7:1906d404cd1e 52 double H200[3][3] {
JesseLohman 3:be922ea2415f 53 {1, 0, L1+L2},
JesseLohman 3:be922ea2415f 54 {0, 1, 0,},
JesseLohman 3:be922ea2415f 55 {0, 0, 1,}
JesseLohman 3:be922ea2415f 56 };
KingMufasa 7:1906d404cd1e 57 double Pe2 [3][1] {
JesseLohman 3:be922ea2415f 58 {0},
JesseLohman 3:be922ea2415f 59 {0},
JesseLohman 3:be922ea2415f 60 {1}
JesseLohman 3:be922ea2415f 61 };
JesseLohman 3:be922ea2415f 62
JesseLohman 8:5896424eb539 63 double u1; // Motor output of the long link
JesseLohman 8:5896424eb539 64 double u2; // Motor of the short link
JesseLohman 8:5896424eb539 65 double u3; // Motor of the gripper
JesseLohman 8:5896424eb539 66 double u4; // Motor of the screwer
JesseLohman 8:5896424eb539 67 FastPWM pwmpin1(D5); // Motor pwm
JesseLohman 8:5896424eb539 68 DigitalOut directionpin1(D4); // Motor direction
JesseLohman 3:be922ea2415f 69 FastPWM pwmpin2 (D6);
JesseLohman 3:be922ea2415f 70 DigitalOut directionpin2 (D7);
JesseLohman 8:5896424eb539 71 FastPWM pwmpin3(A4);
JesseLohman 8:5896424eb539 72 DigitalOut directionpin3(D8);
KingMufasa 7:1906d404cd1e 73 FastPWM pwmpin4(A5);
KingMufasa 7:1906d404cd1e 74 DigitalOut directionpin4(D9);
KingMufasa 7:1906d404cd1e 75
KingMufasa 7:1906d404cd1e 76 double setPointX = 0.3;
KingMufasa 7:1906d404cd1e 77 double setPointY = 0.28;
JesseLohman 3:be922ea2415f 78 double qRef1;
JesseLohman 3:be922ea2415f 79 double qRef2;
JesseLohman 3:be922ea2415f 80 double qMeas1;
JesseLohman 3:be922ea2415f 81 double qMeas2;
JesseLohman 2:5cace74299e7 82
JesseLohman 3:be922ea2415f 83 double C[5][5];
JesseLohman 3:be922ea2415f 84
KingMufasa 7:1906d404cd1e 85 double Kp1 = 10;
KingMufasa 7:1906d404cd1e 86 double Ki1 = 0;
KingMufasa 7:1906d404cd1e 87 double Kd1 = 2;
KingMufasa 7:1906d404cd1e 88 double Kp2 = 10;
KingMufasa 7:1906d404cd1e 89 double Ki2 = 0;
KingMufasa 7:1906d404cd1e 90 double Kd2 = 2;
KingMufasa 5:a1843b698d0d 91
JesseLohman 8:5896424eb539 92 const int samplepack = 250; // Amount of data points before one cycle completes
JesseLohman 8:5896424eb539 93 volatile int counter = 0; // Counts the amount of readings this cycle
JesseLohman 8:5896424eb539 94 volatile double total[4] = {0,0,0,0}; // Total difference between data points and the calibration value this cycle (x+, x-, y+, y-)
JesseLohman 8:5896424eb539 95 double refvaluebic = 10; // Minimum total value for the motor to move (biceps)
JesseLohman 8:5896424eb539 96 double refvaluecalf = 50; // Minimum total value for the motor to move (calfs)
JesseLohman 8:5896424eb539 97 double incr = 0.00002; // Increment
KingMufasa 5:a1843b698d0d 98 bool moving[4] = {0,0,0,0}; //x+, x-, y+, y-
KingMufasa 5:a1843b698d0d 99
JesseLohman 8:5896424eb539 100 volatile int waitingcounter = 0; //How many iterations of waiting have been done
JesseLohman 8:5896424eb539 101 int countermax = 20; //counter until when should be waited
JesseLohman 8:5896424eb539 102 volatile int processnow = 0; //Point in the calibration process
JesseLohman 8:5896424eb539 103 volatile double armresttot = 0;
JesseLohman 8:5896424eb539 104 volatile double armflextot = 0;
JesseLohman 8:5896424eb539 105 volatile double legresttot = 0;
JesseLohman 8:5896424eb539 106 volatile double legflextot = 0;
JesseLohman 8:5896424eb539 107 const int samplecal = 2500; //Amount of data points to consider in the calibration
JesseLohman 8:5896424eb539 108 double ratio = samplecal/samplepack;
JesseLohman 8:5896424eb539 109 volatile double tot = 0; //Total measured EMG of the current type.
JesseLohman 8:5896424eb539 110 volatile double armcal; //Calibated value for arm measurements
JesseLohman 8:5896424eb539 111 volatile double legcal; //Calibrated value for leg measurements
JesseLohman 8:5896424eb539 112
JesseLohman 8:5896424eb539 113 BiQuad hpf(0.8635,-1.7269,0.8635,-1.9359,0.9394); //5 Hz High pass filter
KingMufasa 5:a1843b698d0d 114 BiQuad notch(0.9922,-1.6054,0.9922,-1.6054,0.9844); //50 Hz Notch filter
KingMufasa 5:a1843b698d0d 115 BiQuad lpf(0.9780,1.9561,0.9780,1.9556,0.9565); //250Hz Low pass filter
KingMufasa 5:a1843b698d0d 116 BiQuadChain bqc;
JesseLohman 0:2a5dd6cc0008 117
JesseLohman 0:2a5dd6cc0008 118 void switchToFailureState ()
JesseLohman 0:2a5dd6cc0008 119 {
JesseLohman 0:2a5dd6cc0008 120 failureButton.fall(NULL); // Detaches button, so it can only be activated once
JesseLohman 1:4cb9af313c26 121 led1 = 0;
JesseLohman 1:4cb9af313c26 122 led2 = 1;
JesseLohman 1:4cb9af313c26 123 led3 = 1;
JesseLohman 1:4cb9af313c26 124 pc.printf("SYSTEM FAILED\n");
JesseLohman 0:2a5dd6cc0008 125 currentState = FailureState;
JesseLohman 0:2a5dd6cc0008 126 stateChanged = true;
JesseLohman 0:2a5dd6cc0008 127 }
JesseLohman 0:2a5dd6cc0008 128
KingMufasa 5:a1843b698d0d 129 void measureEMG ()
KingMufasa 5:a1843b698d0d 130 {
KingMufasa 7:1906d404cd1e 131 if (currentState == MovingState) {
KingMufasa 7:1906d404cd1e 132 total[0] += abs(bqc.step(emg0.read()));
KingMufasa 7:1906d404cd1e 133 total[1] += abs(bqc.step(emg1.read()));
KingMufasa 7:1906d404cd1e 134 total[2] += abs(bqc.step(emg2.read()));
KingMufasa 7:1906d404cd1e 135 total[3] += abs(bqc.step(emg3.read()));
KingMufasa 7:1906d404cd1e 136 counter++;
KingMufasa 7:1906d404cd1e 137 if (counter >= samplepack) {
KingMufasa 7:1906d404cd1e 138 moving[0] = 0;
KingMufasa 7:1906d404cd1e 139 moving[1] = 0;
KingMufasa 7:1906d404cd1e 140 moving[2] = 0;
KingMufasa 7:1906d404cd1e 141 moving[3] = 0;
KingMufasa 7:1906d404cd1e 142 if (total[0] >= refvaluebic) {
KingMufasa 7:1906d404cd1e 143 moving[0] = 1;
KingMufasa 7:1906d404cd1e 144 }
KingMufasa 7:1906d404cd1e 145 if (total[1] >= refvaluebic) {
KingMufasa 7:1906d404cd1e 146 moving[1] = 1;
KingMufasa 7:1906d404cd1e 147 }
KingMufasa 7:1906d404cd1e 148 if (total[2] >= refvaluecalf) {
KingMufasa 7:1906d404cd1e 149 moving[2] = 1;
KingMufasa 7:1906d404cd1e 150 }
KingMufasa 7:1906d404cd1e 151 if (total[3] >= refvaluecalf) {
KingMufasa 7:1906d404cd1e 152 moving[3] = 1;
KingMufasa 7:1906d404cd1e 153 }
KingMufasa 7:1906d404cd1e 154 pc.printf("Totals of {x+,x-,y+,y-} are %f, %f, %f, %f \r\n",total[0],total[1],total[2],total[3]);
KingMufasa 7:1906d404cd1e 155 pc.printf("Coordinates: (%f,%f)\r\n", setPointX, setPointY);
JesseLohman 8:5896424eb539 156 counter = 0; // Reset for next cycle
JesseLohman 8:5896424eb539 157 for (int i=0; i<4; i++) { // Clear 'total' array
KingMufasa 7:1906d404cd1e 158 total[i] = 0;
KingMufasa 7:1906d404cd1e 159 }
KingMufasa 5:a1843b698d0d 160 }
KingMufasa 7:1906d404cd1e 161
KingMufasa 7:1906d404cd1e 162 if(moving[0]) {
KingMufasa 7:1906d404cd1e 163 setPointX += incr;
KingMufasa 5:a1843b698d0d 164 }
KingMufasa 7:1906d404cd1e 165 if (moving[1]) {
KingMufasa 7:1906d404cd1e 166 setPointX -= incr;
KingMufasa 5:a1843b698d0d 167 }
KingMufasa 7:1906d404cd1e 168 if (moving[2]) {
KingMufasa 7:1906d404cd1e 169 setPointY += incr;
KingMufasa 7:1906d404cd1e 170 }
KingMufasa 7:1906d404cd1e 171 if (moving[3]) {
KingMufasa 7:1906d404cd1e 172 setPointY -= incr;
KingMufasa 5:a1843b698d0d 173 }
KingMufasa 5:a1843b698d0d 174 }
KingMufasa 5:a1843b698d0d 175 }
KingMufasa 5:a1843b698d0d 176
JesseLohman 2:5cace74299e7 177 double measureVelocity (int motor)
JesseLohman 2:5cace74299e7 178 {
JesseLohman 2:5cace74299e7 179 static double lastPulses = 0;
JesseLohman 2:5cace74299e7 180 double currentPulses;
JesseLohman 2:5cace74299e7 181 static double velocity = 0;
JesseLohman 3:be922ea2415f 182
JesseLohman 2:5cace74299e7 183 static int i = 0;
JesseLohman 2:5cace74299e7 184 if (i == 10) { // Encoder is not accurate enough, so with 1000 Hz the velocity can only be 0, 1000, 2000 or 3000 pulses/s
JesseLohman 3:be922ea2415f 185 switch (motor) { // Check which motor to measure
JesseLohman 3:be922ea2415f 186 case 1:
JesseLohman 3:be922ea2415f 187 currentPulses = encoder1.getPulses();
JesseLohman 3:be922ea2415f 188 break;
JesseLohman 3:be922ea2415f 189 case 2:
JesseLohman 8:5896424eb539 190 currentPulses = encoder2.getPulses();
JesseLohman 3:be922ea2415f 191 break;
JesseLohman 3:be922ea2415f 192 }
JesseLohman 2:5cace74299e7 193
JesseLohman 3:be922ea2415f 194 double deltaPulses = currentPulses - lastPulses;
JesseLohman 3:be922ea2415f 195 velocity = deltaPulses / (sampleTime * 10); // Velocity in pulses/s
JesseLohman 3:be922ea2415f 196 lastPulses = currentPulses;
JesseLohman 3:be922ea2415f 197 i = 0;
JesseLohman 2:5cace74299e7 198 } else {
JesseLohman 2:5cace74299e7 199 i += 1;
JesseLohman 2:5cace74299e7 200 }
JesseLohman 2:5cace74299e7 201 return velocity;
JesseLohman 2:5cace74299e7 202 }
JesseLohman 2:5cace74299e7 203
JesseLohman 3:be922ea2415f 204 void measurePosition() // Measure actual angle position with the encoder
JesseLohman 2:5cace74299e7 205 {
JesseLohman 8:5896424eb539 206 double pulses1 = encoder2.getPulses(); // motor 1 is on M2 and ENC2 of biorobotics shield
JesseLohman 8:5896424eb539 207 double pulses2 = encoder1.getPulses(); // motor 2 is on M1 and ENC1 of biorobotics shield
JesseLohman 8:5896424eb539 208 // Calculate the angle relative to the '0' point + offset (we have 8400 pulses per revolution)
JesseLohman 8:5896424eb539 209 qMeas1 = (pulses1) * 2 * PI / 8400 +140.7 * PI / 180;
JesseLohman 8:5896424eb539 210 qMeas2 = (pulses2) * 2 * PI / 8400 -87 * PI / 180;
JesseLohman 2:5cace74299e7 211
JesseLohman 3:be922ea2415f 212 }
JesseLohman 3:be922ea2415f 213
JesseLohman 3:be922ea2415f 214 template<std::size_t N, std::size_t M, std::size_t P>
JesseLohman 3:be922ea2415f 215 void mult(double A[N][M], double B[M][P])
JesseLohman 3:be922ea2415f 216 {
JesseLohman 3:be922ea2415f 217
JesseLohman 3:be922ea2415f 218 for( int n =0; n < 5; n++) {
JesseLohman 3:be922ea2415f 219 for(int p =0; p < 5; p++) {
JesseLohman 3:be922ea2415f 220 C[n][p] =0;
JesseLohman 3:be922ea2415f 221 }
JesseLohman 3:be922ea2415f 222 }
JesseLohman 3:be922ea2415f 223 for (int n = 0; n < N; n++) {
JesseLohman 3:be922ea2415f 224 for (int p = 0; p < P; p++) {
JesseLohman 3:be922ea2415f 225 double num = 0;
JesseLohman 3:be922ea2415f 226 for (int m = 0; m < M; m++) {
JesseLohman 3:be922ea2415f 227 num += A[n][m] * B[m][p];
JesseLohman 3:be922ea2415f 228
JesseLohman 3:be922ea2415f 229 }
JesseLohman 3:be922ea2415f 230
JesseLohman 3:be922ea2415f 231 C[n][p] = num;
JesseLohman 3:be922ea2415f 232
JesseLohman 3:be922ea2415f 233 }
JesseLohman 3:be922ea2415f 234 }
JesseLohman 3:be922ea2415f 235
JesseLohman 3:be922ea2415f 236 }
JesseLohman 3:be922ea2415f 237
JesseLohman 3:be922ea2415f 238 void inverseKinematics ()
JesseLohman 3:be922ea2415f 239 {
JesseLohman 8:5896424eb539 240 if (currentState == MovingState) { // Only in the MovingState should the qRef1, qRef2 change every sampleTime
KingMufasa 7:1906d404cd1e 241
JesseLohman 8:5896424eb539 242 double Pe_set[3][1] { // Defining setpoint location of end effector
JesseLohman 8:5896424eb539 243 {setPointX}, // Setting xcoord to setPointX
JesseLohman 8:5896424eb539 244 {setPointY}, // Setting ycoord to setPointY
JesseLohman 3:be922ea2415f 245 {1}
JesseLohman 3:be922ea2415f 246 };
JesseLohman 3:be922ea2415f 247
JesseLohman 8:5896424eb539 248 // Calculating new H matrix
JesseLohman 3:be922ea2415f 249 double T1e[3][3] {
JesseLohman 3:be922ea2415f 250 {cos(qRef1), -sin(qRef1), 0},
JesseLohman 3:be922ea2415f 251 {sin(qRef1), cos(qRef1), 0},
JesseLohman 3:be922ea2415f 252 {0, 0, 1}
JesseLohman 3:be922ea2415f 253 };
JesseLohman 3:be922ea2415f 254 double T20e[3][3] {
JesseLohman 3:be922ea2415f 255 {cos(qRef2), -sin(qRef2), L1-L1*cos(qRef2)},
JesseLohman 3:be922ea2415f 256 {sin(qRef2), cos(qRef2), -L1*sin(qRef2)},
JesseLohman 3:be922ea2415f 257 {0, 0, 1}
JesseLohman 3:be922ea2415f 258 };
JesseLohman 3:be922ea2415f 259
JesseLohman 3:be922ea2415f 260
JesseLohman 8:5896424eb539 261 mult<3,3,3>(T1e,T20e); // Matrix multiplication
JesseLohman 3:be922ea2415f 262 double H201[3][3] {
JesseLohman 3:be922ea2415f 263 {C[0][0],C[0][1],C[0][2]},
JesseLohman 3:be922ea2415f 264 {C[1][0],C[1][1],C[1][2]},
JesseLohman 3:be922ea2415f 265 {C[2][0],C[2][1],C[2][2]}
JesseLohman 3:be922ea2415f 266 };
JesseLohman 3:be922ea2415f 267
JesseLohman 8:5896424eb539 268 mult<3,3,3>(H201,H200); // Matrix multiplication
JesseLohman 3:be922ea2415f 269 double H20 [3][3] {
JesseLohman 3:be922ea2415f 270 {C[0][0],C[0][1],C[0][2]},
JesseLohman 3:be922ea2415f 271 {C[1][0],C[1][1],C[1][2]},
JesseLohman 3:be922ea2415f 272 {C[2][0],C[2][1],C[2][2]}
JesseLohman 3:be922ea2415f 273 };
JesseLohman 3:be922ea2415f 274
JesseLohman 8:5896424eb539 275 mult<3,3,1>(H20,Pe2); // Matrix multiplication
JesseLohman 3:be922ea2415f 276 double Pe0[3][1] {
JesseLohman 3:be922ea2415f 277 {C[0][0]},
JesseLohman 3:be922ea2415f 278 {C[1][0]},
JesseLohman 3:be922ea2415f 279 {C[2][0]}
JesseLohman 3:be922ea2415f 280 };
JesseLohman 3:be922ea2415f 281
JesseLohman 8:5896424eb539 282 double pe0x = Pe0[0][0]; // Seperating coordinates of end effector location
JesseLohman 3:be922ea2415f 283 double pe0y = Pe0[1][0];
JesseLohman 3:be922ea2415f 284
JesseLohman 3:be922ea2415f 285 // Determing the jacobian
JesseLohman 3:be922ea2415f 286
JesseLohman 3:be922ea2415f 287 double T_1[3][1] {
JesseLohman 3:be922ea2415f 288 {1},
JesseLohman 3:be922ea2415f 289 {T1[0][2]},
JesseLohman 3:be922ea2415f 290 {T1[1][2]}
JesseLohman 3:be922ea2415f 291 };
JesseLohman 3:be922ea2415f 292
JesseLohman 3:be922ea2415f 293 double T_2[3][1] {
JesseLohman 3:be922ea2415f 294 {1},
JesseLohman 3:be922ea2415f 295 {L1*sin(qRef1)},
JesseLohman 3:be922ea2415f 296 {-L1*cos(qRef1)}
JesseLohman 3:be922ea2415f 297 };
JesseLohman 3:be922ea2415f 298
JesseLohman 3:be922ea2415f 299 double J[3][2] {
JesseLohman 3:be922ea2415f 300 {T_1[0][0], T_2[0][0]},
JesseLohman 3:be922ea2415f 301 {T_1[1][0], T_2[1][0]},
JesseLohman 3:be922ea2415f 302 {T_1[2][0], T_2[2][0]}
JesseLohman 3:be922ea2415f 303 };
JesseLohman 3:be922ea2415f 304
JesseLohman 8:5896424eb539 305 // Determing 'Pulling" force to setpoint
JesseLohman 3:be922ea2415f 306
JesseLohman 8:5896424eb539 307 double kspring= 0.1; // Virtual stifness of the force
JesseLohman 8:5896424eb539 308 double Fs[3][1] { // Force vector from end effector to setpoint
KingMufasa 6:e67250b8b100 309 {kspring*Pe_set[0][0] - kspring*Pe0[0][0]},
KingMufasa 6:e67250b8b100 310 {kspring*Pe_set[1][0] - kspring*Pe0[1][0]},
KingMufasa 6:e67250b8b100 311 {kspring*Pe_set[2][0] - kspring*Pe0[2][0]}
JesseLohman 3:be922ea2415f 312 };
KingMufasa 6:e67250b8b100 313 double Fx = kspring*setPointX - kspring*pe0x;
KingMufasa 6:e67250b8b100 314 double Fy = kspring*setPointY - kspring*pe0y;
JesseLohman 3:be922ea2415f 315 double W0t[3][1] {
JesseLohman 3:be922ea2415f 316 {pe0x*Fy - pe0y*Fx},
JesseLohman 3:be922ea2415f 317 {Fx},
JesseLohman 3:be922ea2415f 318 {Fy}
JesseLohman 3:be922ea2415f 319 };
JesseLohman 3:be922ea2415f 320
JesseLohman 8:5896424eb539 321 double Jt[2][3] { // Transposing jacobian matrix
JesseLohman 3:be922ea2415f 322 {J[0][0], J[1][0], J[2][0]},
JesseLohman 3:be922ea2415f 323 {T_2[0][0], T_2[1][0], T_2[2][0]}
JesseLohman 3:be922ea2415f 324 };
JesseLohman 3:be922ea2415f 325
JesseLohman 3:be922ea2415f 326 mult<2,3,1>(Jt,W0t);
JesseLohman 3:be922ea2415f 327 double tau_st1 = C[0][0];
JesseLohman 3:be922ea2415f 328 double tau_st2 = C[1][0];
JesseLohman 3:be922ea2415f 329
JesseLohman 8:5896424eb539 330 // Calculating joint behaviour
JesseLohman 3:be922ea2415f 331
KingMufasa 7:1906d404cd1e 332 double b1 =1;
KingMufasa 5:a1843b698d0d 333 double b2 =1;
JesseLohman 8:5896424eb539 334 // Joint friction coefficent
JesseLohman 8:5896424eb539 335 double w_s1 = tau_st1/b1; // Angular velocity
JesseLohman 8:5896424eb539 336 double w_s2 = tau_st2/b2; // Angular velocity
JesseLohman 8:5896424eb539 337 // Checking angle boundaries
JesseLohman 8:5896424eb539 338 qRef1 = qRef1 +w_s1*1; // Calculating new angle of qRef1 in time step sampleTime
JesseLohman 3:be922ea2415f 339 if (qRef1 > 2*PI/3) {
JesseLohman 3:be922ea2415f 340 qRef1 = 2*PI/3;
JesseLohman 3:be922ea2415f 341 } else if (qRef1 < PI/6) {
JesseLohman 3:be922ea2415f 342 qRef1= PI/6;
JesseLohman 3:be922ea2415f 343 }
JesseLohman 3:be922ea2415f 344
JesseLohman 8:5896424eb539 345 qRef2 = qRef2 + w_s2*1; // Calculating new angle of qRef2 in time step sampleTime
JesseLohman 3:be922ea2415f 346 if (qRef2 > -PI/4) {
JesseLohman 3:be922ea2415f 347 qRef2 = -PI/4;
JesseLohman 3:be922ea2415f 348 } else if (qRef2 < -PI/2) {
JesseLohman 3:be922ea2415f 349 qRef2= -PI/2;
JesseLohman 3:be922ea2415f 350 }
JesseLohman 3:be922ea2415f 351 }
JesseLohman 3:be922ea2415f 352 }
JesseLohman 3:be922ea2415f 353 void PID_controller() // Put the error trough PID control to make output 'u'
JesseLohman 3:be922ea2415f 354 {
KingMufasa 7:1906d404cd1e 355 if (currentState >= HomingState && currentState < FailureState) {
KingMufasa 7:1906d404cd1e 356 // Should only work when we move the robot to a defined position
JesseLohman 8:5896424eb539 357 double error1 = qRef1 - qMeas1;
JesseLohman 8:5896424eb539 358 double error2 = qRef2 - qMeas2;
JesseLohman 3:be922ea2415f 359
JesseLohman 3:be922ea2415f 360 static double errorIntegral1 = 0;
JesseLohman 3:be922ea2415f 361 static double errorIntegral2 = 0;
JesseLohman 3:be922ea2415f 362 static double errorPrev1 = error1;
JesseLohman 3:be922ea2415f 363 static double errorPrev2 = error2;
JesseLohman 3:be922ea2415f 364 static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
JesseLohman 3:be922ea2415f 365
KingMufasa 4:8f25ecb74221 366 // Proportional part:
KingMufasa 4:8f25ecb74221 367 double u_k1 = Kp1 * error1;
KingMufasa 4:8f25ecb74221 368 double u_k2 = Kp2 * error2;
JesseLohman 3:be922ea2415f 369
JesseLohman 3:be922ea2415f 370 //Integral part:
JesseLohman 3:be922ea2415f 371 errorIntegral1 = errorIntegral1 + error1 * sampleTime;
KingMufasa 4:8f25ecb74221 372 double u_i1 = Ki1 * errorIntegral1;
JesseLohman 3:be922ea2415f 373 errorIntegral2 = errorIntegral2 + error2 * sampleTime;
KingMufasa 4:8f25ecb74221 374 double u_i2 = Ki2 * errorIntegral2;
JesseLohman 3:be922ea2415f 375
JesseLohman 3:be922ea2415f 376 // Derivative part
JesseLohman 3:be922ea2415f 377 double errorDerivative1 = (error1 - errorPrev1)/sampleTime;
JesseLohman 3:be922ea2415f 378 double filteredErrorDerivative1 = LowPassFilter.step(errorDerivative1);
KingMufasa 4:8f25ecb74221 379 double u_d1 = Kd1 * filteredErrorDerivative1;
JesseLohman 3:be922ea2415f 380 errorPrev1 = error1;
JesseLohman 3:be922ea2415f 381 double errorDerivative2 = (error2 - errorPrev2)/sampleTime;
JesseLohman 3:be922ea2415f 382 double filteredErrorDerivative2 = LowPassFilter.step(errorDerivative2);
KingMufasa 4:8f25ecb74221 383 double u_d2 = Kd2 * filteredErrorDerivative2;
JesseLohman 3:be922ea2415f 384 errorPrev2 = error2;
JesseLohman 3:be922ea2415f 385
JesseLohman 8:5896424eb539 386 // Sum all parts
JesseLohman 3:be922ea2415f 387 u1 = u_k1 + u_i1 + u_d1;
JesseLohman 3:be922ea2415f 388 u2 = u_k2 + u_i2 + u_d2;
KingMufasa 5:a1843b698d0d 389 }
JesseLohman 2:5cace74299e7 390 }
JesseLohman 2:5cace74299e7 391
JesseLohman 2:5cace74299e7 392 void controlMotor () // Control direction and speed
JesseLohman 2:5cace74299e7 393 {
JesseLohman 2:5cace74299e7 394 directionpin1 = u1 > 0.0f; // Either true or false
JesseLohman 2:5cace74299e7 395 pwmpin1 = fabs(u1);
JesseLohman 3:be922ea2415f 396 directionpin2 = u2 > 0.0f; // Either true or false
JesseLohman 3:be922ea2415f 397 pwmpin2 = fabs(u2);
KingMufasa 7:1906d404cd1e 398 directionpin3 = u3 > 0.0f; // Either true or false
KingMufasa 7:1906d404cd1e 399 pwmpin3 = fabs(u3);
KingMufasa 7:1906d404cd1e 400 directionpin4 = u4 > 0.0f; // Either true or false
KingMufasa 7:1906d404cd1e 401 pwmpin4 = fabs(u4);
JesseLohman 2:5cace74299e7 402 }
JesseLohman 2:5cace74299e7 403
JesseLohman 8:5896424eb539 404 void EMGwaiting(){ //Flashes LED
JesseLohman 8:5896424eb539 405 led3 = !led3;
JesseLohman 8:5896424eb539 406 waitingcounter++;
JesseLohman 8:5896424eb539 407 }
JesseLohman 8:5896424eb539 408
JesseLohman 8:5896424eb539 409 void EMGsampling(){ //Ticker function of EMG
JesseLohman 8:5896424eb539 410 switch(processnow){
JesseLohman 8:5896424eb539 411 case 0:
JesseLohman 8:5896424eb539 412 case 1:
JesseLohman 8:5896424eb539 413 tot += abs(bqc.step(emg0.read()));
JesseLohman 8:5896424eb539 414 break;
JesseLohman 8:5896424eb539 415 case 2:
JesseLohman 8:5896424eb539 416 case 3:
JesseLohman 8:5896424eb539 417 tot += abs(bqc.step(emg2.read()));
JesseLohman 8:5896424eb539 418 break;
JesseLohman 8:5896424eb539 419 }
JesseLohman 8:5896424eb539 420 counter++;
JesseLohman 8:5896424eb539 421 if (counter >= samplecal){
JesseLohman 8:5896424eb539 422 tot = tot/ratio;
JesseLohman 8:5896424eb539 423 switch(processnow){
JesseLohman 8:5896424eb539 424 case 0:
JesseLohman 8:5896424eb539 425 armresttot = tot;
JesseLohman 8:5896424eb539 426 pc.printf("<Result> Average area of arm signal in rest: %f.\r\n",armresttot);
JesseLohman 8:5896424eb539 427 break;
JesseLohman 8:5896424eb539 428 case 1:
JesseLohman 8:5896424eb539 429 armflextot = tot;
JesseLohman 8:5896424eb539 430 pc.printf("<Result>Average area of arm signal while flexing: %f.\r\n",armflextot);
JesseLohman 8:5896424eb539 431 armcal = (armflextot + armresttot)/2.0;
JesseLohman 8:5896424eb539 432 pc.printf("<Result> Calibration value for arms determined at %f.\r\n",armcal);
JesseLohman 8:5896424eb539 433 break;
JesseLohman 8:5896424eb539 434 case 2:
JesseLohman 8:5896424eb539 435 legresttot = tot;
JesseLohman 8:5896424eb539 436 pc.printf("<Result> Average area of leg signal in rest: %f.\r\n",legresttot);
JesseLohman 8:5896424eb539 437 break;
JesseLohman 8:5896424eb539 438 case 3:
JesseLohman 8:5896424eb539 439 legflextot = tot;
JesseLohman 8:5896424eb539 440 pc.printf("<Result> Average area of leg signal while flexing: %f.\r\n",legflextot);
JesseLohman 8:5896424eb539 441 legcal = (legflextot + legresttot)/2;
JesseLohman 8:5896424eb539 442 pc.printf("<Result> Calibration value for legs determined at %f.\r\n",legcal);
JesseLohman 8:5896424eb539 443 break;
JesseLohman 8:5896424eb539 444 }
JesseLohman 8:5896424eb539 445 processnow++;
JesseLohman 8:5896424eb539 446 tot = 0;
JesseLohman 8:5896424eb539 447 counter = 0;
JesseLohman 8:5896424eb539 448 }
JesseLohman 8:5896424eb539 449 }
JesseLohman 8:5896424eb539 450
JesseLohman 0:2a5dd6cc0008 451 void stateMachine ()
JesseLohman 0:2a5dd6cc0008 452 {
JesseLohman 0:2a5dd6cc0008 453 switch (currentState) {
JesseLohman 0:2a5dd6cc0008 454 case WaitState:
JesseLohman 0:2a5dd6cc0008 455 if (stateChanged == true) {
JesseLohman 8:5896424eb539 456 // Everything that needs to be done when this state is entered the first time
JesseLohman 8:5896424eb539 457 led1 = 0; // Green
JesseLohman 1:4cb9af313c26 458 led2 = 1;
JesseLohman 1:4cb9af313c26 459 led3 = 1;
JesseLohman 2:5cace74299e7 460 u1 = 0; // Turn off all motors
JesseLohman 2:5cace74299e7 461 u2 = 0;
JesseLohman 2:5cace74299e7 462 u3 = 0;
JesseLohman 2:5cace74299e7 463 u4 = 0;
JesseLohman 0:2a5dd6cc0008 464 stateChanged = false;
JesseLohman 0:2a5dd6cc0008 465 }
JesseLohman 0:2a5dd6cc0008 466
JesseLohman 2:5cace74299e7 467 if (startButton.read() == false) { // When button is pressed, value is false
JesseLohman 8:5896424eb539 468 // Everything that needs to be done when leaving this state
JesseLohman 1:4cb9af313c26 469 led1 = 1;
JesseLohman 0:2a5dd6cc0008 470 currentState = MotorCalState;
JesseLohman 0:2a5dd6cc0008 471 stateChanged = true;
JesseLohman 0:2a5dd6cc0008 472 }
JesseLohman 0:2a5dd6cc0008 473
JesseLohman 0:2a5dd6cc0008 474 break;
JesseLohman 0:2a5dd6cc0008 475 case MotorCalState:
JesseLohman 0:2a5dd6cc0008 476 if (stateChanged == true) {
JesseLohman 8:5896424eb539 477 led2 = 0; // Red
JesseLohman 8:5896424eb539 478 u1 = 0.3; // Motor is set to 'low' value
JesseLohman 8:5896424eb539 479 u2 = -0.3;
JesseLohman 0:2a5dd6cc0008 480 stateTimer.reset();
JesseLohman 0:2a5dd6cc0008 481 stateTimer.start();
JesseLohman 0:2a5dd6cc0008 482 stateChanged = false;
JesseLohman 0:2a5dd6cc0008 483 }
JesseLohman 0:2a5dd6cc0008 484
JesseLohman 8:5896424eb539 485 // The robot can only go to the next state after 3s, when the motor velocities are close to 0 and the start button is pressed
JesseLohman 8:5896424eb539 486 if (stateTimer >= 3.0f && fabs(measureVelocity(1)) < 100 && fabs(measureVelocity(2)) < 100 && startButton.read() == false) {
JesseLohman 1:4cb9af313c26 487 led2 = 1;
JesseLohman 2:5cace74299e7 488 encoder1.reset(); // Reset encoder for the 0 position
JesseLohman 8:5896424eb539 489 encoder2.reset();
JesseLohman 0:2a5dd6cc0008 490 currentState = EMGCalState;
JesseLohman 0:2a5dd6cc0008 491 stateChanged = true;
JesseLohman 2:5cace74299e7 492 u1 = 0; // Turn off motors
JesseLohman 2:5cace74299e7 493 u2 = 0;
JesseLohman 0:2a5dd6cc0008 494 }
JesseLohman 0:2a5dd6cc0008 495 break;
JesseLohman 0:2a5dd6cc0008 496 case EMGCalState:
JesseLohman 0:2a5dd6cc0008 497 if (stateChanged == true) {
JesseLohman 8:5896424eb539 498 led3 = 0; // Blue
JesseLohman 0:2a5dd6cc0008 499 stateChanged = false;
JesseLohman 8:5896424eb539 500
JesseLohman 8:5896424eb539 501 pc.printf("<----------Now entering EMG calibration state ---------->\r\n");
JesseLohman 8:5896424eb539 502 pc.printf("<Waiting> Get ready for calibration of resting arms.\r\n");
JesseLohman 8:5896424eb539 503 calTimer.attach(&EMGwaiting,0.5); //Wait
JesseLohman 8:5896424eb539 504 while (waitingcounter < countermax) {}
JesseLohman 8:5896424eb539 505 waitingcounter = 0;
JesseLohman 8:5896424eb539 506 calTimer.detach();
JesseLohman 8:5896424eb539 507
JesseLohman 8:5896424eb539 508 pc.printf("<Measuring> Measuring arm signal at rest. Hold still.\r\n");
JesseLohman 8:5896424eb539 509 EMGsampler.attach(&EMGsampling,sampleTime); //Measure arms rest
JesseLohman 8:5896424eb539 510 while (processnow == 0) {}
JesseLohman 8:5896424eb539 511 EMGsampler.detach();
JesseLohman 8:5896424eb539 512
JesseLohman 8:5896424eb539 513 pc.printf("<Waiting> Get ready for calibration of flexing arms.\r\n");
JesseLohman 8:5896424eb539 514 calTimer.attach(&EMGwaiting,0.5); // Wait
JesseLohman 8:5896424eb539 515 while (waitingcounter < countermax) {}
JesseLohman 8:5896424eb539 516 waitingcounter = 0;
JesseLohman 8:5896424eb539 517 calTimer.detach();
JesseLohman 8:5896424eb539 518
JesseLohman 8:5896424eb539 519 pc.printf("<Measuring> Measuring arm signal while flexing.\r\n");
JesseLohman 8:5896424eb539 520 EMGsampler.attach(&EMGsampling,sampleTime); //Measure arms flexing
JesseLohman 8:5896424eb539 521 while (processnow == 1) {}
JesseLohman 8:5896424eb539 522 EMGsampler.detach();
JesseLohman 8:5896424eb539 523
JesseLohman 8:5896424eb539 524 pc.printf("<Waiting> Get ready for calibration of resting legs.\r\n");
JesseLohman 8:5896424eb539 525 calTimer.attach(&EMGwaiting,0.5); //Wait
JesseLohman 8:5896424eb539 526 while (waitingcounter < countermax) {}
JesseLohman 8:5896424eb539 527 waitingcounter = 0;
JesseLohman 8:5896424eb539 528 calTimer.detach();
JesseLohman 8:5896424eb539 529
JesseLohman 8:5896424eb539 530 pc.printf("<Measuring> Measuring leg signal at rest. Hold still.\r\n");
JesseLohman 8:5896424eb539 531 EMGsampler.attach(&EMGsampling,sampleTime); //Measure legs rest
JesseLohman 8:5896424eb539 532 while (processnow == 2) {}
JesseLohman 8:5896424eb539 533 EMGsampler.detach();
JesseLohman 8:5896424eb539 534
JesseLohman 8:5896424eb539 535 pc.printf("<Waiting> Get ready for calibration of flexing legs.\r\n");
JesseLohman 8:5896424eb539 536 calTimer.attach(&EMGwaiting,0.5); // Wait
JesseLohman 8:5896424eb539 537 while (waitingcounter < countermax) {}
JesseLohman 8:5896424eb539 538 waitingcounter = 0;
JesseLohman 8:5896424eb539 539 calTimer.detach();
JesseLohman 8:5896424eb539 540
JesseLohman 8:5896424eb539 541 pc.printf("<Measuring> Measuring leg signal while flexing.\r\n");
JesseLohman 8:5896424eb539 542 EMGsampler.attach(&EMGsampling,sampleTime); //Measure legs flexing
JesseLohman 8:5896424eb539 543 while (processnow == 3) {}
JesseLohman 8:5896424eb539 544 EMGsampler.detach();
JesseLohman 8:5896424eb539 545
JesseLohman 8:5896424eb539 546 pc.printf("<Result> EMG calibrations complete.\r\n");
JesseLohman 0:2a5dd6cc0008 547 }
JesseLohman 0:2a5dd6cc0008 548
JesseLohman 0:2a5dd6cc0008 549 if (stateTimer >= 3.0f) {
JesseLohman 1:4cb9af313c26 550 led3 = 1;
JesseLohman 0:2a5dd6cc0008 551 currentState = HomingState;
JesseLohman 0:2a5dd6cc0008 552 stateChanged = true;
JesseLohman 0:2a5dd6cc0008 553 }
JesseLohman 0:2a5dd6cc0008 554 break;
JesseLohman 0:2a5dd6cc0008 555 case HomingState:
JesseLohman 0:2a5dd6cc0008 556 if (stateChanged == true) {
JesseLohman 8:5896424eb539 557 led1 = 0; // Yellow
JesseLohman 8:5896424eb539 558 led2 = 0;
JesseLohman 8:5896424eb539 559 // qRef1 and qRef2 are set, so the motors will move to that position
KingMufasa 7:1906d404cd1e 560 qRef1 = 60 * PI / 180;
JesseLohman 8:5896424eb539 561 qRef2 = -90 * PI / 180;
JesseLohman 0:2a5dd6cc0008 562 stateChanged = false;
JesseLohman 0:2a5dd6cc0008 563 }
JesseLohman 0:2a5dd6cc0008 564
JesseLohman 1:4cb9af313c26 565 if (startButton.read() == false) { //TODO: Also add position condition
JesseLohman 1:4cb9af313c26 566 led1 = 1;
JesseLohman 1:4cb9af313c26 567 led2 = 1;
JesseLohman 0:2a5dd6cc0008 568 currentState = MovingState;
JesseLohman 0:2a5dd6cc0008 569 stateChanged = true;
JesseLohman 0:2a5dd6cc0008 570 }
JesseLohman 0:2a5dd6cc0008 571 break;
JesseLohman 0:2a5dd6cc0008 572 case MovingState:
JesseLohman 1:4cb9af313c26 573 if (stateChanged == true) {
JesseLohman 8:5896424eb539 574 led1 = 0; // White
JesseLohman 1:4cb9af313c26 575 led2 = 0;
JesseLohman 8:5896424eb539 576 led3 = 0;
JesseLohman 1:4cb9af313c26 577 stateChanged = false;
JesseLohman 1:4cb9af313c26 578 }
JesseLohman 1:4cb9af313c26 579
KingMufasa 7:1906d404cd1e 580 if (gripperButton.read() == false) {
JesseLohman 0:2a5dd6cc0008 581 led1 = 1;
JesseLohman 0:2a5dd6cc0008 582 led2 = 1;
JesseLohman 0:2a5dd6cc0008 583 led3 = 1;
JesseLohman 1:4cb9af313c26 584 currentState = GrippingState;
JesseLohman 1:4cb9af313c26 585 stateChanged = true;
JesseLohman 1:4cb9af313c26 586 }
JesseLohman 1:4cb9af313c26 587
JesseLohman 1:4cb9af313c26 588 break;
JesseLohman 1:4cb9af313c26 589 case GrippingState:
JesseLohman 1:4cb9af313c26 590 if (stateChanged == true) {
JesseLohman 8:5896424eb539 591 led2 = 0; // Light blue
JesseLohman 8:5896424eb539 592 led3 = 0;
KingMufasa 7:1906d404cd1e 593 stateTimer.reset();
KingMufasa 7:1906d404cd1e 594 stateTimer.start();
JesseLohman 0:2a5dd6cc0008 595 stateChanged = false;
JesseLohman 0:2a5dd6cc0008 596 }
JesseLohman 1:4cb9af313c26 597
KingMufasa 7:1906d404cd1e 598 if (gripperMotorButton.read() == false) {
JesseLohman 8:5896424eb539 599 // Gripper motor is activated
KingMufasa 7:1906d404cd1e 600 u3 = 0.4;
KingMufasa 7:1906d404cd1e 601 if (directionSwitch == true) {
KingMufasa 7:1906d404cd1e 602 // Close gripper, so positive direction
KingMufasa 7:1906d404cd1e 603 } else {
KingMufasa 7:1906d404cd1e 604 // Open gripper
KingMufasa 7:1906d404cd1e 605 u3 = u3 * -1;
KingMufasa 7:1906d404cd1e 606 }
KingMufasa 7:1906d404cd1e 607 } else { // If the button isn't pressed, turn off motor
KingMufasa 7:1906d404cd1e 608 u3 = 0;
JesseLohman 1:4cb9af313c26 609 }
JesseLohman 1:4cb9af313c26 610
JesseLohman 8:5896424eb539 611 // Due to the lack of buttons, we use a delay, so that when we don't immediately go to the next state when we enter this state
KingMufasa 7:1906d404cd1e 612 if (gripperButton.read() == false && stateTimer >= 2.0f) {
JesseLohman 1:4cb9af313c26 613 led2 = 1;
JesseLohman 1:4cb9af313c26 614 led3 = 1;
KingMufasa 7:1906d404cd1e 615 u3 = 0;
JesseLohman 1:4cb9af313c26 616 currentState = ScrewingState;
JesseLohman 1:4cb9af313c26 617 stateChanged = true;
JesseLohman 1:4cb9af313c26 618 }
JesseLohman 1:4cb9af313c26 619 if (startButton.read() == false) {
JesseLohman 1:4cb9af313c26 620 led2 = 1;
JesseLohman 1:4cb9af313c26 621 led3 = 1;
KingMufasa 7:1906d404cd1e 622 u3 = 0;
JesseLohman 1:4cb9af313c26 623 currentState = MovingState;
JesseLohman 1:4cb9af313c26 624 stateChanged = true;
JesseLohman 1:4cb9af313c26 625 }
JesseLohman 0:2a5dd6cc0008 626 break;
JesseLohman 0:2a5dd6cc0008 627 case ScrewingState:
JesseLohman 1:4cb9af313c26 628 if (stateChanged == true) {
JesseLohman 1:4cb9af313c26 629 // Entry action: all the things you do once in this state;
JesseLohman 8:5896424eb539 630 led1 = 0; // Pink
JesseLohman 8:5896424eb539 631 led3 = 0;
JesseLohman 1:4cb9af313c26 632 stateChanged = false;
JesseLohman 1:4cb9af313c26 633 }
JesseLohman 2:5cace74299e7 634
KingMufasa 7:1906d404cd1e 635 if (gripperMotorButton.read() == false) {
JesseLohman 8:5896424eb539 636 u4 = 0.3; // Screwing motor is activated
JesseLohman 8:5896424eb539 637 u3 = -0.35; // Gripper motor is activated with a slightly higher output to make sure the gripper doesn't open
KingMufasa 7:1906d404cd1e 638 if (directionSwitch == true) {
KingMufasa 7:1906d404cd1e 639 // Screw
KingMufasa 7:1906d404cd1e 640 } else {
KingMufasa 7:1906d404cd1e 641 // Unscrew
KingMufasa 7:1906d404cd1e 642 u4 = u4 * -1;
KingMufasa 7:1906d404cd1e 643 u3 = u3 * -1;
KingMufasa 7:1906d404cd1e 644 }
JesseLohman 8:5896424eb539 645 } else { // If the button isn't pressed, turn off motors
KingMufasa 7:1906d404cd1e 646 u4 = 0;
KingMufasa 7:1906d404cd1e 647 u3 = 0;
JesseLohman 1:4cb9af313c26 648 }
JesseLohman 2:5cace74299e7 649
JesseLohman 1:4cb9af313c26 650 if (startButton.read() == false) {
JesseLohman 1:4cb9af313c26 651 led1 = 1;
JesseLohman 1:4cb9af313c26 652 led3 = 1;
JesseLohman 8:5896424eb539 653 u3 = 0; // Turn off motors
KingMufasa 7:1906d404cd1e 654 u4 = 0;
JesseLohman 1:4cb9af313c26 655 currentState = MovingState;
JesseLohman 1:4cb9af313c26 656 stateChanged = true;
JesseLohman 1:4cb9af313c26 657 }
JesseLohman 0:2a5dd6cc0008 658 break;
JesseLohman 0:2a5dd6cc0008 659 case FailureState:
JesseLohman 0:2a5dd6cc0008 660 if (stateChanged == true) {
JesseLohman 0:2a5dd6cc0008 661 // Entry action: all the things you do once in this state
JesseLohman 2:5cace74299e7 662 u1 = 0; // Turn off all motors
JesseLohman 2:5cace74299e7 663 u2 = 0;
JesseLohman 2:5cace74299e7 664 u3 = 0;
JesseLohman 2:5cace74299e7 665 u4 = 0;
JesseLohman 0:2a5dd6cc0008 666 stateChanged = false;
JesseLohman 0:2a5dd6cc0008 667 }
JesseLohman 1:4cb9af313c26 668
KingMufasa 7:1906d404cd1e 669 static double blinkTimer2 = 0;
JesseLohman 8:5896424eb539 670 if (blinkTimer2 >= 0.5) { // The red LED is flashing at 1 Hz
JesseLohman 1:4cb9af313c26 671 led1 = !led1;
KingMufasa 7:1906d404cd1e 672 blinkTimer2 = 0;
JesseLohman 1:4cb9af313c26 673 }
KingMufasa 7:1906d404cd1e 674 blinkTimer2 += sampleTime;
JesseLohman 1:4cb9af313c26 675 break;
JesseLohman 0:2a5dd6cc0008 676 }
JesseLohman 0:2a5dd6cc0008 677 }
JesseLohman 0:2a5dd6cc0008 678
JesseLohman 3:be922ea2415f 679 void measureAll ()
JesseLohman 3:be922ea2415f 680 {
JesseLohman 3:be922ea2415f 681 measurePosition();
KingMufasa 5:a1843b698d0d 682 measureEMG();
JesseLohman 3:be922ea2415f 683 inverseKinematics();
JesseLohman 3:be922ea2415f 684 }
JesseLohman 3:be922ea2415f 685
JesseLohman 8:5896424eb539 686 void mainLoop () // All the major functions are looped every 0.001s
JesseLohman 0:2a5dd6cc0008 687 {
JesseLohman 2:5cace74299e7 688 measureAll();
JesseLohman 0:2a5dd6cc0008 689 stateMachine();
JesseLohman 3:be922ea2415f 690 PID_controller();
JesseLohman 2:5cace74299e7 691 controlMotor();
JesseLohman 0:2a5dd6cc0008 692 }
JesseLohman 0:2a5dd6cc0008 693
JesseLohman 0:2a5dd6cc0008 694 int main()
JesseLohman 0:2a5dd6cc0008 695 {
JesseLohman 8:5896424eb539 696 startButton.mode(PullUp); // Makes the button active
KingMufasa 7:1906d404cd1e 697 failureButton.mode(PullUp);
KingMufasa 7:1906d404cd1e 698 gripperButton.mode(PullUp);
KingMufasa 7:1906d404cd1e 699 directionSwitch.mode(PullUp);
KingMufasa 7:1906d404cd1e 700 gripperMotorButton.mode(PullUp);
KingMufasa 5:a1843b698d0d 701 pc.baud(115200);
JesseLohman 8:5896424eb539 702 bqc.add(&hpf).add(&notch).add(&lpf); // Add bqfilters to bqc
JesseLohman 0:2a5dd6cc0008 703 mainTicker.attach(mainLoop, sampleTime);
JesseLohman 0:2a5dd6cc0008 704 failureButton.fall(&switchToFailureState); // When button is pressed FailureState is activated
JesseLohman 0:2a5dd6cc0008 705
JesseLohman 0:2a5dd6cc0008 706 while (true) {
JesseLohman 0:2a5dd6cc0008 707 }
JesseLohman 0:2a5dd6cc0008 708 }