Jesse Lohman / Mbed 2 deprecated state_program1

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   FastPWM MODSERIAL QEI biquadFilter mbed

Fork of state_program by Fabrice Tshilumba

main.cpp@4:8f25ecb74221, 2018-10-31 (annotated)

Committer:
KingMufasa
Date:
Wed Oct 31 17:40:25 2018 +0000
Revision:
4:8f25ecb74221
Parent:
3:be922ea2415f
Child:
5:a1843b698d0d
state machine works until homing state without pid;

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