main.cpp

00001 #include "mbed.h"
00002 #include "Servo.h"
00003 #include "rtos.h"
00004 #include "attitude.h"
00005 
00006 Serial pc(USBTX, USBRX);
00007 //Serial bt(A7,A2);
00008 Timer timer1; 
00009 Thread thread1;         
00010 
00011 Thread thread2;
00012 
00013 void IMU()
00014 {
00015     while(1) {
00016     if (timer1.read_us()  >=1000)// read time in ms
00017         {
00018             attitude_get();
00019             pc.printf(" %f \t", ax*10 ); 
00020             pc.printf("  %f \t", ay*10 ); 
00021             pc.printf("  %f \t", az*10 - 10); //cm/s*s
00022 
00023             pc.printf(" %f \t", gx ); 
00024             pc.printf("  %f \t", gy );  
00025             pc.printf("  %f  \t", gz );  //deg/s    */
00026             pc.printf("%.0f\t  %.0f \t   \n\r",  roll,   pitch ); 
00027 
00028             timer1.reset(); // reset timer 
00029             }
00030             }
00031 }
00032 
00033 Servo Servo1(D6);
00034 Servo Servo2(D8);
00035 Servo Servo3(D9);
00036 Servo Servo4(D10);
00037 
00038 void servoleft();
00039 void move();
00040 
00041 int pos_down = 1400;
00042 int pos_up = 1000;
00043 int pos_down_end = 1700;
00044 int pos_up_end = 1500;  
00045 int edit_up = 1400;
00046                                                                                                                    
00047 int state_count = 1;
00048 int state_count2 = 1;
00049 int round_count = 1;
00050 int round_count2 = 1;
00051 int main() {
00052     pc.baud(1000000); 
00053     //pc.printf("malin");
00054     
00055     timer1.start(); // start timer counting
00056     if (pc.getc() == '1')
00057     {
00058         thread2.start(move);
00059         //thread1.start(IMU); 
00060     }
00061     
00062 }
00063 void servoleft(){
00064         
00065         if(state_count2 == 1){
00066             Servo1.SetPosition(pos_down);
00067             //pos_down = pos_down+5;
00068             wait(0.01);
00069             pc.printf("left %d\n",pos_down);
00070             if(pos_down == pos_down_end+5 and pos_up == 1000){
00071                 state_count2 = 2;
00072             }
00073         }
00074         else if(state_count2 == 2){
00075             Servo2.SetPosition(pos_up);
00076             //pos_up = pos_up+5;
00077             wait(0.01);
00078             pc.printf("stage2");
00079             if(pos_down == pos_down_end+5 and pos_up == edit_up+5){
00080                 state_count2 = 3;
00081             }
00082         }
00083         else if(state_count2 == 3){
00084             Servo1.SetPosition(pos_down);
00085             //pos_down = pos_down-5;
00086             wait(0.01);
00087             pc.printf("stage3");
00088             if(pos_down == 1395 and pos_up == edit_up+5){
00089                 state_count2 = 4;
00090             }
00091         }
00092         else if(state_count2 == 4){
00093             Servo2.SetPosition(pos_up);
00094             //pos_up = pos_up-5;
00095             wait(0.01); 
00096             pc.printf("stage4");
00097             if(pos_down == 1395 and pos_up == 995){
00098                 state_count2 = 0;
00099             }       
00100         }
00101          else if (state_count2 == 0 and round_count2 < 10){
00102             round_count2 = round_count2+1;
00103             state_count2 = 1;
00104             pos_down = 1400;
00105             pos_up = 1000;
00106         }          
00107          else {
00108              pc.printf("1stop"); 
00109              //thread1.stop(IMU);
00110              } 
00111     
00112 
00113     }
00114 void move() {
00115     attitude_setup();
00116     Servo1.Enable(1000,2000);
00117     Servo2.Enable(1000,2000);
00118     Servo3.Enable(1000,2000);
00119     Servo4.Enable(1000,2000);
00120     //pc.printf("start");
00121     //pc.printf("start");
00122     //pc.printf("start");
00123     //pc.printf("start");
00124     pc.printf("start");
00125     while(1){
00126     servoleft();        
00127         if(state_count == 1){
00128             Servo3.SetPosition(pos_down);
00129             pos_down = pos_down+5;
00130             wait(0.01);
00131             pc.printf("right %d\n",pos_down);
00132             if(pos_down == pos_down_end+5 and pos_up == 1000){
00133                 state_count = 2;
00134             }
00135         }
00136         else if(state_count == 2){
00137             Servo4.SetPosition(pos_up);
00138             pos_up = pos_up+5;
00139             wait(0.01);
00140             if(pos_down == pos_down_end+5 and pos_up == pos_up_end+5){
00141                 state_count = 3;
00142             }
00143         }
00144         else if(state_count == 3){
00145             Servo3.SetPosition(pos_down);
00146             pos_down = pos_down-5;
00147             wait(0.01);
00148             if(pos_down == 1395 and pos_up == pos_up_end+5){
00149                 state_count = 4;
00150             }
00151         }
00152         else if(state_count == 4){
00153             Servo4.SetPosition(pos_up);
00154             pos_up = pos_up-5;
00155             wait(0.01); 
00156             if(pos_down == 1395 and pos_up == 995){
00157                 state_count = 0;
00158             }       
00159         }
00160          else if (state_count == 0 and round_count < 10){
00161             round_count = round_count+1;
00162             state_count = 1;
00163             pos_down = 1400;
00164             pos_up = 1000;
00165         }          
00166          else {
00167              pc.printf("stop"); 
00168              } 
00169     }
00170 }