main.cpp

00001 #include "mbed.h"
00002 #include "QEI.h"
00003 #define CONTROL_PERIOD 0.002 // 500Hz ***
00004 #define SAVE_PERIOD 0.005 // 200HZ
00005 
00006 const int n_samples = 165;
00007 
00008 // 1000 x 3 array of degree values
00009 const float trajectory[n_samples][3] = {
00010 34,10,0,
00011 34,10,0,
00012 34,10,0,
00013 34,10,0,
00014 34,10,0,
00015 34,10,-1,
00016 34,10,-1,
00017 34,10,-1,
00018 34,10,-2,
00019 34,10,-2,
00020 34,10,-2,
00021 34,10,-3,
00022 34,10,-3,
00023 34,10,-3,
00024 33,11,-3,
00025 33,11,-4,
00026 33,11,-4,
00027 33,11,-4,
00028 33,11,-4,
00029 32,12,-4,
00030 32,12,-5,
00031 32,12,-5,
00032 31,13,-5,
00033 31,13,-5,
00034 31,13,-5,
00035 30,14,-5,
00036 30,14,-6,
00037 30,14,-6,
00038 29,15,-6,
00039 29,15,-6,
00040 28,16,-6,
00041 28,16,-6,
00042 27,17,-6,
00043 27,17,-6,
00044 26,18,-6,
00045 26,18,-6,
00046 25,19,-6,
00047 25,19,-6,
00048 24,20,-6,
00049 24,20,-6,
00050 23,21,-6,
00051 23,21,-6,
00052 22,22,-6,
00053 22,22,-6,
00054 21,23,-6,
00055 20,24,-6,
00056 20,24,-6,
00057 19,25,-6,
00058 19,25,-6,
00059 18,26,-6,
00060 18,26,-6,
00061 17,27,-6,
00062 17,27,-6,
00063 16,28,-6,
00064 16,28,-6,
00065 15,29,-6,
00066 15,29,-6,
00067 15,29,-6,
00068 14,30,-6,
00069 14,30,-5,
00070 13,31,-5,
00071 13,31,-5,
00072 13,31,-5,
00073 12,32,-5,
00074 12,32,-5,
00075 12,32,-4,
00076 12,32,-4,
00077 11,33,-4,
00078 11,33,-4,
00079 11,33,-4,
00080 11,33,-3,
00081 10,34,-3,
00082 10,34,-3,
00083 10,34,-3,
00084 10,34,-2,
00085 10,34,-2,
00086 10,34,-2,
00087 10,34,-1,
00088 10,34,-1,
00089 10,34,-1,
00090 10,34,0,
00091 10,34,0,
00092 10,34,0,
00093 10,34,0,
00094 10,34,0,
00095 10,34,0,
00096 10,34,0,
00097 10,34,1,
00098 10,34,1,
00099 10,34,1,
00100 10,34,2,
00101 10,34,2,
00102 10,34,2,
00103 10,34,2,
00104 10,34,3,
00105 10,34,3,
00106 10,34,3,
00107 11,33,3,
00108 11,33,4,
00109 11,33,4,
00110 11,33,4,
00111 12,32,4,
00112 12,32,5,
00113 12,32,5,
00114 12,32,5,
00115 13,31,5,
00116 13,31,5,
00117 13,31,5,
00118 14,30,6,
00119 14,30,6,
00120 15,29,6,
00121 15,29,6,
00122 15,29,6,
00123 16,28,6,
00124 16,28,6,
00125 17,27,6,
00126 17,27,6,
00127 18,26,6,
00128 18,26,6,
00129 19,25,6,
00130 19,25,6,
00131 20,24,6,
00132 20,24,6,
00133 21,23,6,
00134 22,22,6,
00135 22,22,6,
00136 23,21,6,
00137 23,21,6,
00138 24,20,6,
00139 24,20,6,
00140 25,19,6,
00141 25,19,6,
00142 26,18,6,
00143 26,18,6,
00144 27,17,6,
00145 27,17,6,
00146 28,16,6,
00147 28,16,6,
00148 29,15,6,
00149 29,15,6,
00150 30,14,6,
00151 30,14,6,
00152 30,14,5,
00153 31,13,5,
00154 31,13,5,
00155 31,13,5,
00156 32,12,5,
00157 32,12,5,
00158 32,12,4,
00159 33,11,4,
00160 33,11,4,
00161 33,11,4,
00162 33,11,3,
00163 33,11,3,
00164 34,10,3,
00165 34,10,3,
00166 34,10,2,
00167 34,10,2,
00168 34,10,2,
00169 34,10,2,
00170 34,10,1,
00171 34,10,1,
00172 34,10,1,
00173 34,10,0,
00174 34,10,0
00175 };
00176 
00177 //control flow
00178 const int standing_time = 250; //samples at 500Hz, time to stand up
00179 const float standing_step = 1/(float)standing_time; //recipherical of standing_time
00180 const int sitting_time = 1000; //samples at 500Hz, time to sit down
00181 const float sitting_step = 1/(float)sitting_time; //recipherical of sitting time
00182 volatile int getting_up = 1; //are we currently in the process of standing up
00183 volatile int getting_down = 0; //are we currently in the process of sitting down
00184 const float standing_position = 34.0;
00185 const float bent_position = 10.0;
00186 const float spine_start = trajectory[0][2];
00187 
00188 volatile int current_sample = 0;
00189 volatile int current_loop = 0;
00190 const int n_loops = 12;
00191 
00192 Ticker tick;
00193 Ticker tock;
00194 Timer t;
00195 
00196 Serial pc(USBTX, USBRX); // tx, rx
00197 LocalFileSystem local("data");               // Create the local filesystem under the name "local"
00198 
00199 // Declare Three Encoders
00200 QEI rear_encoder(p23, p14, NC, 1200, QEI::X4_ENCODING);  // rear leg
00201 QEI front_encoder(p21, p16, NC, 1200, QEI::X4_ENCODING);  // front leg
00202 QEI spine_encoder(p22, p15, NC, 1200, QEI::X4_ENCODING);  // spine
00203 
00204 // Specify pinout
00205 DigitalOut  rear_motorA(p7);
00206 DigitalOut  rear_motorB(p6);
00207 PwmOut      rear_motorPWM(p26);
00208 AnalogIn    rear_cs(p18);
00209 
00210 DigitalOut  front_motorA(p11);
00211 DigitalOut  front_motorB(p10);
00212 PwmOut      front_motorPWM(p24);
00213 AnalogIn    front_cs(p20);
00214 
00215 DigitalOut  spine_motorA(p9);
00216 DigitalOut  spine_motorB(p8);
00217 PwmOut      spine_motorPWM(p25);
00218 AnalogIn    spine_cs(p19);
00219 
00220 //LEDs for current safety
00221 DigitalOut rear_led(LED1);
00222 DigitalOut front_led(LED2);
00223 DigitalOut spine_led(LED3);
00224 
00225 //number domains for abstraction
00226 const int rear = 0;
00227 const int front = 1;
00228 const int spine = 2;
00229 
00230 // Sensing Variables
00231 volatile int i = 0;
00232 volatile float w = 0;
00233 volatile int id = 4000;
00234 volatile int sign = 0;
00235 
00236 volatile int n[3] = {0,0,0}; //encoder values
00237 volatile int last_n[3] = {0,0,0}; //previous encoder values
00238 
00239 volatile float avg_current[3] = {0.0,0.0,0.0}; //integration of current in time
00240 
00241 // Output Variables
00242 volatile float pwm = 0;
00243 
00244 // Control Constants
00245 const float R = 2.3/1000.0; // [kohm]
00246 const float Kv = 0.1682;
00247 const int Vs = 18; // [V]
00248 const float n2d = 3.3333;
00249 
00250 const float integ_alpha = .05; //peristence of current integration. 0->all past, 1->all present
00251 const float stall_current = 8000; //mA
00252 
00253 const float time_out_current = 5000; //mA if avg_current is above, increment the timeout count
00254 const int time_out_steps = 400; //max steps after which kill the test
00255 volatile int time_out_count[3] = {0,0,0};  //counter for time out
00256 int kill_test = 0; //kill switch
00257 
00258 // Control Parameters
00259 float rear_Kp = 0.001;
00260 float rear_Ks_p = 220.0;
00261 float rear_Ks_d = 500.0;
00262 
00263 float front_Kp = 0.001;
00264 float front_Ks_p = 220.0;
00265 float front_Ks_d = 500.0;
00266 
00267 float spine_Kp = 0.001;
00268 float spine_Ks_p = 300.0;
00269 float spine_Ks_d = 400.0;
00270 
00271 float rear_n_d = 0.0*n2d;
00272 float front_n_d = 0.0*n2d;
00273 float spine_n_d = 0.0*n2d;
00274 float rear_w_d = 0;
00275 float front_w_d = 0;
00276 float spine_w_d = 0;
00277 
00278 
00279 FILE *fp = fopen("/data/out.txt", "w");  // Open "out.txt" on the local file system for writing
00280 
00281 int read_current(int which_domain) {
00282     int current = 0;
00283     if (which_domain == rear) {                // rear
00284         current = rear_cs.read()*23570;
00285     } else if (which_domain == front) {         // front
00286         current = front_cs.read()*23570;
00287     } else if (which_domain == spine){          // spine
00288         current = spine_cs.read()*23570;
00289     }
00290     avg_current[which_domain] = (1-integ_alpha)*avg_current[which_domain] + integ_alpha*current;  //integrate
00291     return current; //mA
00292 }
00293 
00294 void updateMotor(int which_motor, float power) {
00295     int dir = 0;
00296     
00297     if (power < 0) { 
00298         power = -power; 
00299         dir = 0;
00300     } else {
00301         dir = 1;
00302     }
00303     if (power > 1) { power = 1; }
00304     if (power < 0) { power = 0; }
00305     
00306     if (which_motor == rear) {                 // rear
00307         if (dir == 1) {
00308             rear_motorA = 0;
00309             rear_motorB = 1;
00310         } else {
00311             rear_motorA = 1;
00312             rear_motorB = 0;
00313         }
00314         rear_motorPWM.write(power);
00315     } else if (which_motor == front) {          // front
00316         if (dir == 1) {
00317             front_motorA = 0;
00318             front_motorB = 1;
00319         } else {
00320             front_motorA = 1;
00321             front_motorB = 0;
00322         }
00323         front_motorPWM.write(power);
00324     } else if (which_motor == spine) {          // spine
00325         if (dir == 1) {
00326             spine_motorA = 0;
00327             spine_motorB = 1;
00328         } else {
00329             spine_motorA = 1;
00330             spine_motorB = 0;
00331         }
00332         spine_motorPWM.write(power);
00333     }
00334 }
00335 
00336 float updateEncoder(int which_encoder) {
00337     last_n[which_encoder] = n[which_encoder];
00338     if(which_encoder == rear){
00339         n[which_encoder] = rear_encoder.getPulses();
00340     }
00341     else if(which_encoder == front){
00342         n[which_encoder] = front_encoder.getPulses();
00343     }
00344     else if(which_encoder == spine){
00345         n[which_encoder] = spine_encoder.getPulses();
00346     }
00347     w = (n[which_encoder]-last_n[which_encoder])*1.047;       //steps/s --> rad/s
00348     return w;
00349  }
00350 
00351 void shutdown(){
00352     tick.detach();
00353     tock.detach();
00354     updateMotor(rear, 0);
00355     updateMotor(front, 0);
00356     updateMotor(spine, 0);
00357     fclose(fp);
00358     kill_test = 1;
00359 }
00360 
00361 void control() {
00362     if(getting_up){
00363         rear_n_d = -standing_position*(standing_step*current_sample)*n2d; //linear ramp up over 500 samples
00364         front_n_d = -bent_position*(standing_step*current_sample)*n2d; //linear ramp up over 500 samples
00365         spine_n_d = -spine_start*(standing_step*current_sample)*n2d; 
00366     }
00367     else if(getting_down){
00368         rear_n_d = -standing_position*(1-sitting_step*current_sample)*n2d; //linear ramp up over 500 samples
00369         front_n_d = -bent_position*(1-sitting_step*current_sample)*n2d; //linear ramp up over 500 samples
00370         spine_n_d = -spine_start*(1-sitting_step*current_sample)*n2d;; 
00371     }
00372     else{
00373         rear_n_d = -trajectory[current_sample][rear]*n2d;
00374         front_n_d = -trajectory[current_sample][front]*n2d;
00375         spine_n_d = -trajectory[current_sample][spine]*n2d;
00376     }
00377     
00378     // rear
00379     i = read_current(rear);
00380     w = updateEncoder(rear);      
00381     id = rear_Ks_p*(rear_n_d-n[rear]) + rear_Ks_d*(rear_w_d-w);
00382     sign = abs(id)/id;
00383     id = abs(id);
00384     pwm = sign*(id*R-sign*Kv*w+rear_Kp*(id-i))/Vs;
00385     if (avg_current[rear] > stall_current){pwm = 0;}
00386     updateMotor(rear,pwm); 
00387     
00388     // front
00389     i = read_current(front);
00390     w = updateEncoder(front);      
00391     id = front_Ks_p*(front_n_d-n[front]) + front_Ks_d*(front_w_d-w);
00392     sign = abs(id)/id;
00393     id = abs(id);
00394     pwm = sign*(id*R-sign*Kv*w+front_Kp*(id-i))/Vs;
00395     if (avg_current[front] > stall_current){pwm = 0;}
00396     updateMotor(front,pwm); 
00397     
00398     // spine
00399     i = read_current(spine);
00400     w = updateEncoder(spine);
00401     id = spine_Ks_p*(spine_n_d-n[spine]) + spine_Ks_d*(spine_w_d-w);
00402     sign = abs(id)/id;
00403     id = abs(id);
00404     pwm = sign*(id*R-sign*Kv*w+spine_Kp*(id-i))/Vs;
00405     if (avg_current[spine] > stall_current){pwm = 0;}
00406     updateMotor(spine,pwm); 
00407 
00408     //timeout for motor safety
00409     if( avg_current[rear] > time_out_current){ time_out_count[rear]++;}
00410     if( avg_current[front] > time_out_current){ time_out_count[front]++;}
00411     if( avg_current[spine] > time_out_current){ time_out_count[spine]++;}
00412     if( time_out_count[rear]>time_out_steps){ rear_led=1;shutdown();}
00413     if( time_out_count[front]>time_out_steps){ front_led=1;shutdown();}
00414     if( time_out_count[spine]>time_out_steps){ spine_led=1;shutdown();}
00415 
00416 
00417 
00418     //step to next control point
00419     if (getting_up){
00420         if(current_sample == standing_time){ getting_up = 0; current_sample = 0;} //we're up
00421         else{  current_sample++;} //still getting up
00422     }
00423     else if(getting_down){
00424         if(current_sample == sitting_time){ //we're down
00425             shutdown();
00426         }
00427         else{current_sample++;}    //still getting down
00428     }
00429     else if (current_sample == n_samples-1){ //normal operation
00430         if (current_loop == n_loops-1){ getting_down = 1; current_sample=0;} //ready to sit
00431         else{ //end of loop, ready for next
00432             current_sample = 0;
00433             current_loop++;
00434         }
00435     }
00436     else{ current_sample++;} //middle of running loop
00437 } 
00438 
00439 void save() {
00440     pc.printf("%i %i %i %i %f %f %f %i\n", t.read_ms(), n[rear], n[front], n[spine], avg_current[0], avg_current[1], avg_current[2], current_sample);
00441     pc.printf("%i",rear_encoder.getPulses());
00442 }
00443 
00444 int main() {
00445     rear_motorPWM.period(0.00005);   //20kHz
00446     front_motorPWM.period(0.00005);   //20kHz
00447     spine_motorPWM.period(0.00005);   //20kHz
00448     tick.attach(&control,CONTROL_PERIOD);
00449     tock.attach(&save,SAVE_PERIOD);
00450     t.start();
00451     
00452     while(~kill_test) {
00453         //DEBUG
00454         save();
00455         //pc.printf("%i %f %i %f %i %i\n", t.read_ms(), pwm, n, w, id, i);
00456     }
00457 }