Mark Chounlakone / Mbed 2 deprecated Jerby

2.74 Jerboa Robot Code

Dependencies:   EthernetInterface ExperimentServer QEI_pmw experiment_example mbed-rtos mbed

Fork of experiment_example by Patrick Wensing

main.cpp@3:3c00f1332d9b, 2017-12-05 (annotated)

Committer:
mchoun95
Date:
Tue Dec 05 02:37:28 2017 +0000
Revision:
3:3c00f1332d9b
Parent:
1:95a7fddd25a9 
2.74 Jerboa Robot 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"
pwensing 0:43448bf056e8 6
mchoun95 3:3c00f1332d9b 7 #define NUM_INPUTS 10
mchoun95 3:3c00f1332d9b 8 #define NUM_OUTPUTS 9
pwensing 0:43448bf056e8 9
mchoun95 3:3c00f1332d9b 10 //leg 50 deg
mchoun95 3:3c00f1332d9b 11 //tail 45 deg
mchoun95 3:3c00f1332d9b 12
mchoun95 3:3c00f1332d9b 13 Ticker control;
pwensing 0:43448bf056e8 14 Serial pc(USBTX, USBRX); // USB Serial Terminal
pwensing 0:43448bf056e8 15 ExperimentServer server; // Object that lets us communicate with MATLAB
mchoun95 3:3c00f1332d9b 16
mchoun95 3:3c00f1332d9b 17 // Tail Motor Control Pins
mchoun95 3:3c00f1332d9b 18 PwmOut tailPWM(D5); // Tail motor PWM output
mchoun95 3:3c00f1332d9b 19 DigitalOut tailFwd(D6); // Tail motor forward enable
mchoun95 3:3c00f1332d9b 20 DigitalOut tailRev(D7); // Tail motor backward enable
mchoun95 3:3c00f1332d9b 21
mchoun95 3:3c00f1332d9b 22 // Leg Motor Control Pins
mchoun95 3:3c00f1332d9b 23 PwmOut legPWM(D11); // Leg motor PWM output
mchoun95 3:3c00f1332d9b 24 DigitalOut legFwd(D10); // Leg motor forward enable
mchoun95 3:3c00f1332d9b 25 DigitalOut legRev(D9); // Leg motor backward enable
mchoun95 3:3c00f1332d9b 26
mchoun95 3:3c00f1332d9b 27 Timer t; // Timer to measure elapsed time of experiment
mchoun95 3:3c00f1332d9b 28 float time_ = 0.0; // time variable
mchoun95 3:3c00f1332d9b 29 float tfinal = 20;
mchoun95 3:3c00f1332d9b 30
mchoun95 3:3c00f1332d9b 31 // Motor current sensors
mchoun95 3:3c00f1332d9b 32 AnalogIn tailCurrent(A0); // Tail current sensor
mchoun95 3:3c00f1332d9b 33 AnalogIn legCurrent(A1); // Leg current sensor
mchoun95 3:3c00f1332d9b 34
mchoun95 3:3c00f1332d9b 35 // Motor Encoders
mchoun95 3:3c00f1332d9b 36 QEI tailEncoder(D1 ,D2 ,NC ,1200, QEI::X4_ENCODING); // Tail encoder
mchoun95 3:3c00f1332d9b 37 QEI legEncoder(D3 ,D4 ,NC ,1200, QEI::X4_ENCODING); // Leg encoder
mchoun95 3:3c00f1332d9b 38
mchoun95 3:3c00f1332d9b 39 // Physical motor parameters
mchoun95 3:3c00f1332d9b 40 float R = 2.79; // Estimated motor resistance from lab
mchoun95 3:3c00f1332d9b 41 float Kb = .1603; // Estimated motor torque constant measured from lab
mchoun95 3:3c00f1332d9b 42 float Kcurrent = 1.5; // Proportional gain of the current control
mchoun95 3:3c00f1332d9b 43 float v = 0.01; // BackEMF constant
mchoun95 3:3c00f1332d9b 44
mchoun95 3:3c00f1332d9b 45
mchoun95 3:3c00f1332d9b 46 // Matlab inputs/Control variables
mchoun95 3:3c00f1332d9b 47 float tailCmd = 0.0; // Commanded angle (Rads) for the tail
mchoun95 3:3c00f1332d9b 48 float legCmd = 0.0; // Commanded angle (Rads) for the leg
mchoun95 3:3c00f1332d9b 49 float tailt0 = 0.0; // Initial angle (Rads)
mchoun95 3:3c00f1332d9b 50 float legt0 = 0.0; // Initial angle (Rads)
mchoun95 3:3c00f1332d9b 51 float Kptail = 0.0; // Tail proportional gain of position control
mchoun95 3:3c00f1332d9b 52 float Kpleg = 0.0; // Leg proportional gain of position control
mchoun95 3:3c00f1332d9b 53 float Kdtail = 0.0; // Tail derivative gain of position control
mchoun95 3:3c00f1332d9b 54 float Kdleg = 0.0; // Leg derivative gain for position control
mchoun95 3:3c00f1332d9b 55
mchoun95 3:3c00f1332d9b 56 // State varibles/sensed parameters
mchoun95 3:3c00f1332d9b 57 float idtail = 0.0; // Tail desired current
mchoun95 3:3c00f1332d9b 58 float idleg = 0.0; // Leg desired current
mchoun95 3:3c00f1332d9b 59 float taild = 0.0; // Desired angle (Rads) for the tail
mchoun95 3:3c00f1332d9b 60 float legd = 0.0; // Desired angle (Rads) for the leg
mchoun95 3:3c00f1332d9b 61 float wtail = 0.0; // Measured angular velocity of the tail
mchoun95 3:3c00f1332d9b 62 float wleg = 0.0; // Measured angular velocity of the leg
mchoun95 3:3c00f1332d9b 63 float ttail = 0.0; // Measured angular position of the tail
mchoun95 3:3c00f1332d9b 64 float tleg = 0.0; // Measured angular position of the leg
mchoun95 3:3c00f1332d9b 65 float itail = 0.0; // Current sensed in the tail motor
mchoun95 3:3c00f1332d9b 66 float ileg = 0.0; // Current sensed in the leg motor
mchoun95 3:3c00f1332d9b 67 float vctail = 0.0; // Tail voltage command
mchoun95 3:3c00f1332d9b 68 float vcleg = 0.0; // Leg voltage command
mchoun95 3:3c00f1332d9b 69
mchoun95 3:3c00f1332d9b 70 // Error terms
mchoun95 3:3c00f1332d9b 71 float etail = 0.0; // Error term for tail
mchoun95 3:3c00f1332d9b 72 float eleg = 0.0; // Error term for leg
mchoun95 3:3c00f1332d9b 73 float eptail = 0.0; // Previous error term for tail
mchoun95 3:3c00f1332d9b 74 float epleg = 0.0; // Previous error term for leg
mchoun95 3:3c00f1332d9b 75 float edtail = 0.0; // Derivative error term for tail
mchoun95 3:3c00f1332d9b 76 float edleg = 0.0; // Derivative error term for leg
mchoun95 3:3c00f1332d9b 77
mchoun95 3:3c00f1332d9b 78 float max_cmd = 1.0;
mchoun95 3:3c00f1332d9b 79
mchoun95 3:3c00f1332d9b 80 bool new_data = false;
mchoun95 3:3c00f1332d9b 81 bool end = false;
mchoun95 3:3c00f1332d9b 82
pwensing 0:43448bf056e8 83
mchoun95 3:3c00f1332d9b 84 void control_loop() {
mchoun95 3:3c00f1332d9b 85 if (end){
mchoun95 3:3c00f1332d9b 86 tailPWM.write(0);
mchoun95 3:3c00f1332d9b 87 legPWM.write(0);
mchoun95 3:3c00f1332d9b 88 }
mchoun95 3:3c00f1332d9b 89 else{
mchoun95 3:3c00f1332d9b 90 //Sensing
mchoun95 3:3c00f1332d9b 91 itail = (tailCurrent.read()-.5)*36.7; // Tail current
mchoun95 3:3c00f1332d9b 92 ileg = (legCurrent.read()-.5)*36.7; // Leg current
mchoun95 3:3c00f1332d9b 93 ttail = tailEncoder.getPulses()/1200.0*2*3.14159 + tailt0; // Leg angle
mchoun95 3:3c00f1332d9b 94 tleg = legEncoder.getPulses()/1200.0*2*3.14159 + legt0; // Leg angle
mchoun95 3:3c00f1332d9b 95 wtail = tailEncoder.getVelocity()*2*3.14159/1200.0; // Tail angular velocity
mchoun95 3:3c00f1332d9b 96 wleg = legEncoder.getVelocity()*2*3.14159/1200.0; // Leg angular velocity
mchoun95 3:3c00f1332d9b 97 time_ = t.read();
mchoun95 3:3c00f1332d9b 98
mchoun95 3:3c00f1332d9b 99 // Error update
mchoun95 3:3c00f1332d9b 100 etail = taild - ttail; // Error in tail's angle position
mchoun95 3:3c00f1332d9b 101 eleg = legd - tleg; // Error in leg's angle position
mchoun95 3:3c00f1332d9b 102 edtail = (etail - eptail)/.001; // Rate of change in tail's error
mchoun95 3:3c00f1332d9b 103 edleg = (eleg - epleg)/.001; // Rate of change in leg's error
mchoun95 3:3c00f1332d9b 104 eptail = etail; // Update previous tail error
mchoun95 3:3c00f1332d9b 105 epleg = eleg; // Update previous leg error
mchoun95 3:3c00f1332d9b 106
mchoun95 3:3c00f1332d9b 107 // Perform control loop logic
mchoun95 3:3c00f1332d9b 108 idtail = (Kptail*etail + Kdtail*edtail + v*wtail) / Kb; // Tail position control
mchoun95 3:3c00f1332d9b 109 idleg = (Kpleg*eleg + Kdleg*edleg + v*wleg) / Kb; // Leg position control
mchoun95 3:3c00f1332d9b 110
mchoun95 3:3c00f1332d9b 111 vctail = (R*idtail + Kcurrent * (idtail - itail) - Kb * wtail) / 24.0; // Tail current control
mchoun95 3:3c00f1332d9b 112 vcleg = (R*idleg + Kcurrent * (idleg - ileg) - Kb * wleg) / 24.0; // Leg current control
mchoun95 3:3c00f1332d9b 113
mchoun95 3:3c00f1332d9b 114 // Tail set command
mchoun95 3:3c00f1332d9b 115 if (vctail < 0){
mchoun95 3:3c00f1332d9b 116 tailFwd = 1;
mchoun95 3:3c00f1332d9b 117 tailRev = 0;
mchoun95 3:3c00f1332d9b 118 if (abs(vctail) > 1){
mchoun95 3:3c00f1332d9b 119 vctail = -1;
mchoun95 3:3c00f1332d9b 120 }
mchoun95 3:3c00f1332d9b 121 tailPWM.write(max_cmd*abs(vctail));
mchoun95 3:3c00f1332d9b 122 }else if (vctail > 0){
mchoun95 3:3c00f1332d9b 123 tailFwd = 0;
mchoun95 3:3c00f1332d9b 124 tailRev = 1;
mchoun95 3:3c00f1332d9b 125 if (abs(vctail) > 1){
mchoun95 3:3c00f1332d9b 126 vctail = 1;
mchoun95 3:3c00f1332d9b 127 }
mchoun95 3:3c00f1332d9b 128 tailPWM.write(max_cmd*abs(vctail));
mchoun95 3:3c00f1332d9b 129 } else {
mchoun95 3:3c00f1332d9b 130 tailPWM.write(0);
mchoun95 3:3c00f1332d9b 131 }
mchoun95 3:3c00f1332d9b 132
mchoun95 3:3c00f1332d9b 133 // Leg set command
mchoun95 3:3c00f1332d9b 134 if (vcleg < 0){
mchoun95 3:3c00f1332d9b 135 legFwd = 1;
mchoun95 3:3c00f1332d9b 136 legRev = 0;
mchoun95 3:3c00f1332d9b 137 if (abs(vcleg) > 1){
mchoun95 3:3c00f1332d9b 138 vcleg = -1;
mchoun95 3:3c00f1332d9b 139 }
mchoun95 3:3c00f1332d9b 140 legPWM.write(max_cmd*abs(vcleg));
mchoun95 3:3c00f1332d9b 141 }else if (vcleg > 0){
mchoun95 3:3c00f1332d9b 142 legFwd = 0;
mchoun95 3:3c00f1332d9b 143 legRev = 1;
mchoun95 3:3c00f1332d9b 144 if (abs(vcleg) > 1){
mchoun95 3:3c00f1332d9b 145 vcleg = 1;
mchoun95 3:3c00f1332d9b 146 }
mchoun95 3:3c00f1332d9b 147 legPWM.write(max_cmd*abs(vcleg));
mchoun95 3:3c00f1332d9b 148 } else {
mchoun95 3:3c00f1332d9b 149 legPWM.write(0);
mchoun95 3:3c00f1332d9b 150 }
mchoun95 3:3c00f1332d9b 151 new_data = true;
mchoun95 3:3c00f1332d9b 152 }
mchoun95 3:3c00f1332d9b 153 }
pwensing 0:43448bf056e8 154
pwensing 0:43448bf056e8 155 int main (void) {
pwensing 0:43448bf056e8 156 // Link the terminal with our server and start it up
pwensing 0:43448bf056e8 157 server.attachTerminal(pc);
pwensing 0:43448bf056e8 158 server.init();
pwensing 0:43448bf056e8 159
Patrick Wensing 1:95a7fddd25a9 160 // PWM period should nominally be a multiple of our control loop
mchoun95 3:3c00f1332d9b 161 tailPWM.period_us(500);
mchoun95 3:3c00f1332d9b 162 legPWM.period_us(500);
pwensing 0:43448bf056e8 163
pwensing 0:43448bf056e8 164 // Continually get input from MATLAB and run experiments
pwensing 0:43448bf056e8 165 float input_params[NUM_INPUTS];
mchoun95 3:3c00f1332d9b 166 control.attach(&control_loop, .001);
mchoun95 3:3c00f1332d9b 167
pwensing 0:43448bf056e8 168
pwensing 0:43448bf056e8 169 while(1) {
pwensing 0:43448bf056e8 170 if (server.getParams(input_params,NUM_INPUTS)) {
mchoun95 3:3c00f1332d9b 171 tailCmd = input_params[0]; // Desired angle (Rads) for the tail
mchoun95 3:3c00f1332d9b 172 legCmd = input_params[1]; // Desired angle (Rads) for the leg
mchoun95 3:3c00f1332d9b 173 tailt0 = input_params[2]; // Initial angle (Rads)
mchoun95 3:3c00f1332d9b 174 legt0 = input_params[3]; // Initial angle (Rads)
mchoun95 3:3c00f1332d9b 175 Kptail = input_params[4]; // Tail proportional gain of position control
mchoun95 3:3c00f1332d9b 176 Kpleg = input_params[5]; // Leg proportional gain of position control
mchoun95 3:3c00f1332d9b 177 Kdtail = input_params[6]; // Tail derivative gain of position control
mchoun95 3:3c00f1332d9b 178 Kdleg = input_params[7]; // Leg derivative gain for position control
mchoun95 3:3c00f1332d9b 179 max_cmd = input_params[8]; // cmd thresholder
mchoun95 3:3c00f1332d9b 180 tfinal = input_params[9];
mchoun95 3:3c00f1332d9b 181
mchoun95 3:3c00f1332d9b 182 if (max_cmd > 1.0 || max_cmd < 0){
mchoun95 3:3c00f1332d9b 183 max_cmd = 1.0;
mchoun95 3:3c00f1332d9b 184 }
pwensing 0:43448bf056e8 185 // Setup experiment
pwensing 0:43448bf056e8 186 t.reset();
pwensing 0:43448bf056e8 187 t.start();
mchoun95 3:3c00f1332d9b 188 end = false;
mchoun95 3:3c00f1332d9b 189
mchoun95 3:3c00f1332d9b 190 // Reset the encoders
mchoun95 3:3c00f1332d9b 191 tailEncoder.reset();
mchoun95 3:3c00f1332d9b 192 legEncoder.reset();
mchoun95 3:3c00f1332d9b 193
mchoun95 3:3c00f1332d9b 194 // Default forward rotation
mchoun95 3:3c00f1332d9b 195 tailFwd = 1;
mchoun95 3:3c00f1332d9b 196 tailRev = 0;
mchoun95 3:3c00f1332d9b 197 legFwd = 1;
mchoun95 3:3c00f1332d9b 198 legRev = 0;
mchoun95 3:3c00f1332d9b 199
mchoun95 3:3c00f1332d9b 200 // Stop the motors
mchoun95 3:3c00f1332d9b 201 tailPWM.write(0);
mchoun95 3:3c00f1332d9b 202 legPWM.write(0);
mchoun95 3:3c00f1332d9b 203
pwensing 0:43448bf056e8 204 // Run experiment
mchoun95 3:3c00f1332d9b 205 while( t.read() < tfinal) {
mchoun95 3:3c00f1332d9b 206 // Set desired parameters at specific times
mchoun95 3:3c00f1332d9b 207 if(t.read()<5){
mchoun95 3:3c00f1332d9b 208 taild = tailt0;
mchoun95 3:3c00f1332d9b 209 legd = legt0;
mchoun95 3:3c00f1332d9b 210 }else{
mchoun95 3:3c00f1332d9b 211 taild = tailCmd;
mchoun95 3:3c00f1332d9b 212 legd = legCmd;
mchoun95 3:3c00f1332d9b 213 }
mchoun95 3:3c00f1332d9b 214
pwensing 0:43448bf056e8 215 // Send data to MATLAB
mchoun95 3:3c00f1332d9b 216 if(new_data) {
mchoun95 3:3c00f1332d9b 217 // Form output to send to MATLAB
mchoun95 3:3c00f1332d9b 218 float output_data[NUM_OUTPUTS];
mchoun95 3:3c00f1332d9b 219 output_data[0] = time_;
mchoun95 3:3c00f1332d9b 220 output_data[1] = vctail;
mchoun95 3:3c00f1332d9b 221 output_data[2] = vcleg;
mchoun95 3:3c00f1332d9b 222 output_data[3] = itail;
mchoun95 3:3c00f1332d9b 223 output_data[4] = ileg;
mchoun95 3:3c00f1332d9b 224 output_data[5] = ttail;
mchoun95 3:3c00f1332d9b 225 output_data[6] = tleg;
mchoun95 3:3c00f1332d9b 226 output_data[7] = wtail;
mchoun95 3:3c00f1332d9b 227 output_data[8] = wleg;
mchoun95 3:3c00f1332d9b 228 server.sendData(output_data,NUM_OUTPUTS);
mchoun95 3:3c00f1332d9b 229 new_data = false;
mchoun95 3:3c00f1332d9b 230 }
pwensing 0:43448bf056e8 231 }
mchoun95 3:3c00f1332d9b 232 end = true;
pwensing 0:43448bf056e8 233 // Cleanup after experiment
pwensing 0:43448bf056e8 234 server.setExperimentComplete();
mchoun95 3:3c00f1332d9b 235 tailPWM.write(0);
mchoun95 3:3c00f1332d9b 236 legPWM.write(0);
pwensing 0:43448bf056e8 237 } // end if
pwensing 0:43448bf056e8 238 } // end while
pwensing 0:43448bf056e8 239 } // end main