main.cpp

00001 #include "mbed.h"
00002 #include "Servo.h"
00003 #include "rtos.h"
00004 #include "attitude.h"
00005 #include "math.h"
00006 
00007 Serial pc(USBTX, USBRX);
00008 
00009 Timer timer1;
00010 Timer timerwalk;
00011 
00012 Thread thread1;         
00013 Thread thread2;
00014 
00015 Servo Servo1(D6);
00016 Servo Servo2(D8);
00017 Servo Servo3(D9);
00018 Servo Servo4(D10);
00019 
00020 void IMU();
00021 void Cal_si();
00022 void Avg();
00023 void Cal_Cg_linear();
00024 void Cal_Cg_log();
00025 void move();
00026 void receive_hormone();
00027 void cal_step_down(); //calculate step
00028 void cal_step_up();
00029 void servo();
00030 void servo_Right();
00031 
00032 int walking_time;
00033 int i = 0;
00034 
00035 float avg = 0.00;
00036 float sum = 0.00;
00037 float roll_data[10];
00038 float Si = 0.00 ;
00039 float Cg = 0.00;
00040 float Cg_down = 0.00;
00041 float Cg_up = 0;
00042 float standardDeviation = 0;
00043 float sd = 0; 
00044  
00045 float pos_down_start = 1400.00;
00046 float pos_up_start = 1000.00;
00047 float down_degree = 90.00;
00048 float up_degree = 45.00; 
00049 float stepmin = 1;
00050 float round = 5;
00051 float waittime = 0.001 ; 
00052  
00053 float pos_down_left = 1400.00;
00054 float pos_up_left = 1000.00;
00055 float pos_down_end_left;
00056 float pos_up_end_left; 
00057 float state_count_left = 1;
00058 float round_count_left = 1;
00059 float step_down_left;
00060 float step_up_left;
00061  
00062 float pos_down_right = 1400.00;
00063 float pos_up_right = 1000.00;
00064 float pos_down_end_right;
00065 float pos_up_end_right;
00066 float state_count_right = 1;
00067 float round_count_right = 1;
00068 float step_up_right;
00069 float step_down_right;
00070 
00071  
00072 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00073 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00074                                                                                                                                                 //
00075 int main() {                                                                                                                                    //
00076     pc.baud(1000000);                                                                                                                           //
00077     timer1.start(); // start timer counting                                                                                                     //                                                                                                                          
00078     pc.printf("PRESS '1'\n");                                                                                                                   //
00079     if (pc.getc() == '1'){                                                                                                                      //
00080         thread2.start(servo);                                                                                                                   //                   
00081     }                                                                                                                                           //     
00082 }                                                                                                                                               //        
00083                                                                                                                                                 //
00084 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00085 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00086 
00087 ///////////////////////////Control SERVO////////////////////////////////////////
00088 ////////////////////////////////////////////////////////////////////////////////
00089 void servo() {
00090     pc.printf("start\n"); 
00091     attitude_setup();
00092     thread1.start(IMU);
00093     //wait(0.01);
00094     timerwalk.start(); // start timer counting
00095     move();
00096     //pc.printf("%f\n",roll_min);
00097     //pc.printf("%f\n",roll_max);
00098 }
00099 
00100 void receive_hormone(){
00101     down_degree = 90.00*(1.00-(0.06*Cg_down)); 
00102     /*pc.printf("%f \t",Si);
00103     pc.printf("down\t"); 
00104     pc.printf("%f \t",down_degree);
00105     pc.printf("%f\t",Cg);*/
00106     up_degree = 45.00*(1.00+(0.7*Cg_up));
00107     /*pc.printf("up\t"); 
00108     pc.printf("%f \t",up_degree);
00109     pc.printf("%f\n",Cg);*/
00110     if(down_degree < 85){
00111         down_degree = 85;
00112         if(up_degree > 75){
00113         up_degree = 75;
00114         }
00115     }
00116 }
00117 
00118 void cal_step_down(){
00119     //pc.printf("down"); 
00120     //pc.printf("%f \n",down_degree);
00121     pos_down_end_left = (1000.00 + ((700.00/90.00)*(down_degree))); 
00122     pos_down_end_right = (1070.00 + ((700.00/90.00)* (down_degree))); 
00123     if (pos_down_end_right > pos_down_end_left){
00124         step_down_right = (pos_down_end_right - pos_down_start)*stepmin/(pos_down_end_left - pos_down_start);
00125         step_down_left = stepmin;
00126     } else if (pos_down_end_right < pos_down_end_left){
00127         step_down_right = stepmin;
00128         step_down_left = (pos_down_end_left - pos_down_start)*stepmin/(pos_down_end_right - pos_down_start);
00129     } else{
00130         step_down_right = stepmin;
00131         step_down_left = stepmin;
00132     }
00133     /*pc.printf("pos_down_right");
00134     pc.printf("%f\n",pos_down_end_right);
00135     pc.printf("pos_down_left");
00136     pc.printf("%f\n",pos_down_end_left);
00137     pc.printf("step_down_right");
00138     pc.printf("%f\n",step_down_right);
00139     pc.printf("step_down_left");
00140     pc.printf("%f\n",step_down_left);*/
00141 }
00142  
00143 void cal_step_up(){
00144     //pc.printf("up"); 
00145     //pc.printf("%f \n",up_degree);    
00146     pos_up_end_left = 1000.00 + ((700.00/90.00)*(up_degree)); 
00147     pos_up_end_right = 1000.00 + ((700.00/90.00)* (up_degree)); 
00148     if (pos_up_end_right > pos_up_end_left){
00149         step_up_right = (pos_up_end_right - pos_up_start)*stepmin/(pos_up_end_left - pos_up_start);
00150         step_up_left = stepmin;
00151     } else if (pos_up_end_right < pos_up_end_left){
00152         step_up_right = stepmin;
00153         step_up_left = (pos_up_end_left - pos_up_start)*stepmin/(pos_up_end_right - pos_up_start);
00154     } else{
00155         step_up_right = stepmin;
00156         step_up_left = stepmin;
00157     }
00158     /*pc.printf("pos_up_right");
00159     pc.printf("%f\n",pos_up_end_right);
00160     pc.printf("pos_up_left");
00161     pc.printf("%f\n",pos_up_end_left);
00162     pc.printf("step_up_right");
00163     pc.printf("%f\n",step_up_right);;
00164     pc.printf("step_up_left");
00165     pc.printf("%f\n",step_up_left);  */
00166 }
00167  
00168 void move(){
00169     Servo1.Enable(1000,20000);
00170     Servo2.Enable(1000,20000);
00171     Servo3.Enable(1000,20000);
00172     Servo4.Enable(1000,20000);
00173     
00174     /*pc.printf("Si\t"); 
00175     pc.printf("%f \t",Si);
00176     pc.printf("down\t"); 
00177     pc.printf("%f \t",down_degree);
00178     pc.printf("%f\t",Cg);
00179     pc.printf("up\t"); 
00180     pc.printf("%f \t",up_degree);
00181     pc.printf("%f\n",Cg);*/
00182     while(1) {
00183         cal_step_down();
00184         cal_step_up();
00185         servo_Right();
00186         if(state_count_left == 1) {
00187             Servo1.SetPosition(pos_down_left);
00188             wait(waittime);
00189             pos_down_left = pos_down_left + step_down_left;
00190             if(pos_down_left >= pos_down_end_left + step_down_left and pos_up_left == pos_up_start) {
00191                 state_count_left = 2;
00192             }
00193             /*pc.printf("LAD");
00194             pc.printf("%f\n",pos_down_left);
00195             pc.printf("LAP");
00196             pc.printf("%f\n",pos_up_left);*/
00197         } else if(state_count_left == 2) {
00198             Servo2.SetPosition(pos_up_left);
00199             wait(waittime);
00200             pos_up_left = pos_up_left + step_up_left;
00201             if(pos_down_left >= pos_down_end_left + step_down_left and pos_up_left >= pos_up_end_left + step_up_left) {
00202                 state_count_left = 3;
00203             }
00204             /*pc.printf("LBD");
00205             pc.printf("%f\n",pos_down_left);
00206             pc.printf("LBP");
00207             pc.printf("%f\n",pos_up_left);*/
00208         } else if(state_count_left == 3) {
00209             Servo1.SetPosition(pos_down_left);
00210             wait(waittime);
00211             pos_down_left = pos_down_left - step_down_left;
00212             if(pos_down_left <= pos_down_start - step_down_left and pos_up_left >= pos_up_end_left + step_up_left) {
00213                 state_count_left = 4;
00214             }
00215             /*pc.printf("LCD");
00216             pc.printf("%f\n",pos_down_left);
00217             pc.printf("LCP");
00218             pc.printf("%f\n",pos_up_left);*/
00219         } else if(state_count_left == 4) {
00220             Servo2.SetPosition(pos_up_left);
00221             wait(waittime);
00222             pos_up_left = pos_up_left - step_up_left;
00223             if(pos_down_left <= pos_down_start - step_down_left and pos_up_left <= pos_up_start - step_up_left) {
00224                 state_count_left = 0;
00225             }
00226             /*pc.printf("LDD");
00227             pc.printf("%f\n",pos_down_left);
00228             pc.printf("LDP");
00229             pc.printf("%f\n",pos_up_left);*/
00230         } else if (state_count_left == 0 and round_count_left < round) {
00231             round_count_left = round_count_left+1;
00232             state_count_left = 1;
00233             pos_down_left = pos_down_start;
00234             pos_up_left = pos_up_start;
00235         } else if (state_count_left == 0 and round_count_left == round and state_count_right == 0 and round_count_right == round){
00236             thread1.terminate();
00237             pc.printf("Finish \n");
00238             walking_time = timerwalk.read_ms(); 
00239             pc.printf("Walking time = %d  \n", walking_time); 
00240             break;         
00241         }
00242     }
00243 }
00244  
00245 void servo_Right()
00246 {
00247     if(state_count_right == 1) {
00248         Servo3.SetPosition(pos_down_right);
00249         wait(waittime);
00250         pos_down_right = pos_down_right + step_down_right;
00251         if(pos_down_right >= pos_down_end_right + step_down_right and pos_up_right == pos_up_start) {
00252             state_count_right = 2;
00253         }
00254         /*pc.printf("RAD");
00255         pc.printf("%f\n",pos_down_right);
00256         pc.printf("RAP");
00257         pc.printf("%f\n",pos_up_right);*/
00258     } else if(state_count_right == 2) {
00259         Servo4.SetPosition(pos_up_right);
00260         wait(waittime);
00261         pos_up_right = pos_up_right + step_up_right;
00262         if(pos_down_right >= pos_down_end_right + step_down_right and pos_up_right >= pos_up_end_right + step_up_right) {
00263             state_count_right = 3;
00264         }
00265         /*pc.printf("RBD");
00266         pc.printf("%f\n",pos_down_right);
00267         pc.printf("RBP");
00268         pc.printf("%f\n",pos_up_right);*/
00269     } else if(state_count_right == 3) {
00270         Servo3.SetPosition(pos_down_right);
00271         wait(waittime);
00272         pos_down_right = pos_down_right - step_down_right;
00273         if(pos_down_right <= pos_down_start - step_down_right and pos_up_right >= pos_up_end_right + step_up_right) {
00274             state_count_right = 4;
00275         }
00276         /*pc.printf("RCD");
00277         pc.printf("%f\n",pos_down_right);
00278         pc.printf("RCP");
00279         pc.printf("%f\n",pos_up_right);*/
00280     } else if(state_count_right == 4) {
00281         Servo4.SetPosition(pos_up_right);
00282         wait(waittime);
00283         pos_up_right = pos_up_right - step_up_right;
00284         if(pos_down_right <= pos_down_start - step_down_right and pos_up_right <= pos_up_start - step_up_right) {
00285             state_count_right = 0;
00286         }
00287         /*pc.printf("RDD");
00288         pc.printf("%f\n",pos_down_right);
00289         pc.printf("RDP");
00290         pc.printf("%f\n",pos_up_right);*/
00291     } else if (state_count_right == 0 and round_count_right < round) {
00292         round_count_right = round_count_right+1;
00293         state_count_right = 1;
00294         pos_down_right = pos_down_start;
00295         pos_up_right = pos_up_start;
00296         receive_hormone();
00297     }
00298 }
00299 
00300 ///////////////////////////Control IMU////////////////////////////////////////
00301 ////////////////////////////////////////////////////////////////////////////////
00302 void IMU(){
00303     pc.printf("roll\t");
00304     pc.printf("Si\t");
00305     pc.printf("Cg\t");
00306     pc.printf("down\t");
00307     pc.printf("up\n"); 
00308     
00309     while(1) {
00310         if (timer1.read_us()  >=10000)// read time in ms
00311         {
00312             attitude_get();
00313             
00314             //pc.printf(" %f \t", ax*10 ); 
00315             //pc.printf("  %f \t", ay*10 ); 
00316             //pc.printf("  %f \t", az*10-10); //cm/s*s
00317 
00318             //pc.printf("%f\t  %.0f \t  %.0f \n\r",  roll,   pitch, yaw);
00319             
00320             //pc.printf("up\t"); 
00321             //pc.printf("%f \t",up_degree);
00322             //pc.printf("%f\t",Cg);
00323             //pc.printf("down\t"); 
00324             //pc.printf("%f \t",down_degree);
00325             //pc.printf("%f\n",Cg);
00326          
00327             timer1.reset(); // reset timer 
00328             Cal_si();
00329         }
00330     }
00331 }
00332 
00333 void Cal_si(){
00334     if(state_count_left == 4 or state_count_right == 4){
00335         roll_data[i] = roll;
00336         //pc.printf("%f\n",roll_data[i]);
00337         Avg();
00338         //pc.printf("Avg ");
00339         //pc.printf("%f\n",avg);
00340         Cal_Cg_linear();
00341         //pc.printf("roll\t");
00342         pc.printf("%f\t",roll_data[i]);
00343         //pc.printf("Si\t");
00344         pc.printf("%f\t",Si);
00345         //pc.printf("Cg\t");
00346         pc.printf("%f\t",Cg);
00347         //pc.printf("down\t"); 
00348         pc.printf("%f \t",down_degree);
00349         //pc.printf("up\t"); 
00350         pc.printf("%f \n",up_degree);
00351         if(i == 9){
00352             i = 0;
00353         }else{
00354             i = i+1;
00355         }
00356     }
00357 }
00358 
00359 void Avg(){
00360     sum = sum + roll;
00361     avg = sum/10;
00362     standardDeviation = (roll - avg)*(roll-avg); 
00363     sd = sqrt(standardDeviation/10);
00364     if(avg < 0){
00365         avg = avg*(-1);
00366     }
00367 }
00368 
00369 
00370 
00371 void Cal_Cg_linear(){
00372     Si = sd/0.6;
00373     sum = sum - roll_data[i];
00374     if(Si > 0){
00375         Cg = (0.8*Si)+(0.5*Cg);
00376         if(Cg > 1){
00377             Cg = 1;
00378         }
00379     }else{
00380         Cg = 0.00;
00381     }
00382     Cg_down = 1/0.1*exp(2*(Cg-2.081))-0.15;
00383     Cg_up = (0.8*log(Cg+0.4))+0.734;
00384 }