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@7:1906d404cd1e, 2018-11-06 (annotated)

Committer:
KingMufasa
Date:
Tue Nov 06 10:32:00 2018 +0000
Revision:
7:1906d404cd1e
Parent:
6:e67250b8b100
Child:
8:5896424eb539
newest version

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
KingMufasa 7:1906d404cd1e 8 enum States {WaitState, MotorCalState, EMGCalState, HomingState, DemoState ,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);
JesseLohman 2:5cace74299e7 25 //QEI encoder3(A4, A5), NC, 4200);
KingMufasa 5:a1843b698d0d 26
KingMufasa 5:a1843b698d0d 27 AnalogIn emg0(A0);
KingMufasa 5:a1843b698d0d 28 AnalogIn emg1(A1);
KingMufasa 5:a1843b698d0d 29 AnalogIn emg2(A2);
KingMufasa 5:a1843b698d0d 30 AnalogIn emg3(A3);
JesseLohman 0:2a5dd6cc0008 31
JesseLohman 0:2a5dd6cc0008 32 bool stateChanged = true;
JesseLohman 0:2a5dd6cc0008 33
JesseLohman 0:2a5dd6cc0008 34 Ticker mainTicker;
JesseLohman 0:2a5dd6cc0008 35 Timer stateTimer;
JesseLohman 2:5cace74299e7 36
JesseLohman 2:5cace74299e7 37 const double sampleTime = 0.001;
JesseLohman 2:5cace74299e7 38 const float maxVelocity=8.4; // in rad/s
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 3:be922ea2415f 63 double u1;
JesseLohman 3:be922ea2415f 64 double u2; // u1 is motor output of the long link, u2 is motor of the short link, u3 is motor of gripper, u4 is motor of screwer
JesseLohman 3:be922ea2415f 65 double u3;
JesseLohman 3:be922ea2415f 66 double u4;
JesseLohman 2:5cace74299e7 67 FastPWM pwmpin1(D5); //motor pwm
JesseLohman 2:5cace74299e7 68 DigitalOut directionpin1(D4); // motor direction
JesseLohman 3:be922ea2415f 69 FastPWM pwmpin2 (D6);
JesseLohman 3:be922ea2415f 70 DigitalOut directionpin2 (D7);
KingMufasa 7:1906d404cd1e 71 FastPWM pwmpin3(A4); //motor pwm
KingMufasa 7:1906d404cd1e 72 DigitalOut directionpin3(D8); // motor direction
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 v; // Global variable for printf
JesseLohman 3:be922ea2415f 84 double Dpulses; // Global variable for printf
KingMufasa 4:8f25ecb74221 85 double error1; // Global variable for printf
KingMufasa 5:a1843b698d0d 86 double error2; // Global variable for printf
KingMufasa 6:e67250b8b100 87 double pulses1; // Global varaible for printf
KingMufasa 6:e67250b8b100 88 double pulses2; // Global varaible for printf
JesseLohman 3:be922ea2415f 89
JesseLohman 3:be922ea2415f 90 double C[5][5];
JesseLohman 3:be922ea2415f 91
KingMufasa 7:1906d404cd1e 92 double Kp1 = 10;
KingMufasa 7:1906d404cd1e 93 double Ki1 = 0;
KingMufasa 7:1906d404cd1e 94 double Kd1 = 2;
KingMufasa 7:1906d404cd1e 95 double Kp2 = 10;
KingMufasa 7:1906d404cd1e 96 double Ki2 = 0;
KingMufasa 7:1906d404cd1e 97 double Kd2 = 2;
KingMufasa 5:a1843b698d0d 98
KingMufasa 5:a1843b698d0d 99 const int samplepack = 250; //Amount of data points before one cycle completes
KingMufasa 5:a1843b698d0d 100 volatile int counter = 0; //Counts the amount of readings this cycle
KingMufasa 5:a1843b698d0d 101 volatile double total[4] = {0,0,0,0}; //Total difference between data points and the calibration value this cycle (x+, x-, y+, y-)
KingMufasa 5:a1843b698d0d 102 double refvaluebic = 10; //Minimum total value for the motor to move (biceps)
KingMufasa 5:a1843b698d0d 103 double refvaluecalf = 50; //Minimum total value for the motor to move (calfs)
KingMufasa 7:1906d404cd1e 104 double incr = 0.00002; //increment
KingMufasa 5:a1843b698d0d 105 bool moving[4] = {0,0,0,0}; //x+, x-, y+, y-
KingMufasa 5:a1843b698d0d 106
KingMufasa 5:a1843b698d0d 107 BiQuad hpf(0.8635,-1.7269,0.8635,-1.9359,0.9394); //5Hz High pass filter
KingMufasa 5:a1843b698d0d 108 BiQuad notch(0.9922,-1.6054,0.9922,-1.6054,0.9844); //50 Hz Notch filter
KingMufasa 5:a1843b698d0d 109 BiQuad lpf(0.9780,1.9561,0.9780,1.9556,0.9565); //250Hz Low pass filter
KingMufasa 5:a1843b698d0d 110 BiQuadChain bqc;
JesseLohman 0:2a5dd6cc0008 111
JesseLohman 0:2a5dd6cc0008 112 void switchToFailureState ()
JesseLohman 0:2a5dd6cc0008 113 {
JesseLohman 0:2a5dd6cc0008 114 failureButton.fall(NULL); // Detaches button, so it can only be activated once
JesseLohman 1:4cb9af313c26 115 led1 = 0;
JesseLohman 1:4cb9af313c26 116 led2 = 1;
JesseLohman 1:4cb9af313c26 117 led3 = 1;
JesseLohman 1:4cb9af313c26 118 pc.printf("SYSTEM FAILED\n");
JesseLohman 0:2a5dd6cc0008 119 currentState = FailureState;
JesseLohman 0:2a5dd6cc0008 120 stateChanged = true;
JesseLohman 0:2a5dd6cc0008 121 }
JesseLohman 0:2a5dd6cc0008 122
KingMufasa 7:1906d404cd1e 123
KingMufasa 7:1906d404cd1e 124
KingMufasa 7:1906d404cd1e 125
KingMufasa 7:1906d404cd1e 126
KingMufasa 7:1906d404cd1e 127
KingMufasa 5:a1843b698d0d 128 void measureEMG ()
KingMufasa 5:a1843b698d0d 129 {
KingMufasa 7:1906d404cd1e 130 if (currentState == MovingState) {
KingMufasa 7:1906d404cd1e 131 total[0] += abs(bqc.step(emg0.read()));
KingMufasa 7:1906d404cd1e 132 total[1] += abs(bqc.step(emg1.read()));
KingMufasa 7:1906d404cd1e 133 total[2] += abs(bqc.step(emg2.read()));
KingMufasa 7:1906d404cd1e 134 total[3] += abs(bqc.step(emg3.read()));
KingMufasa 7:1906d404cd1e 135 counter++;
KingMufasa 7:1906d404cd1e 136 if (counter >= samplepack) {
KingMufasa 7:1906d404cd1e 137 moving[0] = 0;
KingMufasa 7:1906d404cd1e 138 moving[1] = 0;
KingMufasa 7:1906d404cd1e 139 moving[2] = 0;
KingMufasa 7:1906d404cd1e 140 moving[3] = 0;
KingMufasa 7:1906d404cd1e 141 if (total[0] >= refvaluebic) {
KingMufasa 7:1906d404cd1e 142 moving[0] = 1;
KingMufasa 7:1906d404cd1e 143 }
KingMufasa 7:1906d404cd1e 144 if (total[1] >= refvaluebic) {
KingMufasa 7:1906d404cd1e 145 moving[1] = 1;
KingMufasa 7:1906d404cd1e 146 }
KingMufasa 7:1906d404cd1e 147 if (total[2] >= refvaluecalf) {
KingMufasa 7:1906d404cd1e 148 moving[2] = 1;
KingMufasa 7:1906d404cd1e 149 }
KingMufasa 7:1906d404cd1e 150 if (total[3] >= refvaluecalf) {
KingMufasa 7:1906d404cd1e 151 moving[3] = 1;
KingMufasa 7:1906d404cd1e 152 }
KingMufasa 7:1906d404cd1e 153 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 154 pc.printf("Coordinates: (%f,%f)\r\n", setPointX, setPointY);
KingMufasa 7:1906d404cd1e 155 counter = 0; //Reset for next cycle
KingMufasa 7:1906d404cd1e 156 for (int i=0; i<4; i++) { //clear 'total' array
KingMufasa 7:1906d404cd1e 157 total[i] = 0;
KingMufasa 7:1906d404cd1e 158 }
KingMufasa 5:a1843b698d0d 159 }
KingMufasa 7:1906d404cd1e 160
KingMufasa 7:1906d404cd1e 161 if(moving[0]) {
KingMufasa 7:1906d404cd1e 162 setPointX += incr;
KingMufasa 5:a1843b698d0d 163 }
KingMufasa 7:1906d404cd1e 164 if (moving[1]) {
KingMufasa 7:1906d404cd1e 165 setPointX -= incr;
KingMufasa 5:a1843b698d0d 166 }
KingMufasa 7:1906d404cd1e 167 if (moving[2]) {
KingMufasa 7:1906d404cd1e 168 setPointY += incr;
KingMufasa 7:1906d404cd1e 169 }
KingMufasa 7:1906d404cd1e 170 if (moving[3]) {
KingMufasa 7:1906d404cd1e 171 setPointY -= incr;
KingMufasa 5:a1843b698d0d 172 }
KingMufasa 5:a1843b698d0d 173 }
KingMufasa 5:a1843b698d0d 174 }
KingMufasa 5:a1843b698d0d 175
JesseLohman 2:5cace74299e7 176 double measureVelocity (int motor)
JesseLohman 2:5cace74299e7 177 {
JesseLohman 2:5cace74299e7 178 static double lastPulses = 0;
JesseLohman 2:5cace74299e7 179 double currentPulses;
JesseLohman 2:5cace74299e7 180 static double velocity = 0;
JesseLohman 3:be922ea2415f 181
JesseLohman 2:5cace74299e7 182 static int i = 0;
JesseLohman 2:5cace74299e7 183 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 184 switch (motor) { // Check which motor to measure
JesseLohman 3:be922ea2415f 185 case 1:
JesseLohman 3:be922ea2415f 186 currentPulses = encoder1.getPulses();
JesseLohman 3:be922ea2415f 187 break;
JesseLohman 3:be922ea2415f 188 case 2:
JesseLohman 3:be922ea2415f 189 //currentPulses = encoder2.getPulses();
JesseLohman 3:be922ea2415f 190 break;
JesseLohman 3:be922ea2415f 191 case 3:
JesseLohman 3:be922ea2415f 192 //currentPulses = encoder3.getPulses();
JesseLohman 3:be922ea2415f 193 break;
JesseLohman 3:be922ea2415f 194 }
JesseLohman 2:5cace74299e7 195
JesseLohman 3:be922ea2415f 196 double deltaPulses = currentPulses - lastPulses;
JesseLohman 3:be922ea2415f 197 Dpulses = deltaPulses;
JesseLohman 3:be922ea2415f 198 velocity = deltaPulses / (sampleTime * 10); // Velocity in pulses/s
JesseLohman 3:be922ea2415f 199 lastPulses = currentPulses;
JesseLohman 3:be922ea2415f 200 i = 0;
JesseLohman 2:5cace74299e7 201 } else {
JesseLohman 2:5cace74299e7 202 i += 1;
JesseLohman 2:5cace74299e7 203 }
JesseLohman 2:5cace74299e7 204 v = velocity;
JesseLohman 2:5cace74299e7 205 return velocity;
JesseLohman 2:5cace74299e7 206 }
JesseLohman 2:5cace74299e7 207
JesseLohman 3:be922ea2415f 208 void measurePosition() // Measure actual angle position with the encoder
JesseLohman 2:5cace74299e7 209 {
KingMufasa 7:1906d404cd1e 210 pulses1 = encoder2.getPulses(); // motor 1 is on M2 and ENC2 of biorobotics shield
KingMufasa 7:1906d404cd1e 211 pulses2 = encoder1.getPulses(); // motor 2 is on M1 and ENC1 of biorobotics shield
KingMufasa 6:e67250b8b100 212 qMeas1 = (pulses1) * 2 * PI / 8400 +140.7*PI/180 ; // Calculate the angle relative to the starting point (8400 pulses per revolution) + offset
KingMufasa 7:1906d404cd1e 213 qMeas2 = (pulses2) * 2 * PI / 8400 -87*PI/180;
JesseLohman 2:5cace74299e7 214
JesseLohman 2:5cace74299e7 215 }
JesseLohman 2:5cace74299e7 216
JesseLohman 2:5cace74299e7 217 void getMotorControlSignal () // Milestone 1 code, not relevant anymore
JesseLohman 2:5cace74299e7 218 {
KingMufasa 7:1906d404cd1e 219 //double potSignal = pot.read() * 2 - 1; // read pot and scale to motor control signal
JesseLohman 3:be922ea2415f 220 //pc.printf("motor control signal = %f\n", posampleTimeignal);
KingMufasa 7:1906d404cd1e 221 u1 = 0;
KingMufasa 7:1906d404cd1e 222 u2 = 0;
JesseLohman 3:be922ea2415f 223 }
JesseLohman 3:be922ea2415f 224
JesseLohman 3:be922ea2415f 225 template<std::size_t N, std::size_t M, std::size_t P>
JesseLohman 3:be922ea2415f 226 void mult(double A[N][M], double B[M][P])
JesseLohman 3:be922ea2415f 227 {
JesseLohman 3:be922ea2415f 228
JesseLohman 3:be922ea2415f 229 for( int n =0; n < 5; n++) {
JesseLohman 3:be922ea2415f 230 for(int p =0; p < 5; p++) {
JesseLohman 3:be922ea2415f 231 C[n][p] =0;
JesseLohman 3:be922ea2415f 232 }
JesseLohman 3:be922ea2415f 233 }
JesseLohman 3:be922ea2415f 234 for (int n = 0; n < N; n++) {
JesseLohman 3:be922ea2415f 235 for (int p = 0; p < P; p++) {
JesseLohman 3:be922ea2415f 236 double num = 0;
JesseLohman 3:be922ea2415f 237 for (int m = 0; m < M; m++) {
JesseLohman 3:be922ea2415f 238 num += A[n][m] * B[m][p];
JesseLohman 3:be922ea2415f 239
JesseLohman 3:be922ea2415f 240 }
JesseLohman 3:be922ea2415f 241
JesseLohman 3:be922ea2415f 242 C[n][p] = num;
JesseLohman 3:be922ea2415f 243
JesseLohman 3:be922ea2415f 244 }
JesseLohman 3:be922ea2415f 245 }
JesseLohman 3:be922ea2415f 246
JesseLohman 3:be922ea2415f 247 }
JesseLohman 3:be922ea2415f 248
JesseLohman 3:be922ea2415f 249 void inverseKinematics ()
JesseLohman 3:be922ea2415f 250 {
KingMufasa 7:1906d404cd1e 251 if (currentState == MovingState || currentState == DemoState) { // Only in the HomingState should the qRef1, qRef2 consistently change
KingMufasa 7:1906d404cd1e 252
JesseLohman 3:be922ea2415f 253 double Pe_set[3][1] { // defining setpoint location of end effector
KingMufasa 6:e67250b8b100 254 {setPointX}, //setting xcoord to pot 1
KingMufasa 6:e67250b8b100 255 {setPointY}, // setting ycoord to pot 2
JesseLohman 3:be922ea2415f 256 {1}
JesseLohman 3:be922ea2415f 257 };
JesseLohman 3:be922ea2415f 258
JesseLohman 3:be922ea2415f 259 //Calculating new H matrix
JesseLohman 3:be922ea2415f 260 double T1e[3][3] {
JesseLohman 3:be922ea2415f 261 {cos(qRef1), -sin(qRef1), 0},
JesseLohman 3:be922ea2415f 262 {sin(qRef1), cos(qRef1), 0},
JesseLohman 3:be922ea2415f 263 {0, 0, 1}
JesseLohman 3:be922ea2415f 264 };
JesseLohman 3:be922ea2415f 265 double T20e[3][3] {
JesseLohman 3:be922ea2415f 266 {cos(qRef2), -sin(qRef2), L1-L1*cos(qRef2)},
JesseLohman 3:be922ea2415f 267 {sin(qRef2), cos(qRef2), -L1*sin(qRef2)},
JesseLohman 3:be922ea2415f 268 {0, 0, 1}
JesseLohman 3:be922ea2415f 269 };
JesseLohman 3:be922ea2415f 270
JesseLohman 3:be922ea2415f 271
JesseLohman 3:be922ea2415f 272 mult<3,3,3>(T1e,T20e); // matrix multiplication
JesseLohman 3:be922ea2415f 273 double H201[3][3] {
JesseLohman 3:be922ea2415f 274 {C[0][0],C[0][1],C[0][2]},
JesseLohman 3:be922ea2415f 275 {C[1][0],C[1][1],C[1][2]},
JesseLohman 3:be922ea2415f 276 {C[2][0],C[2][1],C[2][2]}
JesseLohman 3:be922ea2415f 277 };
JesseLohman 3:be922ea2415f 278
JesseLohman 3:be922ea2415f 279 mult<3,3,3>(H201,H200); // matrix multiplication
JesseLohman 3:be922ea2415f 280 double H20 [3][3] {
JesseLohman 3:be922ea2415f 281 {C[0][0],C[0][1],C[0][2]},
JesseLohman 3:be922ea2415f 282 {C[1][0],C[1][1],C[1][2]},
JesseLohman 3:be922ea2415f 283 {C[2][0],C[2][1],C[2][2]}
JesseLohman 3:be922ea2415f 284 };
JesseLohman 3:be922ea2415f 285
JesseLohman 3:be922ea2415f 286 mult<3,3,1>(H20,Pe2); // matrix multiplication
JesseLohman 3:be922ea2415f 287 double Pe0[3][1] {
JesseLohman 3:be922ea2415f 288 {C[0][0]},
JesseLohman 3:be922ea2415f 289 {C[1][0]},
JesseLohman 3:be922ea2415f 290 {C[2][0]}
JesseLohman 3:be922ea2415f 291 };
JesseLohman 3:be922ea2415f 292
JesseLohman 3:be922ea2415f 293 double pe0x = Pe0[0][0]; // seperating coordinates of end effector location
JesseLohman 3:be922ea2415f 294 double pe0y = Pe0[1][0];
JesseLohman 3:be922ea2415f 295
JesseLohman 3:be922ea2415f 296 // Determing the jacobian
JesseLohman 3:be922ea2415f 297
JesseLohman 3:be922ea2415f 298 double T_1[3][1] {
JesseLohman 3:be922ea2415f 299 {1},
JesseLohman 3:be922ea2415f 300 {T1[0][2]},
JesseLohman 3:be922ea2415f 301 {T1[1][2]}
JesseLohman 3:be922ea2415f 302 };
JesseLohman 3:be922ea2415f 303
JesseLohman 3:be922ea2415f 304 double T_2[3][1] {
JesseLohman 3:be922ea2415f 305 {1},
JesseLohman 3:be922ea2415f 306 {L1*sin(qRef1)},
JesseLohman 3:be922ea2415f 307 {-L1*cos(qRef1)}
JesseLohman 3:be922ea2415f 308 };
JesseLohman 3:be922ea2415f 309
JesseLohman 3:be922ea2415f 310 double J[3][2] {
JesseLohman 3:be922ea2415f 311 {T_1[0][0], T_2[0][0]},
JesseLohman 3:be922ea2415f 312 {T_1[1][0], T_2[1][0]},
JesseLohman 3:be922ea2415f 313 {T_1[2][0], T_2[2][0]}
JesseLohman 3:be922ea2415f 314 };
JesseLohman 3:be922ea2415f 315
JesseLohman 3:be922ea2415f 316 //Determing 'Pulling" force to setpoint
JesseLohman 3:be922ea2415f 317
KingMufasa 7:1906d404cd1e 318 double kspring= 0.1; //virtual stifness of the force
JesseLohman 3:be922ea2415f 319 double Fs[3][1] { //force vector from end effector to setpoint
KingMufasa 6:e67250b8b100 320 {kspring*Pe_set[0][0] - kspring*Pe0[0][0]},
KingMufasa 6:e67250b8b100 321 {kspring*Pe_set[1][0] - kspring*Pe0[1][0]},
KingMufasa 6:e67250b8b100 322 {kspring*Pe_set[2][0] - kspring*Pe0[2][0]}
JesseLohman 3:be922ea2415f 323 };
KingMufasa 6:e67250b8b100 324 double Fx = kspring*setPointX - kspring*pe0x;
KingMufasa 6:e67250b8b100 325 double Fy = kspring*setPointY - kspring*pe0y;
JesseLohman 3:be922ea2415f 326 double W0t[3][1] {
JesseLohman 3:be922ea2415f 327 {pe0x*Fy - pe0y*Fx},
JesseLohman 3:be922ea2415f 328 {Fx},
JesseLohman 3:be922ea2415f 329 {Fy}
JesseLohman 3:be922ea2415f 330 };
JesseLohman 3:be922ea2415f 331
JesseLohman 3:be922ea2415f 332 double Jt[2][3] { // transposing jacobian matrix
JesseLohman 3:be922ea2415f 333 {J[0][0], J[1][0], J[2][0]},
JesseLohman 3:be922ea2415f 334 {T_2[0][0], T_2[1][0], T_2[2][0]}
JesseLohman 3:be922ea2415f 335 };
JesseLohman 3:be922ea2415f 336
JesseLohman 3:be922ea2415f 337 mult<2,3,1>(Jt,W0t);
JesseLohman 3:be922ea2415f 338 double tau_st1 = C[0][0];
JesseLohman 3:be922ea2415f 339 double tau_st2 = C[1][0];
JesseLohman 3:be922ea2415f 340
JesseLohman 3:be922ea2415f 341 //Calculating joint behaviour
JesseLohman 3:be922ea2415f 342
KingMufasa 7:1906d404cd1e 343 double b1 =1;
KingMufasa 5:a1843b698d0d 344 double b2 =1;
JesseLohman 3:be922ea2415f 345 //joint friction coefficent
JesseLohman 3:be922ea2415f 346 //double sampleTime = 1/1000; //Time step to reach the new angle
KingMufasa 4:8f25ecb74221 347 double w_s1 = tau_st1/b1; // angular velocity
KingMufasa 4:8f25ecb74221 348 double w_s2 = tau_st2/b2; // angular velocity
JesseLohman 3:be922ea2415f 349 //checking angle boundaries
KingMufasa 5:a1843b698d0d 350 qRef1 = qRef1 +w_s1*1; // calculating new angle of qRef1 in time step sampleTime
JesseLohman 3:be922ea2415f 351 if (qRef1 > 2*PI/3) {
JesseLohman 3:be922ea2415f 352 qRef1 = 2*PI/3;
JesseLohman 3:be922ea2415f 353 } else if (qRef1 < PI/6) {
JesseLohman 3:be922ea2415f 354 qRef1= PI/6;
JesseLohman 3:be922ea2415f 355 }
JesseLohman 3:be922ea2415f 356
KingMufasa 5:a1843b698d0d 357 qRef2 = qRef2 + w_s2*1; // calculating new angle of qRef2 in time step sampleTime
JesseLohman 3:be922ea2415f 358 if (qRef2 > -PI/4) {
JesseLohman 3:be922ea2415f 359 qRef2 = -PI/4;
JesseLohman 3:be922ea2415f 360 } else if (qRef2 < -PI/2) {
JesseLohman 3:be922ea2415f 361 qRef2= -PI/2;
JesseLohman 3:be922ea2415f 362 }
JesseLohman 3:be922ea2415f 363 }
JesseLohman 3:be922ea2415f 364 }
JesseLohman 3:be922ea2415f 365 void PID_controller() // Put the error trough PID control to make output 'u'
KingMufasa 5:a1843b698d0d 366
JesseLohman 3:be922ea2415f 367 {
KingMufasa 7:1906d404cd1e 368 if (currentState >= HomingState && currentState < FailureState) {
KingMufasa 7:1906d404cd1e 369 // Should only work when we move the robot to a defined position
KingMufasa 7:1906d404cd1e 370 error1 = qRef1 - qMeas1;
KingMufasa 5:a1843b698d0d 371 error2 = qRef2 - qMeas2;
JesseLohman 3:be922ea2415f 372
JesseLohman 3:be922ea2415f 373 static double errorIntegral1 = 0;
JesseLohman 3:be922ea2415f 374 static double errorIntegral2 = 0;
JesseLohman 3:be922ea2415f 375 static double errorPrev1 = error1;
JesseLohman 3:be922ea2415f 376 static double errorPrev2 = error2;
JesseLohman 3:be922ea2415f 377 static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
JesseLohman 3:be922ea2415f 378 //Ki = pot2.read() * 0.5; //Only Kd is controlled by a pot, Kp and Ki are constant
JesseLohman 3:be922ea2415f 379
KingMufasa 4:8f25ecb74221 380 // Proportional part:
KingMufasa 4:8f25ecb74221 381 double u_k1 = Kp1 * error1;
KingMufasa 4:8f25ecb74221 382 double u_k2 = Kp2 * error2;
JesseLohman 3:be922ea2415f 383
JesseLohman 3:be922ea2415f 384 //Integral part:
JesseLohman 3:be922ea2415f 385 errorIntegral1 = errorIntegral1 + error1 * sampleTime;
KingMufasa 4:8f25ecb74221 386 double u_i1 = Ki1 * errorIntegral1;
JesseLohman 3:be922ea2415f 387 errorIntegral2 = errorIntegral2 + error2 * sampleTime;
KingMufasa 4:8f25ecb74221 388 double u_i2 = Ki2 * errorIntegral2;
JesseLohman 3:be922ea2415f 389
JesseLohman 3:be922ea2415f 390 // Derivative part
JesseLohman 3:be922ea2415f 391 double errorDerivative1 = (error1 - errorPrev1)/sampleTime;
JesseLohman 3:be922ea2415f 392 double filteredErrorDerivative1 = LowPassFilter.step(errorDerivative1);
KingMufasa 4:8f25ecb74221 393 double u_d1 = Kd1 * filteredErrorDerivative1;
JesseLohman 3:be922ea2415f 394 errorPrev1 = error1;
JesseLohman 3:be922ea2415f 395 double errorDerivative2 = (error2 - errorPrev2)/sampleTime;
JesseLohman 3:be922ea2415f 396 double filteredErrorDerivative2 = LowPassFilter.step(errorDerivative2);
KingMufasa 4:8f25ecb74221 397 double u_d2 = Kd2 * filteredErrorDerivative2;
JesseLohman 3:be922ea2415f 398 errorPrev2 = error2;
JesseLohman 3:be922ea2415f 399
JesseLohman 3:be922ea2415f 400 // Sum all parsampleTime
JesseLohman 3:be922ea2415f 401 u1 = u_k1 + u_i1 + u_d1;
JesseLohman 3:be922ea2415f 402 u2 = u_k2 + u_i2 + u_d2;
KingMufasa 7:1906d404cd1e 403
KingMufasa 7:1906d404cd1e 404
KingMufasa 5:a1843b698d0d 405 }
JesseLohman 2:5cace74299e7 406 }
JesseLohman 2:5cace74299e7 407
JesseLohman 2:5cace74299e7 408 void controlMotor () // Control direction and speed
JesseLohman 2:5cace74299e7 409 {
JesseLohman 2:5cace74299e7 410 directionpin1 = u1 > 0.0f; // Either true or false
JesseLohman 2:5cace74299e7 411 pwmpin1 = fabs(u1);
JesseLohman 3:be922ea2415f 412 directionpin2 = u2 > 0.0f; // Either true or false
JesseLohman 3:be922ea2415f 413 pwmpin2 = fabs(u2);
KingMufasa 7:1906d404cd1e 414 directionpin3 = u3 > 0.0f; // Either true or false
KingMufasa 7:1906d404cd1e 415 pwmpin3 = fabs(u3);
KingMufasa 7:1906d404cd1e 416 directionpin4 = u4 > 0.0f; // Either true or false
KingMufasa 7:1906d404cd1e 417 pwmpin4 = fabs(u4);
JesseLohman 2:5cace74299e7 418 }
JesseLohman 2:5cace74299e7 419
JesseLohman 0:2a5dd6cc0008 420 void stateMachine ()
JesseLohman 0:2a5dd6cc0008 421 {
JesseLohman 0:2a5dd6cc0008 422 switch (currentState) {
JesseLohman 0:2a5dd6cc0008 423 case WaitState:
JesseLohman 0:2a5dd6cc0008 424 if (stateChanged == true) {
JesseLohman 1:4cb9af313c26 425 led1 = 0;
JesseLohman 1:4cb9af313c26 426 led2 = 1;
JesseLohman 1:4cb9af313c26 427 led3 = 1;
JesseLohman 0:2a5dd6cc0008 428 // Entry action: all the things you do once in this state
JesseLohman 2:5cace74299e7 429 u1 = 0; // Turn off all motors
JesseLohman 2:5cace74299e7 430 u2 = 0;
JesseLohman 2:5cace74299e7 431 u3 = 0;
JesseLohman 2:5cace74299e7 432 u4 = 0;
JesseLohman 0:2a5dd6cc0008 433 stateChanged = false;
JesseLohman 0:2a5dd6cc0008 434 }
JesseLohman 0:2a5dd6cc0008 435
JesseLohman 2:5cace74299e7 436 if (startButton.read() == false) { // When button is pressed, value is false
JesseLohman 1:4cb9af313c26 437 //pc.printf("Switching to motor calibration");
JesseLohman 1:4cb9af313c26 438 led1 = 1;
JesseLohman 0:2a5dd6cc0008 439 currentState = MotorCalState;
JesseLohman 0:2a5dd6cc0008 440 stateChanged = true;
JesseLohman 0:2a5dd6cc0008 441 }
JesseLohman 0:2a5dd6cc0008 442
JesseLohman 0:2a5dd6cc0008 443 break;
JesseLohman 0:2a5dd6cc0008 444 case MotorCalState:
JesseLohman 0:2a5dd6cc0008 445 if (stateChanged == true) {
JesseLohman 0:2a5dd6cc0008 446 // Entry action: all the things you do once in this state
JesseLohman 1:4cb9af313c26 447 led2 = 0;
JesseLohman 0:2a5dd6cc0008 448 // Set motorpwm to 'low' value
JesseLohman 3:be922ea2415f 449 //u1 = 0.6; //TODO: Check if direction is right
JesseLohman 3:be922ea2415f 450 //u2 = 0.6;
JesseLohman 0:2a5dd6cc0008 451 stateTimer.reset();
JesseLohman 0:2a5dd6cc0008 452 stateTimer.start();
JesseLohman 0:2a5dd6cc0008 453 stateChanged = false;
JesseLohman 0:2a5dd6cc0008 454 }
JesseLohman 0:2a5dd6cc0008 455
JesseLohman 0:2a5dd6cc0008 456 // Add stuff you do every loop
JesseLohman 2:5cace74299e7 457 getMotorControlSignal();
JesseLohman 0:2a5dd6cc0008 458
KingMufasa 7:1906d404cd1e 459 if (stateTimer >= 3.0f && fabs(measureVelocity(1)) < 100 && startButton.read() == false && fabs(measureVelocity(2)) < 0.1f) { //TODO: add
JesseLohman 2:5cace74299e7 460 //TODO: Add reset of encoder2
JesseLohman 1:4cb9af313c26 461 led2 = 1;
JesseLohman 2:5cace74299e7 462 encoder1.reset(); // Reset encoder for the 0 position
JesseLohman 0:2a5dd6cc0008 463 currentState = EMGCalState;
JesseLohman 0:2a5dd6cc0008 464 stateChanged = true;
JesseLohman 2:5cace74299e7 465 u1 = 0; // Turn off motors
JesseLohman 2:5cace74299e7 466 u2 = 0;
JesseLohman 0:2a5dd6cc0008 467 }
JesseLohman 0:2a5dd6cc0008 468 break;
JesseLohman 0:2a5dd6cc0008 469 case EMGCalState:
JesseLohman 0:2a5dd6cc0008 470 if (stateChanged == true) {
JesseLohman 0:2a5dd6cc0008 471 // Entry action: all the things you do once in this state;
JesseLohman 1:4cb9af313c26 472 led3 = 0;
JesseLohman 0:2a5dd6cc0008 473 stateTimer.reset();
JesseLohman 0:2a5dd6cc0008 474 stateTimer.start();
JesseLohman 0:2a5dd6cc0008 475 stateChanged = false;
JesseLohman 0:2a5dd6cc0008 476 }
JesseLohman 0:2a5dd6cc0008 477
JesseLohman 0:2a5dd6cc0008 478 // Add stuff you do every loop
JesseLohman 0:2a5dd6cc0008 479
JesseLohman 0:2a5dd6cc0008 480 if (stateTimer >= 3.0f) {
JesseLohman 1:4cb9af313c26 481 //pc.printf("Starting homing...\n");
JesseLohman 1:4cb9af313c26 482 led3 = 1;
JesseLohman 0:2a5dd6cc0008 483 currentState = HomingState;
JesseLohman 0:2a5dd6cc0008 484 stateChanged = true;
JesseLohman 0:2a5dd6cc0008 485 }
JesseLohman 0:2a5dd6cc0008 486 break;
JesseLohman 0:2a5dd6cc0008 487 case HomingState:
JesseLohman 0:2a5dd6cc0008 488 if (stateChanged == true) {
JesseLohman 0:2a5dd6cc0008 489 // Entry action: all the things you do once in this state;
JesseLohman 1:4cb9af313c26 490 led1 = 0;
JesseLohman 3:be922ea2415f 491 led2 = 0; // EmisampleTime yellow together
JesseLohman 3:be922ea2415f 492 //TODO: Set qRef1 and qRef2
KingMufasa 7:1906d404cd1e 493 qRef1 = 60 * PI / 180;
KingMufasa 7:1906d404cd1e 494 qRef2 = -90 *PI/180;
JesseLohman 0:2a5dd6cc0008 495 stateChanged = false;
JesseLohman 0:2a5dd6cc0008 496 }
JesseLohman 0:2a5dd6cc0008 497
JesseLohman 0:2a5dd6cc0008 498 // Nothing extra happens till robot reaches starting position and button is pressed
KingMufasa 7:1906d404cd1e 499 if (gripperButton.read() == false) { //TODO: Also add position condition
KingMufasa 7:1906d404cd1e 500 led1 = 1;
KingMufasa 7:1906d404cd1e 501 led2 = 1;
KingMufasa 7:1906d404cd1e 502 currentState = DemoState;
KingMufasa 7:1906d404cd1e 503 stateChanged = true;
KingMufasa 7:1906d404cd1e 504 }
JesseLohman 1:4cb9af313c26 505 if (startButton.read() == false) { //TODO: Also add position condition
JesseLohman 1:4cb9af313c26 506 led1 = 1;
JesseLohman 1:4cb9af313c26 507 led2 = 1;
JesseLohman 0:2a5dd6cc0008 508 currentState = MovingState;
JesseLohman 0:2a5dd6cc0008 509 stateChanged = true;
JesseLohman 0:2a5dd6cc0008 510 }
JesseLohman 0:2a5dd6cc0008 511 break;
KingMufasa 7:1906d404cd1e 512 case DemoState:
KingMufasa 7:1906d404cd1e 513 if (stateChanged == true) {
KingMufasa 7:1906d404cd1e 514 stateTimer.reset();
KingMufasa 7:1906d404cd1e 515 setPointX = 0.15;
KingMufasa 7:1906d404cd1e 516 setPointY = 0.3;
KingMufasa 7:1906d404cd1e 517 stateTimer.start();
KingMufasa 7:1906d404cd1e 518 stateChanged = false;
KingMufasa 7:1906d404cd1e 519 }
KingMufasa 7:1906d404cd1e 520 static double blinkTimer = 0;
KingMufasa 7:1906d404cd1e 521 if (blinkTimer >= 0.5) {
KingMufasa 7:1906d404cd1e 522 led2 = !led2;
KingMufasa 7:1906d404cd1e 523 blinkTimer = 0;
KingMufasa 7:1906d404cd1e 524 }
KingMufasa 7:1906d404cd1e 525 blinkTimer += sampleTime;
KingMufasa 7:1906d404cd1e 526 if (stateTimer >= 5.0) {
KingMufasa 7:1906d404cd1e 527 setPointX = -0.1;
KingMufasa 7:1906d404cd1e 528 setPointY = 0.3;
KingMufasa 7:1906d404cd1e 529 }
KingMufasa 7:1906d404cd1e 530
KingMufasa 7:1906d404cd1e 531 if (stateTimer >= 20.0) {
KingMufasa 7:1906d404cd1e 532 stateTimer.reset();
KingMufasa 7:1906d404cd1e 533 currentState = HomingState;
KingMufasa 7:1906d404cd1e 534 stateChanged = true;
KingMufasa 7:1906d404cd1e 535 }
KingMufasa 7:1906d404cd1e 536 break;
JesseLohman 0:2a5dd6cc0008 537 case MovingState:
JesseLohman 1:4cb9af313c26 538 if (stateChanged == true) {
JesseLohman 0:2a5dd6cc0008 539 // Entry action: all the things you do once in this state;
JesseLohman 1:4cb9af313c26 540 led1 = 0;
JesseLohman 1:4cb9af313c26 541 led2 = 0;
JesseLohman 3:be922ea2415f 542 led3 = 0; // EmisampleTime white together
JesseLohman 1:4cb9af313c26 543 stateChanged = false;
JesseLohman 1:4cb9af313c26 544 }
JesseLohman 1:4cb9af313c26 545
KingMufasa 7:1906d404cd1e 546 if (gripperButton.read() == false) {
JesseLohman 0:2a5dd6cc0008 547 led1 = 1;
JesseLohman 0:2a5dd6cc0008 548 led2 = 1;
JesseLohman 0:2a5dd6cc0008 549 led3 = 1;
JesseLohman 1:4cb9af313c26 550 currentState = GrippingState;
JesseLohman 1:4cb9af313c26 551 stateChanged = true;
JesseLohman 1:4cb9af313c26 552 }
JesseLohman 1:4cb9af313c26 553
JesseLohman 1:4cb9af313c26 554 break;
JesseLohman 1:4cb9af313c26 555 case GrippingState:
JesseLohman 1:4cb9af313c26 556 if (stateChanged == true) {
JesseLohman 1:4cb9af313c26 557 // Entry action: all the things you do once in this state;
JesseLohman 1:4cb9af313c26 558 led2 = 0;
JesseLohman 3:be922ea2415f 559 led3 = 0; // EmisampleTime light blue together
KingMufasa 7:1906d404cd1e 560 stateTimer.reset();
KingMufasa 7:1906d404cd1e 561 stateTimer.start();
JesseLohman 0:2a5dd6cc0008 562 stateChanged = false;
JesseLohman 0:2a5dd6cc0008 563 }
JesseLohman 1:4cb9af313c26 564
KingMufasa 7:1906d404cd1e 565 if (gripperMotorButton.read() == false) {
KingMufasa 7:1906d404cd1e 566 pc.printf("Button pressed \r\n");
KingMufasa 7:1906d404cd1e 567 u3 = 0.4;
KingMufasa 7:1906d404cd1e 568 if (directionSwitch == true) {
KingMufasa 7:1906d404cd1e 569 // Close gripper, so positive direction
KingMufasa 7:1906d404cd1e 570 } else {
KingMufasa 7:1906d404cd1e 571 // Open gripper
KingMufasa 7:1906d404cd1e 572 u3 = u3 * -1;
KingMufasa 7:1906d404cd1e 573 }
KingMufasa 7:1906d404cd1e 574 } else { // If the button isn't pressed, turn off motor
KingMufasa 7:1906d404cd1e 575 u3 = 0;
JesseLohman 1:4cb9af313c26 576 }
JesseLohman 1:4cb9af313c26 577
KingMufasa 7:1906d404cd1e 578 if (gripperButton.read() == false && stateTimer >= 2.0f) {
JesseLohman 1:4cb9af313c26 579 led2 = 1;
JesseLohman 1:4cb9af313c26 580 led3 = 1;
KingMufasa 7:1906d404cd1e 581 u3 = 0;
JesseLohman 1:4cb9af313c26 582 currentState = ScrewingState;
JesseLohman 1:4cb9af313c26 583 stateChanged = true;
JesseLohman 1:4cb9af313c26 584 }
JesseLohman 1:4cb9af313c26 585 if (startButton.read() == false) {
JesseLohman 1:4cb9af313c26 586 led2 = 1;
JesseLohman 1:4cb9af313c26 587 led3 = 1;
KingMufasa 7:1906d404cd1e 588 u3 = 0;
JesseLohman 1:4cb9af313c26 589 currentState = MovingState;
JesseLohman 1:4cb9af313c26 590 stateChanged = true;
JesseLohman 1:4cb9af313c26 591 }
JesseLohman 0:2a5dd6cc0008 592 break;
JesseLohman 0:2a5dd6cc0008 593 case ScrewingState:
JesseLohman 1:4cb9af313c26 594 if (stateChanged == true) {
JesseLohman 1:4cb9af313c26 595 // Entry action: all the things you do once in this state;
JesseLohman 1:4cb9af313c26 596 led1 = 0;
JesseLohman 3:be922ea2415f 597 led3 = 0; // EmisampleTime pink together
JesseLohman 1:4cb9af313c26 598 stateChanged = false;
JesseLohman 1:4cb9af313c26 599 }
JesseLohman 2:5cace74299e7 600
KingMufasa 7:1906d404cd1e 601 if (gripperMotorButton.read() == false) {
KingMufasa 7:1906d404cd1e 602 u4 = 0.3;
KingMufasa 7:1906d404cd1e 603 u3 = -0.35;
KingMufasa 7:1906d404cd1e 604 if (directionSwitch == true) {
KingMufasa 7:1906d404cd1e 605 // Screw
KingMufasa 7:1906d404cd1e 606 } else {
KingMufasa 7:1906d404cd1e 607 // Unscrew
KingMufasa 7:1906d404cd1e 608 u4 = u4 * -1;
KingMufasa 7:1906d404cd1e 609 u3 = u3 * -1;
KingMufasa 7:1906d404cd1e 610 }
JesseLohman 1:4cb9af313c26 611 } else {
KingMufasa 7:1906d404cd1e 612 u4 = 0;
KingMufasa 7:1906d404cd1e 613 u3 = 0;
JesseLohman 1:4cb9af313c26 614 }
JesseLohman 2:5cace74299e7 615
JesseLohman 1:4cb9af313c26 616 if (startButton.read() == false) {
JesseLohman 1:4cb9af313c26 617 led1 = 1;
JesseLohman 1:4cb9af313c26 618 led3 = 1;
KingMufasa 7:1906d404cd1e 619 u3 = 0;
KingMufasa 7:1906d404cd1e 620 u4 = 0;
JesseLohman 1:4cb9af313c26 621 currentState = MovingState;
JesseLohman 1:4cb9af313c26 622 stateChanged = true;
JesseLohman 1:4cb9af313c26 623 }
JesseLohman 0:2a5dd6cc0008 624 break;
JesseLohman 0:2a5dd6cc0008 625 case FailureState:
JesseLohman 0:2a5dd6cc0008 626 if (stateChanged == true) {
JesseLohman 0:2a5dd6cc0008 627 // Entry action: all the things you do once in this state
JesseLohman 2:5cace74299e7 628 u1 = 0; // Turn off all motors
JesseLohman 2:5cace74299e7 629 u2 = 0;
JesseLohman 2:5cace74299e7 630 u3 = 0;
JesseLohman 2:5cace74299e7 631 u4 = 0;
JesseLohman 0:2a5dd6cc0008 632 stateChanged = false;
JesseLohman 0:2a5dd6cc0008 633 }
JesseLohman 1:4cb9af313c26 634
KingMufasa 7:1906d404cd1e 635 static double blinkTimer2 = 0;
KingMufasa 7:1906d404cd1e 636 if (blinkTimer2 >= 0.5) {
JesseLohman 1:4cb9af313c26 637 led1 = !led1;
KingMufasa 7:1906d404cd1e 638 blinkTimer2 = 0;
JesseLohman 1:4cb9af313c26 639 }
KingMufasa 7:1906d404cd1e 640 blinkTimer2 += sampleTime;
JesseLohman 1:4cb9af313c26 641
JesseLohman 1:4cb9af313c26 642 break;
JesseLohman 0:2a5dd6cc0008 643 }
JesseLohman 0:2a5dd6cc0008 644 }
JesseLohman 0:2a5dd6cc0008 645
JesseLohman 3:be922ea2415f 646 void measureAll ()
JesseLohman 3:be922ea2415f 647 {
JesseLohman 3:be922ea2415f 648 measurePosition();
KingMufasa 5:a1843b698d0d 649 measureEMG();
JesseLohman 3:be922ea2415f 650 inverseKinematics();
JesseLohman 3:be922ea2415f 651 }
JesseLohman 3:be922ea2415f 652
JesseLohman 0:2a5dd6cc0008 653 void mainLoop ()
JesseLohman 0:2a5dd6cc0008 654 {
KingMufasa 7:1906d404cd1e 655 // Add measure, motor controller and output function
JesseLohman 2:5cace74299e7 656 measureAll();
JesseLohman 0:2a5dd6cc0008 657 stateMachine();
JesseLohman 3:be922ea2415f 658 PID_controller();
JesseLohman 2:5cace74299e7 659 controlMotor();
JesseLohman 0:2a5dd6cc0008 660 }
JesseLohman 0:2a5dd6cc0008 661
JesseLohman 0:2a5dd6cc0008 662 int main()
JesseLohman 0:2a5dd6cc0008 663 {
KingMufasa 7:1906d404cd1e 664 startButton.mode(PullUp);
KingMufasa 7:1906d404cd1e 665 failureButton.mode(PullUp);
KingMufasa 7:1906d404cd1e 666 gripperButton.mode(PullUp);
KingMufasa 7:1906d404cd1e 667 directionSwitch.mode(PullUp);
KingMufasa 7:1906d404cd1e 668 gripperMotorButton.mode(PullUp);
KingMufasa 5:a1843b698d0d 669 pc.baud(115200);
JesseLohman 2:5cace74299e7 670 pc.printf("checkpoint 1\n");
KingMufasa 7:1906d404cd1e 671 //pwmpin1.period(sampleTime);
KingMufasa 7:1906d404cd1e 672 //pwmpin2.period(sampleTime);
KingMufasa 5:a1843b698d0d 673 bqc.add(&hpf).add(&notch).add(&lpf); //Add bqfilters to bqc
JesseLohman 0:2a5dd6cc0008 674 mainTicker.attach(mainLoop, sampleTime);
JesseLohman 0:2a5dd6cc0008 675 failureButton.fall(&switchToFailureState); // When button is pressed FailureState is activated
JesseLohman 0:2a5dd6cc0008 676
JesseLohman 0:2a5dd6cc0008 677 while (true) {
JesseLohman 2:5cace74299e7 678 //pc.printf("State = %i\n", currentState);
JesseLohman 2:5cace74299e7 679 //int pulses = encoder1.getPulses();
JesseLohman 2:5cace74299e7 680 //pc.printf("pulses = %i\n", pulses);
KingMufasa 7:1906d404cd1e 681 // pc.printf("v = %f\n", v);
KingMufasa 6:e67250b8b100 682 pc.printf("Error1 = %f Error2 = %f \n qRef1 = %f rQref2 = %f \r\n qMeas1 = %f qMeas2 = %f \n, Pulses1 = %f Pulses2 = %f \n", error1,error2,qRef1,qRef2,qMeas1,qMeas2, pulses1, pulses2);
KingMufasa 5:a1843b698d0d 683 wait(2);
JesseLohman 0:2a5dd6cc0008 684 }
JesseLohman 0:2a5dd6cc0008 685 }