Fabrice Tshilumba / Mbed 2 deprecated state_program

Dependencies:   FastPWM MODSERIAL QEI biquadFilter mbed

Fork of state_program by Jesse Lohman

main.cpp@5:a1843b698d0d, 2018-11-01 (annotated)

Committer:
KingMufasa
Date:
Thu Nov 01 12:35:45 2018 +0000
Revision:
5:a1843b698d0d
Parent:
4:8f25ecb74221
Child:
6:e67250b8b100
emg added;

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