Fabrice Tshilumba / Mbed 2 deprecated state_program

Dependencies:   FastPWM MODSERIAL QEI biquadFilter mbed

Fork of state_program by Jesse Lohman

main.cpp@6:e67250b8b100, 2018-11-01 (annotated)

Committer:
KingMufasa
Date:
Thu Nov 01 19:35:15 2018 +0000
Revision:
6:e67250b8b100
Parent:
5:a1843b698d0d
Child:
7:1906d404cd1e
it works yay;

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