2.74 Jerboa Code First attempt at controlled locomotion
Dependencies: EthernetInterface ExperimentServer FXOS8700Q QEI_pmw experiment_example mbed-rtos mbed
Fork of experiment_example by
main.cpp@3:646dc3b2a681, 2017-12-05 (annotated)
- Committer:
- mchoun95
- Date:
- Tue Dec 05 02:38:49 2017 +0000
- Revision:
- 3:646dc3b2a681
- Parent:
- 1:95a7fddd25a9
2.74 Jerboa Code
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
pwensing | 0:43448bf056e8 | 1 | #include "mbed.h" |
pwensing | 0:43448bf056e8 | 2 | #include "rtos.h" |
pwensing | 0:43448bf056e8 | 3 | #include "EthernetInterface.h" |
pwensing | 0:43448bf056e8 | 4 | #include "ExperimentServer.h" |
pwensing | 0:43448bf056e8 | 5 | #include "QEI.h" |
mchoun95 | 3:646dc3b2a681 | 6 | #include "FXOS8700Q.h" |
pwensing | 0:43448bf056e8 | 7 | |
mchoun95 | 3:646dc3b2a681 | 8 | #define NUM_INPUTS 10 |
mchoun95 | 3:646dc3b2a681 | 9 | #define NUM_OUTPUTS 11 |
mchoun95 | 3:646dc3b2a681 | 10 | |
mchoun95 | 3:646dc3b2a681 | 11 | // States |
mchoun95 | 3:646dc3b2a681 | 12 | #define INIT 0 |
mchoun95 | 3:646dc3b2a681 | 13 | #define STANCE 1 |
mchoun95 | 3:646dc3b2a681 | 14 | #define FLIGHT 2 |
pwensing | 0:43448bf056e8 | 15 | |
mchoun95 | 3:646dc3b2a681 | 16 | //Debug |
mchoun95 | 3:646dc3b2a681 | 17 | //DigitalOut led1(LED1); |
mchoun95 | 3:646dc3b2a681 | 18 | |
mchoun95 | 3:646dc3b2a681 | 19 | // Initialize accelerometer |
mchoun95 | 3:646dc3b2a681 | 20 | FXOS8700Q_acc acc( PTE25, PTE24, FXOS8700CQ_SLAVE_ADDR1); |
mchoun95 | 3:646dc3b2a681 | 21 | MotionSensorDataUnits acc_data; |
mchoun95 | 3:646dc3b2a681 | 22 | |
mchoun95 | 3:646dc3b2a681 | 23 | Ticker control; |
pwensing | 0:43448bf056e8 | 24 | Serial pc(USBTX, USBRX); // USB Serial Terminal |
pwensing | 0:43448bf056e8 | 25 | ExperimentServer server; // Object that lets us communicate with MATLAB |
mchoun95 | 3:646dc3b2a681 | 26 | |
mchoun95 | 3:646dc3b2a681 | 27 | // Tail Motor Control Pins |
mchoun95 | 3:646dc3b2a681 | 28 | PwmOut tailPWM(D5); // Tail motor PWM output |
mchoun95 | 3:646dc3b2a681 | 29 | DigitalOut tailFwd(D6); // Tail motor forward enable |
mchoun95 | 3:646dc3b2a681 | 30 | DigitalOut tailRev(D7); // Tail motor backward enable |
mchoun95 | 3:646dc3b2a681 | 31 | |
mchoun95 | 3:646dc3b2a681 | 32 | // Leg Motor Control Pins |
mchoun95 | 3:646dc3b2a681 | 33 | PwmOut legPWM(D11); // Leg motor PWM output |
mchoun95 | 3:646dc3b2a681 | 34 | DigitalOut legFwd(D10); // Leg motor forward enable |
mchoun95 | 3:646dc3b2a681 | 35 | DigitalOut legRev(D9); // Leg motor backward enable |
mchoun95 | 3:646dc3b2a681 | 36 | |
mchoun95 | 3:646dc3b2a681 | 37 | Timer t; // Timer to measure elapsed time of experiment |
mchoun95 | 3:646dc3b2a681 | 38 | float time_ = 0.0; // time variable |
mchoun95 | 3:646dc3b2a681 | 39 | float tfinal = 0.0; |
mchoun95 | 3:646dc3b2a681 | 40 | |
mchoun95 | 3:646dc3b2a681 | 41 | // Motor current sensors |
mchoun95 | 3:646dc3b2a681 | 42 | AnalogIn tailCurrent(A0); // Tail current sensor |
mchoun95 | 3:646dc3b2a681 | 43 | AnalogIn legCurrent(A1); // Leg current sensor |
mchoun95 | 3:646dc3b2a681 | 44 | |
mchoun95 | 3:646dc3b2a681 | 45 | // Motor Encoders |
mchoun95 | 3:646dc3b2a681 | 46 | QEI tailEncoder(D1 ,D2 ,NC ,1200, QEI::X4_ENCODING); // Tail encoder |
mchoun95 | 3:646dc3b2a681 | 47 | QEI legEncoder(D3 ,D4 ,NC ,1200, QEI::X4_ENCODING); // Leg encoder |
mchoun95 | 3:646dc3b2a681 | 48 | |
mchoun95 | 3:646dc3b2a681 | 49 | // Physical motor parameters |
mchoun95 | 3:646dc3b2a681 | 50 | float R = 2.79; // Estimated motor resistance from lab |
mchoun95 | 3:646dc3b2a681 | 51 | float Kb = .1603; // Estimated motor torque constant measured from lab |
mchoun95 | 3:646dc3b2a681 | 52 | float Kcurrent = 1.5; // Proportional gain of the current control |
mchoun95 | 3:646dc3b2a681 | 53 | float v = 0.01; // BackEMF constant |
mchoun95 | 3:646dc3b2a681 | 54 | |
mchoun95 | 3:646dc3b2a681 | 55 | |
mchoun95 | 3:646dc3b2a681 | 56 | // Matlab inputs/Control variables |
mchoun95 | 3:646dc3b2a681 | 57 | float tailCmd = 0.0; // Commanded angle (Rads) for the tail |
mchoun95 | 3:646dc3b2a681 | 58 | float legCmd = 0.0; // Commanded angle (Rads) for the leg |
mchoun95 | 3:646dc3b2a681 | 59 | float tailt0 = 0.0; // Initial angle (Rads) |
mchoun95 | 3:646dc3b2a681 | 60 | float legt0 = 0.0; // Initial angle (Rads) |
mchoun95 | 3:646dc3b2a681 | 61 | float Kptail = 0.0; // Tail proportional gain of position control |
mchoun95 | 3:646dc3b2a681 | 62 | float Kpleg = 0.0; // Leg proportional gain of position control |
mchoun95 | 3:646dc3b2a681 | 63 | float Kdtail = 0.0; // Tail derivative gain of position control |
mchoun95 | 3:646dc3b2a681 | 64 | float Kdleg = 0.0; // Leg derivative gain for position control |
mchoun95 | 3:646dc3b2a681 | 65 | |
mchoun95 | 3:646dc3b2a681 | 66 | // State varibles/sensed parameters |
mchoun95 | 3:646dc3b2a681 | 67 | float idtail = 0.0; // Tail desired current |
mchoun95 | 3:646dc3b2a681 | 68 | float idleg = 0.0; // Leg desired current |
mchoun95 | 3:646dc3b2a681 | 69 | float taild = 0.0; // Desired angle (Rads) for the tail |
mchoun95 | 3:646dc3b2a681 | 70 | float legd = 0.0; // Desired angle (Rads) for the leg |
mchoun95 | 3:646dc3b2a681 | 71 | float wtail = 0.0; // Measured angular velocity of the tail |
mchoun95 | 3:646dc3b2a681 | 72 | float wleg = 0.0; // Measured angular velocity of the leg |
mchoun95 | 3:646dc3b2a681 | 73 | float ttail = 0.0; // Measured angular position of the tail |
mchoun95 | 3:646dc3b2a681 | 74 | float tleg = 0.0; // Measured angular position of the leg |
mchoun95 | 3:646dc3b2a681 | 75 | float itail = 0.0; // Current sensed in the tail motor |
mchoun95 | 3:646dc3b2a681 | 76 | float ileg = 0.0; // Current sensed in the leg motor |
mchoun95 | 3:646dc3b2a681 | 77 | float vctail = 0.0; // Tail voltage command |
mchoun95 | 3:646dc3b2a681 | 78 | float vcleg = 0.0; // Leg voltage command |
mchoun95 | 3:646dc3b2a681 | 79 | float faX, faY, faZ; // Accelerometer data |
mchoun95 | 3:646dc3b2a681 | 80 | float mg = 0.0; // Acceleration Magnitude |
mchoun95 | 3:646dc3b2a681 | 81 | float mgprev = 0.0; // Previous Acceleration Magnitude |
mchoun95 | 3:646dc3b2a681 | 82 | float impact = 0.0; // Impact event detection variable |
mchoun95 | 3:646dc3b2a681 | 83 | |
mchoun95 | 3:646dc3b2a681 | 84 | // Error terms |
mchoun95 | 3:646dc3b2a681 | 85 | float etail = 0.0; // Error term for tail |
mchoun95 | 3:646dc3b2a681 | 86 | float eleg = 0.0; // Error term for leg |
mchoun95 | 3:646dc3b2a681 | 87 | float eptail = 0.0; // Previous error term for tail |
mchoun95 | 3:646dc3b2a681 | 88 | float epleg = 0.0; // Previous error term for leg |
mchoun95 | 3:646dc3b2a681 | 89 | float edtail = 0.0; // Derivative error term for tail |
mchoun95 | 3:646dc3b2a681 | 90 | float edleg = 0.0; // Derivative error term for leg |
mchoun95 | 3:646dc3b2a681 | 91 | |
mchoun95 | 3:646dc3b2a681 | 92 | float max_cmd = 1.0; |
mchoun95 | 3:646dc3b2a681 | 93 | |
mchoun95 | 3:646dc3b2a681 | 94 | bool new_data = false; |
mchoun95 | 3:646dc3b2a681 | 95 | bool end = false; |
mchoun95 | 3:646dc3b2a681 | 96 | |
mchoun95 | 3:646dc3b2a681 | 97 | int state = INIT; |
mchoun95 | 3:646dc3b2a681 | 98 | uint32_t tdebounce = 0; |
mchoun95 | 3:646dc3b2a681 | 99 | uint32_t ttouchdown = 0; |
pwensing | 0:43448bf056e8 | 100 | |
mchoun95 | 3:646dc3b2a681 | 101 | void control_loop() { |
mchoun95 | 3:646dc3b2a681 | 102 | if (end){ |
mchoun95 | 3:646dc3b2a681 | 103 | tailPWM.write(0); |
mchoun95 | 3:646dc3b2a681 | 104 | legPWM.write(0); |
mchoun95 | 3:646dc3b2a681 | 105 | } |
mchoun95 | 3:646dc3b2a681 | 106 | else{ |
mchoun95 | 3:646dc3b2a681 | 107 | //Sensing |
mchoun95 | 3:646dc3b2a681 | 108 | itail = (tailCurrent.read()-.5)*36.7; // Tail current |
mchoun95 | 3:646dc3b2a681 | 109 | ileg = (legCurrent.read()-.5)*36.7; // Leg current |
mchoun95 | 3:646dc3b2a681 | 110 | ttail = tailEncoder.getPulses()/1200.0*2*3.14159 + tailt0; // Leg angle |
mchoun95 | 3:646dc3b2a681 | 111 | tleg = legEncoder.getPulses()/1200.0*2*3.14159 + legt0; // Leg angle |
mchoun95 | 3:646dc3b2a681 | 112 | wtail = tailEncoder.getVelocity()*2*3.14159/1200.0; // Tail angular velocity |
mchoun95 | 3:646dc3b2a681 | 113 | wleg = legEncoder.getVelocity()*2*3.14159/1200.0; // Leg angular velocity |
mchoun95 | 3:646dc3b2a681 | 114 | time_ = t.read(); // Time(sec) |
mchoun95 | 3:646dc3b2a681 | 115 | acc.getAxis(acc_data); // Accelerometer Data |
mchoun95 | 3:646dc3b2a681 | 116 | acc.getX(&faX); |
mchoun95 | 3:646dc3b2a681 | 117 | acc.getY(&faY); |
mchoun95 | 3:646dc3b2a681 | 118 | acc.getZ(&faZ); |
mchoun95 | 3:646dc3b2a681 | 119 | |
mchoun95 | 3:646dc3b2a681 | 120 | // Processing the accelerometer data |
mchoun95 | 3:646dc3b2a681 | 121 | mg = sqrt(faX*faX+faY*faY+faZ*faZ); // Magnitude Data |
mchoun95 | 3:646dc3b2a681 | 122 | impact = (mg - mgprev)/.001; // Change in magnitude |
mchoun95 | 3:646dc3b2a681 | 123 | mgprev = mg; // Update previous magnitude |
mchoun95 | 3:646dc3b2a681 | 124 | |
mchoun95 | 3:646dc3b2a681 | 125 | // Check for state change |
mchoun95 | 3:646dc3b2a681 | 126 | if(state == FLIGHT && impact > 500 && t.read_ms()-tdebounce > 100){ |
mchoun95 | 3:646dc3b2a681 | 127 | state = STANCE; |
mchoun95 | 3:646dc3b2a681 | 128 | tdebounce = t.read_ms(); |
mchoun95 | 3:646dc3b2a681 | 129 | ttouchdown = t.read_ms(); |
mchoun95 | 3:646dc3b2a681 | 130 | }else if(state == STANCE && mg < .7 && t.read_ms()-tdebounce > 100){ |
mchoun95 | 3:646dc3b2a681 | 131 | state = FLIGHT; |
mchoun95 | 3:646dc3b2a681 | 132 | tdebounce = t.read_ms(); |
mchoun95 | 3:646dc3b2a681 | 133 | } |
mchoun95 | 3:646dc3b2a681 | 134 | |
mchoun95 | 3:646dc3b2a681 | 135 | if(state == INIT){ |
mchoun95 | 3:646dc3b2a681 | 136 | // Error update |
mchoun95 | 3:646dc3b2a681 | 137 | etail = taild - ttail; // Error in tail's angle position |
mchoun95 | 3:646dc3b2a681 | 138 | eleg = legd - tleg; // Error in leg's angle position |
mchoun95 | 3:646dc3b2a681 | 139 | edtail = (etail - eptail)/.001; // Rate of change in tail's error |
mchoun95 | 3:646dc3b2a681 | 140 | edleg = (eleg - epleg)/.001; // Rate of change in leg's error |
mchoun95 | 3:646dc3b2a681 | 141 | eptail = etail; // Update previous tail error |
mchoun95 | 3:646dc3b2a681 | 142 | epleg = eleg; // Update previous leg error |
mchoun95 | 3:646dc3b2a681 | 143 | |
mchoun95 | 3:646dc3b2a681 | 144 | // Perform control loop logic |
mchoun95 | 3:646dc3b2a681 | 145 | idtail = (Kptail*etail + Kdtail*edtail + v*wtail) / Kb; // Tail position control |
mchoun95 | 3:646dc3b2a681 | 146 | idleg = (Kpleg*eleg + Kdleg*edleg + v*wleg) / Kb; // Leg position control |
mchoun95 | 3:646dc3b2a681 | 147 | |
mchoun95 | 3:646dc3b2a681 | 148 | vctail = (R*idtail + Kcurrent * (idtail - itail) - Kb * wtail) / 24.0; // Tail current control |
mchoun95 | 3:646dc3b2a681 | 149 | vcleg = (R*idleg + Kcurrent * (idleg - ileg) - Kb * wleg) / 24.0; // Leg current control |
mchoun95 | 3:646dc3b2a681 | 150 | |
mchoun95 | 3:646dc3b2a681 | 151 | // Tail set command |
mchoun95 | 3:646dc3b2a681 | 152 | if (vctail < 0){ |
mchoun95 | 3:646dc3b2a681 | 153 | tailFwd = 1; |
mchoun95 | 3:646dc3b2a681 | 154 | tailRev = 0; |
mchoun95 | 3:646dc3b2a681 | 155 | if (abs(vctail) > 1){ |
mchoun95 | 3:646dc3b2a681 | 156 | vctail = -1; |
mchoun95 | 3:646dc3b2a681 | 157 | } |
mchoun95 | 3:646dc3b2a681 | 158 | tailPWM.write(max_cmd*abs(vctail)); |
mchoun95 | 3:646dc3b2a681 | 159 | }else if (vctail > 0){ |
mchoun95 | 3:646dc3b2a681 | 160 | tailFwd = 0; |
mchoun95 | 3:646dc3b2a681 | 161 | tailRev = 1; |
mchoun95 | 3:646dc3b2a681 | 162 | if (abs(vctail) > 1){ |
mchoun95 | 3:646dc3b2a681 | 163 | vctail = 1; |
mchoun95 | 3:646dc3b2a681 | 164 | } |
mchoun95 | 3:646dc3b2a681 | 165 | tailPWM.write(max_cmd*abs(vctail)); |
mchoun95 | 3:646dc3b2a681 | 166 | } else { |
mchoun95 | 3:646dc3b2a681 | 167 | tailPWM.write(0); |
mchoun95 | 3:646dc3b2a681 | 168 | } |
mchoun95 | 3:646dc3b2a681 | 169 | |
mchoun95 | 3:646dc3b2a681 | 170 | // Leg set command |
mchoun95 | 3:646dc3b2a681 | 171 | if (vcleg < 0){ |
mchoun95 | 3:646dc3b2a681 | 172 | legFwd = 1; |
mchoun95 | 3:646dc3b2a681 | 173 | legRev = 0; |
mchoun95 | 3:646dc3b2a681 | 174 | if (abs(vcleg) > 1){ |
mchoun95 | 3:646dc3b2a681 | 175 | vcleg = -1; |
mchoun95 | 3:646dc3b2a681 | 176 | } |
mchoun95 | 3:646dc3b2a681 | 177 | legPWM.write(max_cmd*abs(vcleg)); |
mchoun95 | 3:646dc3b2a681 | 178 | }else if (vcleg > 0){ |
mchoun95 | 3:646dc3b2a681 | 179 | legFwd = 0; |
mchoun95 | 3:646dc3b2a681 | 180 | legRev = 1; |
mchoun95 | 3:646dc3b2a681 | 181 | if (abs(vcleg) > 1){ |
mchoun95 | 3:646dc3b2a681 | 182 | vcleg = 1; |
mchoun95 | 3:646dc3b2a681 | 183 | } |
mchoun95 | 3:646dc3b2a681 | 184 | legPWM.write(max_cmd*abs(vcleg)); |
mchoun95 | 3:646dc3b2a681 | 185 | } else { |
mchoun95 | 3:646dc3b2a681 | 186 | legPWM.write(0); |
mchoun95 | 3:646dc3b2a681 | 187 | } |
mchoun95 | 3:646dc3b2a681 | 188 | new_data = true; |
mchoun95 | 3:646dc3b2a681 | 189 | |
mchoun95 | 3:646dc3b2a681 | 190 | }else if (state == STANCE){ |
mchoun95 | 3:646dc3b2a681 | 191 | //led1 = false; |
mchoun95 | 3:646dc3b2a681 | 192 | |
mchoun95 | 3:646dc3b2a681 | 193 | legd = 3.14159/80; //Set the leg angle for SLIP |
mchoun95 | 3:646dc3b2a681 | 194 | taild = -3.14159/12*cos(2*3.14159*(t.read_ms()-ttouchdown)/1000.0); //Set the tail angle based on cos |
mchoun95 | 3:646dc3b2a681 | 195 | |
mchoun95 | 3:646dc3b2a681 | 196 | // Error update |
mchoun95 | 3:646dc3b2a681 | 197 | etail = taild - ttail; // Error in tail's angle position |
mchoun95 | 3:646dc3b2a681 | 198 | eleg = legd - tleg; // Error in leg's angle position |
mchoun95 | 3:646dc3b2a681 | 199 | edtail = (etail - eptail)/.001; // Rate of change in tail's error |
mchoun95 | 3:646dc3b2a681 | 200 | edleg = (eleg - epleg)/.001; // Rate of change in leg's error |
mchoun95 | 3:646dc3b2a681 | 201 | eptail = etail; // Update previous tail error |
mchoun95 | 3:646dc3b2a681 | 202 | epleg = eleg; // Update previous leg error |
mchoun95 | 3:646dc3b2a681 | 203 | |
mchoun95 | 3:646dc3b2a681 | 204 | // Tail Control |
mchoun95 | 3:646dc3b2a681 | 205 | idtail = (Kptail*etail + Kdtail*edtail + v*wtail)/ Kb; |
mchoun95 | 3:646dc3b2a681 | 206 | vctail = (R*idtail + Kcurrent * (idtail - itail) - Kb * wtail) / 24.0; |
mchoun95 | 3:646dc3b2a681 | 207 | |
mchoun95 | 3:646dc3b2a681 | 208 | // Leg Control |
mchoun95 | 3:646dc3b2a681 | 209 | idleg = (Kpleg*eleg + Kdleg*edleg + v*wleg) / Kb; |
mchoun95 | 3:646dc3b2a681 | 210 | vcleg = (R*idleg + Kcurrent * (idleg - ileg) - Kb * wleg) / 24.0; |
mchoun95 | 3:646dc3b2a681 | 211 | |
mchoun95 | 3:646dc3b2a681 | 212 | // Tail set command |
mchoun95 | 3:646dc3b2a681 | 213 | if (vctail < 0){ |
mchoun95 | 3:646dc3b2a681 | 214 | tailFwd = 1; |
mchoun95 | 3:646dc3b2a681 | 215 | tailRev = 0; |
mchoun95 | 3:646dc3b2a681 | 216 | if (abs(vctail) > 1){ |
mchoun95 | 3:646dc3b2a681 | 217 | vctail = -1; |
mchoun95 | 3:646dc3b2a681 | 218 | } |
mchoun95 | 3:646dc3b2a681 | 219 | tailPWM.write(max_cmd*abs(vctail)); |
mchoun95 | 3:646dc3b2a681 | 220 | }else if (vctail > 0){ |
mchoun95 | 3:646dc3b2a681 | 221 | tailFwd = 0; |
mchoun95 | 3:646dc3b2a681 | 222 | tailRev = 1; |
mchoun95 | 3:646dc3b2a681 | 223 | if (abs(vctail) > 1){ |
mchoun95 | 3:646dc3b2a681 | 224 | vctail = 1; |
mchoun95 | 3:646dc3b2a681 | 225 | } |
mchoun95 | 3:646dc3b2a681 | 226 | tailPWM.write(max_cmd*abs(vctail)); |
mchoun95 | 3:646dc3b2a681 | 227 | } else { |
mchoun95 | 3:646dc3b2a681 | 228 | tailPWM.write(0); |
mchoun95 | 3:646dc3b2a681 | 229 | } |
mchoun95 | 3:646dc3b2a681 | 230 | |
mchoun95 | 3:646dc3b2a681 | 231 | // Leg set command |
mchoun95 | 3:646dc3b2a681 | 232 | if (vcleg < 0){ |
mchoun95 | 3:646dc3b2a681 | 233 | legFwd = 1; |
mchoun95 | 3:646dc3b2a681 | 234 | legRev = 0; |
mchoun95 | 3:646dc3b2a681 | 235 | if (abs(vcleg) > 1){ |
mchoun95 | 3:646dc3b2a681 | 236 | vcleg = -1; |
mchoun95 | 3:646dc3b2a681 | 237 | } |
mchoun95 | 3:646dc3b2a681 | 238 | legPWM.write(max_cmd*abs(vcleg)); |
mchoun95 | 3:646dc3b2a681 | 239 | }else if (vcleg > 0){ |
mchoun95 | 3:646dc3b2a681 | 240 | legFwd = 0; |
mchoun95 | 3:646dc3b2a681 | 241 | legRev = 1; |
mchoun95 | 3:646dc3b2a681 | 242 | if (abs(vcleg) > 1){ |
mchoun95 | 3:646dc3b2a681 | 243 | vcleg = 1; |
mchoun95 | 3:646dc3b2a681 | 244 | } |
mchoun95 | 3:646dc3b2a681 | 245 | legPWM.write(max_cmd*abs(vcleg)); |
mchoun95 | 3:646dc3b2a681 | 246 | } else { |
mchoun95 | 3:646dc3b2a681 | 247 | legPWM.write(0); |
mchoun95 | 3:646dc3b2a681 | 248 | } |
mchoun95 | 3:646dc3b2a681 | 249 | new_data = true; |
mchoun95 | 3:646dc3b2a681 | 250 | }else if (state == FLIGHT){ |
mchoun95 | 3:646dc3b2a681 | 251 | // led1 = true; |
mchoun95 | 3:646dc3b2a681 | 252 | // Set desired angles |
mchoun95 | 3:646dc3b2a681 | 253 | legd = -3.14159/50.0; |
mchoun95 | 3:646dc3b2a681 | 254 | taild = -3.14159/16.0; |
mchoun95 | 3:646dc3b2a681 | 255 | |
mchoun95 | 3:646dc3b2a681 | 256 | // Error update |
mchoun95 | 3:646dc3b2a681 | 257 | etail = taild - ttail; // Error in tail's angle position |
mchoun95 | 3:646dc3b2a681 | 258 | eleg = legd - tleg; // Error in leg's angle position |
mchoun95 | 3:646dc3b2a681 | 259 | edtail = (etail - eptail)/.001; // Rate of change in tail's error |
mchoun95 | 3:646dc3b2a681 | 260 | edleg = (eleg - epleg)/.001; // Rate of change in leg's error |
mchoun95 | 3:646dc3b2a681 | 261 | eptail = etail; // Update previous tail error |
mchoun95 | 3:646dc3b2a681 | 262 | epleg = eleg; // Update previous leg error |
mchoun95 | 3:646dc3b2a681 | 263 | |
mchoun95 | 3:646dc3b2a681 | 264 | // Tail Control |
mchoun95 | 3:646dc3b2a681 | 265 | idtail = (Kptail*etail + Kdtail*edtail + v*wtail) / Kb; |
mchoun95 | 3:646dc3b2a681 | 266 | vctail = (R*idtail + Kcurrent * (idtail - itail) - Kb * wtail) / 24.0; |
mchoun95 | 3:646dc3b2a681 | 267 | // Leg Control |
mchoun95 | 3:646dc3b2a681 | 268 | idleg = (Kpleg*eleg + Kdleg*edleg + v*wleg) / Kb; |
mchoun95 | 3:646dc3b2a681 | 269 | vcleg = (R*idleg + Kcurrent * (idleg - ileg) - Kb * wleg) / 24.0; |
mchoun95 | 3:646dc3b2a681 | 270 | |
mchoun95 | 3:646dc3b2a681 | 271 | // Tail set command |
mchoun95 | 3:646dc3b2a681 | 272 | if (vctail < 0){ |
mchoun95 | 3:646dc3b2a681 | 273 | tailFwd = 1; |
mchoun95 | 3:646dc3b2a681 | 274 | tailRev = 0; |
mchoun95 | 3:646dc3b2a681 | 275 | if (abs(vctail) > 1){ |
mchoun95 | 3:646dc3b2a681 | 276 | vctail = -1; |
mchoun95 | 3:646dc3b2a681 | 277 | } |
mchoun95 | 3:646dc3b2a681 | 278 | tailPWM.write(max_cmd*abs(vctail)); |
mchoun95 | 3:646dc3b2a681 | 279 | }else if (vctail > 0){ |
mchoun95 | 3:646dc3b2a681 | 280 | tailFwd = 0; |
mchoun95 | 3:646dc3b2a681 | 281 | tailRev = 1; |
mchoun95 | 3:646dc3b2a681 | 282 | if (abs(vctail) > 1){ |
mchoun95 | 3:646dc3b2a681 | 283 | vctail = 1; |
mchoun95 | 3:646dc3b2a681 | 284 | } |
mchoun95 | 3:646dc3b2a681 | 285 | tailPWM.write(max_cmd*abs(vctail)); |
mchoun95 | 3:646dc3b2a681 | 286 | } else { |
mchoun95 | 3:646dc3b2a681 | 287 | tailPWM.write(0); |
mchoun95 | 3:646dc3b2a681 | 288 | } |
mchoun95 | 3:646dc3b2a681 | 289 | |
mchoun95 | 3:646dc3b2a681 | 290 | // Leg set command |
mchoun95 | 3:646dc3b2a681 | 291 | if (vcleg < 0){ |
mchoun95 | 3:646dc3b2a681 | 292 | legFwd = 1; |
mchoun95 | 3:646dc3b2a681 | 293 | legRev = 0; |
mchoun95 | 3:646dc3b2a681 | 294 | if (abs(vcleg) > 1){ |
mchoun95 | 3:646dc3b2a681 | 295 | vcleg = -1; |
mchoun95 | 3:646dc3b2a681 | 296 | } |
mchoun95 | 3:646dc3b2a681 | 297 | legPWM.write(max_cmd*abs(vcleg)); |
mchoun95 | 3:646dc3b2a681 | 298 | }else if (vcleg > 0){ |
mchoun95 | 3:646dc3b2a681 | 299 | legFwd = 0; |
mchoun95 | 3:646dc3b2a681 | 300 | legRev = 1; |
mchoun95 | 3:646dc3b2a681 | 301 | if (abs(vcleg) > 1){ |
mchoun95 | 3:646dc3b2a681 | 302 | vcleg = 1; |
mchoun95 | 3:646dc3b2a681 | 303 | } |
mchoun95 | 3:646dc3b2a681 | 304 | legPWM.write(max_cmd*abs(vcleg)); |
mchoun95 | 3:646dc3b2a681 | 305 | } else { |
mchoun95 | 3:646dc3b2a681 | 306 | legPWM.write(0); |
mchoun95 | 3:646dc3b2a681 | 307 | } |
mchoun95 | 3:646dc3b2a681 | 308 | new_data = true; |
mchoun95 | 3:646dc3b2a681 | 309 | } |
mchoun95 | 3:646dc3b2a681 | 310 | } |
mchoun95 | 3:646dc3b2a681 | 311 | } |
pwensing | 0:43448bf056e8 | 312 | |
pwensing | 0:43448bf056e8 | 313 | int main (void) { |
mchoun95 | 3:646dc3b2a681 | 314 | //led1 = false; |
mchoun95 | 3:646dc3b2a681 | 315 | |
pwensing | 0:43448bf056e8 | 316 | // Link the terminal with our server and start it up |
pwensing | 0:43448bf056e8 | 317 | server.attachTerminal(pc); |
pwensing | 0:43448bf056e8 | 318 | server.init(); |
mchoun95 | 3:646dc3b2a681 | 319 | |
mchoun95 | 3:646dc3b2a681 | 320 | // Accelerometer |
mchoun95 | 3:646dc3b2a681 | 321 | acc.enable(); |
mchoun95 | 3:646dc3b2a681 | 322 | |
Patrick Wensing |
1:95a7fddd25a9 | 323 | // PWM period should nominally be a multiple of our control loop |
mchoun95 | 3:646dc3b2a681 | 324 | tailPWM.period_us(500); |
mchoun95 | 3:646dc3b2a681 | 325 | legPWM.period_us(500); |
pwensing | 0:43448bf056e8 | 326 | |
pwensing | 0:43448bf056e8 | 327 | // Continually get input from MATLAB and run experiments |
pwensing | 0:43448bf056e8 | 328 | float input_params[NUM_INPUTS]; |
mchoun95 | 3:646dc3b2a681 | 329 | control.attach(&control_loop, .001); |
mchoun95 | 3:646dc3b2a681 | 330 | |
pwensing | 0:43448bf056e8 | 331 | |
pwensing | 0:43448bf056e8 | 332 | while(1) { |
pwensing | 0:43448bf056e8 | 333 | if (server.getParams(input_params,NUM_INPUTS)) { |
mchoun95 | 3:646dc3b2a681 | 334 | tailCmd = input_params[0]; // Desired angle (Rads) for the tail |
mchoun95 | 3:646dc3b2a681 | 335 | legCmd = input_params[1]; // Desired angle (Rads) for the leg |
mchoun95 | 3:646dc3b2a681 | 336 | tailt0 = input_params[2]; // Initial angle (Rads) |
mchoun95 | 3:646dc3b2a681 | 337 | legt0 = input_params[3]; // Initial angle (Rads) |
mchoun95 | 3:646dc3b2a681 | 338 | Kptail = input_params[4]; // Tail proportional gain of position control |
mchoun95 | 3:646dc3b2a681 | 339 | Kpleg = input_params[5]; // Leg proportional gain of position control |
mchoun95 | 3:646dc3b2a681 | 340 | Kdtail = input_params[6]; // Tail derivative gain of position control |
mchoun95 | 3:646dc3b2a681 | 341 | Kdleg = input_params[7]; // Leg derivative gain for position control |
mchoun95 | 3:646dc3b2a681 | 342 | max_cmd = input_params[8]; // cmd thresholder |
mchoun95 | 3:646dc3b2a681 | 343 | tfinal = input_params[9]; |
mchoun95 | 3:646dc3b2a681 | 344 | |
mchoun95 | 3:646dc3b2a681 | 345 | state = INIT; |
mchoun95 | 3:646dc3b2a681 | 346 | |
mchoun95 | 3:646dc3b2a681 | 347 | if (max_cmd > 1.0 || max_cmd < 0){ |
mchoun95 | 3:646dc3b2a681 | 348 | max_cmd = 1.0; |
mchoun95 | 3:646dc3b2a681 | 349 | } |
pwensing | 0:43448bf056e8 | 350 | // Setup experiment |
pwensing | 0:43448bf056e8 | 351 | t.reset(); |
pwensing | 0:43448bf056e8 | 352 | t.start(); |
mchoun95 | 3:646dc3b2a681 | 353 | end = false; |
mchoun95 | 3:646dc3b2a681 | 354 | |
mchoun95 | 3:646dc3b2a681 | 355 | // Reset the encoders |
mchoun95 | 3:646dc3b2a681 | 356 | tailEncoder.reset(); |
mchoun95 | 3:646dc3b2a681 | 357 | legEncoder.reset(); |
mchoun95 | 3:646dc3b2a681 | 358 | |
mchoun95 | 3:646dc3b2a681 | 359 | // Default forward rotation |
mchoun95 | 3:646dc3b2a681 | 360 | tailFwd = 1; |
mchoun95 | 3:646dc3b2a681 | 361 | tailRev = 0; |
mchoun95 | 3:646dc3b2a681 | 362 | legFwd = 1; |
mchoun95 | 3:646dc3b2a681 | 363 | legRev = 0; |
mchoun95 | 3:646dc3b2a681 | 364 | |
mchoun95 | 3:646dc3b2a681 | 365 | // Stop the motors |
mchoun95 | 3:646dc3b2a681 | 366 | tailPWM.write(0); |
mchoun95 | 3:646dc3b2a681 | 367 | legPWM.write(0); |
mchoun95 | 3:646dc3b2a681 | 368 | |
pwensing | 0:43448bf056e8 | 369 | // Run experiment |
mchoun95 | 3:646dc3b2a681 | 370 | while( t.read() < tfinal) { |
mchoun95 | 3:646dc3b2a681 | 371 | // Set desired parameters at specific times |
mchoun95 | 3:646dc3b2a681 | 372 | if(t.read()<5){ |
mchoun95 | 3:646dc3b2a681 | 373 | taild = tailt0; |
mchoun95 | 3:646dc3b2a681 | 374 | legd = legt0; |
mchoun95 | 3:646dc3b2a681 | 375 | }else if(t.read() < 10 && state == INIT){ |
mchoun95 | 3:646dc3b2a681 | 376 | taild = tailCmd; |
mchoun95 | 3:646dc3b2a681 | 377 | legd = legCmd; |
mchoun95 | 3:646dc3b2a681 | 378 | } else{ |
mchoun95 | 3:646dc3b2a681 | 379 | state = STANCE; |
mchoun95 | 3:646dc3b2a681 | 380 | } |
mchoun95 | 3:646dc3b2a681 | 381 | // if(state = STANCE){ |
mchoun95 | 3:646dc3b2a681 | 382 | // |
mchoun95 | 3:646dc3b2a681 | 383 | // } |
mchoun95 | 3:646dc3b2a681 | 384 | |
pwensing | 0:43448bf056e8 | 385 | // Send data to MATLAB |
mchoun95 | 3:646dc3b2a681 | 386 | if(new_data) { |
mchoun95 | 3:646dc3b2a681 | 387 | // Form output to send to MATLAB |
mchoun95 | 3:646dc3b2a681 | 388 | float output_data[NUM_OUTPUTS]; |
mchoun95 | 3:646dc3b2a681 | 389 | output_data[0] = time_; |
mchoun95 | 3:646dc3b2a681 | 390 | output_data[1] = vctail; |
mchoun95 | 3:646dc3b2a681 | 391 | output_data[2] = vcleg; |
mchoun95 | 3:646dc3b2a681 | 392 | output_data[3] = itail; |
mchoun95 | 3:646dc3b2a681 | 393 | output_data[4] = ileg; |
mchoun95 | 3:646dc3b2a681 | 394 | output_data[5] = ttail; |
mchoun95 | 3:646dc3b2a681 | 395 | output_data[6] = tleg; |
mchoun95 | 3:646dc3b2a681 | 396 | output_data[7] = wtail; |
mchoun95 | 3:646dc3b2a681 | 397 | output_data[8] = wleg; |
mchoun95 | 3:646dc3b2a681 | 398 | output_data[9] = impact; |
mchoun95 | 3:646dc3b2a681 | 399 | output_data[10] = mg; |
mchoun95 | 3:646dc3b2a681 | 400 | server.sendData(output_data,NUM_OUTPUTS); |
mchoun95 | 3:646dc3b2a681 | 401 | new_data = false; |
mchoun95 | 3:646dc3b2a681 | 402 | } |
pwensing | 0:43448bf056e8 | 403 | } |
mchoun95 | 3:646dc3b2a681 | 404 | end = true; |
pwensing | 0:43448bf056e8 | 405 | // Cleanup after experiment |
pwensing | 0:43448bf056e8 | 406 | server.setExperimentComplete(); |
mchoun95 | 3:646dc3b2a681 | 407 | tailPWM.write(0); |
mchoun95 | 3:646dc3b2a681 | 408 | legPWM.write(0); |
mchoun95 | 3:646dc3b2a681 | 409 | state = INIT; |
pwensing | 0:43448bf056e8 | 410 | } // end if |
pwensing | 0:43448bf056e8 | 411 | } // end while |
pwensing | 0:43448bf056e8 | 412 | } // end main |