Mark Chounlakone / Mbed 2 deprecated Jerby_v2

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 Patrick Wensing

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?

UserRevisionLine numberNew 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