Fabrice Tshilumba / Mbed 2 deprecated state_program

Dependencies:   FastPWM MODSERIAL QEI biquadFilter mbed

Fork of state_program by Jesse Lohman

main.cpp@3:be922ea2415f, 2018-10-31 (annotated)

Committer:
JesseLohman
Date:
Wed Oct 31 12:15:34 2018 +0000
Revision:
3:be922ea2415f
Parent:
2:5cace74299e7
Child:
4:8f25ecb74221
Added reverse kinematics and PID control

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