Jesse Lohman / Mbed 2 deprecated state_program

Dependencies:   FastPWM MODSERIAL QEI biquadFilter mbed

main.cpp@4:e7187a17c732, 2018-11-01 (annotated)

Committer:
JesseLohman
Date:
Thu Nov 01 22:53:01 2018 +0000
Revision:
4:e7187a17c732
Parent:
3:be922ea2415f 
gripper buttons;

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