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 Thread thread2;
00011 
00012 void IMU()
00013 {
00014     while(1) {
00015     if (timer1.read_us()  >=1000)// read time in ms
00016         {
00017             attitude_get();
00018             //pc.printf("IMU \n");
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  %.0f \n\r",  roll,   pitch, yaw); 
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 int value;
00039 
00040 void servo_Right();
00041 void move();
00042 void cal_step_down();
00043 void cal_step_up();
00044 void servo();
00045 void getvalue();
00046  
00047 float pos_down_start = 1400.00;
00048 float pos_up_start = 1000.00;
00049 float down_degree = 0.00 ;
00050 float up_degree = 0.00 ; 
00051 float stepmin = 1;
00052 float round = 10;
00053 float waittime = 0.001 ; 
00054  
00055 float pos_down_left = 1400.00;
00056 float pos_up_left = 1000.00;
00057 float pos_down_end_left = (1000.00 + ((700.00/90.00)*(down_degree))); //left down  //90 ใน80 นอก(45)+7 ใน85 นอก+5
00058 float pos_up_end_left = 1000.00 + ((700.00/90.00)*(up_degree)); //left up// 15 , 30+10 , 45-1.75 , 60-5 , 75-5 , 45+5
00059 float state_count_left = 1;
00060 float round_count_left = 1;
00061 float step_down_left;
00062 float step_up_left;
00063  
00064 float pos_down_right = 1400.00;
00065 float pos_up_right = 1000.00;
00066 float pos_down_end_right = (1070.00 + ((700.00/90.00)* (down_degree)))  ; //right down   //99
00067 float pos_up_end_right = 1000.00 + ((700.00/90.00)* (up_degree)); //right up// 15 , 30-10 , 45+1.75 , 60+5 , 75+5 , 45-5
00068 float state_count_right = 1;
00069 float round_count_right = 1;
00070 float step_up_right;
00071 float step_down_right;
00072  
00073 
00074 
00075 int main() {
00076     pc.baud(1000000); 
00077     //pc.printf("malin");
00078     //getvalue();
00079     timer1.start(); // start timer counting
00080     //if (pc.getc() == '1')
00081     //{
00082       pc.printf("ma"); 
00083       thread2.start(servo);
00084       //thread1.start(IMU); 
00085     //}   
00086 }
00087 void cal_step_down(){
00088     pos_down_end_left = (1000.00 + ((700.00/90.00)*(down_degree))); 
00089     pos_down_end_right = (1070.00 + ((700.00/90.00)* (down_degree)))  ; 
00090     if (pos_down_end_right > pos_down_end_left){
00091         step_down_right = (pos_down_end_right - pos_down_start)*stepmin/(pos_down_end_left - pos_down_start);
00092         step_down_left = stepmin;
00093     } else if (pos_down_end_right < pos_down_end_left){
00094         step_down_right = stepmin;
00095         step_down_left = (pos_down_end_left - pos_down_start)*stepmin/(pos_down_end_right - pos_down_start);
00096     } else{
00097         step_down_right = stepmin;
00098         step_down_left = stepmin;
00099     }
00100     pc.printf("pos_down_right");
00101     pc.printf("%f\n",pos_down_end_right);
00102     pc.printf("pos_down_left");
00103     pc.printf("%f\n",pos_down_end_left);
00104     pc.printf("step_down_right");
00105     pc.printf("%f\n",step_down_right);
00106     pc.printf("step_down_left");
00107     pc.printf("%f\n",step_down_left);    
00108 }
00109  
00110 void cal_step_up(){    
00111     pos_up_end_left = 1000.00 + ((700.00/90.00)*(up_degree)); 
00112     pos_up_end_right = 1000.00 + ((700.00/90.00)* (up_degree)); 
00113     if (pos_up_end_right > pos_up_end_left){
00114         step_up_right = (pos_up_end_right - pos_up_start)*stepmin/(pos_up_end_left - pos_up_start);
00115         step_up_left = stepmin;
00116     } else if (pos_up_end_right < pos_up_end_left){
00117         step_up_right = stepmin;
00118         step_up_left = (pos_up_end_left - pos_up_start)*stepmin/(pos_up_end_right - pos_up_start);
00119     } else{
00120         step_up_right = stepmin;
00121         step_up_left = stepmin;
00122     }
00123     pc.printf("pos_up_right");
00124     pc.printf("%f\n",pos_up_end_right);
00125     pc.printf("pos_up_left");
00126     pc.printf("%f\n",pos_up_end_left);
00127     pc.printf("step_up_right");
00128     pc.printf("%f\n",step_up_right);;
00129     pc.printf("step_up_left");
00130     pc.printf("%f\n",step_up_left);  
00131 }
00132  
00133 void move(){
00134     Servo1.Enable(1000,2000);
00135     Servo2.Enable(1000,2000);
00136     Servo3.Enable(1000,2000);
00137     Servo4.Enable(1000,2000);
00138     while(1) {
00139         servo_Right();
00140         if(state_count_left == 1) {
00141             Servo1.SetPosition(pos_down_left);
00142             wait(waittime);
00143             pos_down_left = pos_down_left + step_down_left;
00144             if(pos_down_left >= pos_down_end_left + step_down_left and pos_up_left == pos_up_start) {
00145                 state_count_left = 2;
00146             }
00147             /*pc.printf("LAD");
00148             pc.printf("%f\n",pos_down_left);
00149             pc.printf("LAP");
00150             pc.printf("%f\n",pos_up_left);*/
00151         } else if(state_count_left == 2) {
00152             Servo2.SetPosition(pos_up_left);
00153             wait(waittime);
00154             pos_up_left = pos_up_left + step_up_left;
00155             if(pos_down_left >= pos_down_end_left + step_down_left and pos_up_left >= pos_up_end_left + step_up_left) {
00156                 state_count_left = 3;
00157             }
00158             /*pc.printf("LBD");
00159             pc.printf("%f\n",pos_down_left);
00160             pc.printf("LBP");
00161             pc.printf("%f\n",pos_up_left);*/
00162         } else if(state_count_left == 3) {
00163             Servo1.SetPosition(pos_down_left);
00164             wait(waittime);
00165             pos_down_left = pos_down_left - step_down_left;
00166             if(pos_down_left <= pos_down_start - step_down_left and pos_up_left >= pos_up_end_left + step_up_left) {
00167                 state_count_left = 4;
00168             }
00169             /*pc.printf("LCD");
00170             pc.printf("%f\n",pos_down_left);
00171             pc.printf("LCP");
00172             pc.printf("%f\n",pos_up_left);*/
00173         } else if(state_count_left == 4) {
00174             Servo2.SetPosition(pos_up_left);
00175             wait(waittime);
00176             pos_up_left = pos_up_left - step_up_left;
00177             if(pos_down_left <= pos_down_start - step_down_left and pos_up_left <= pos_up_start - step_up_left) {
00178                 state_count_left = 0;
00179             }
00180             /*pc.printf("LDD");
00181             pc.printf("%f\n",pos_down_left);
00182             pc.printf("LDP");
00183             pc.printf("%f\n",pos_up_left);*/
00184         } else if (state_count_left == 0 and round_count_left < round) {
00185             round_count_left = round_count_left+1;
00186             state_count_left = 1;
00187             pos_down_left = pos_down_start;
00188             pos_up_left = pos_up_start;
00189         } else if (state_count_left == 0 and round_count_left == round and state_count_right == 0 and round_count_right == round){
00190             pc.printf("Finish");
00191             thread1.terminate();
00192             break;         
00193         }
00194     }
00195 }
00196  
00197 void servo_Right()
00198 {
00199     if(state_count_right == 1) {
00200         Servo3.SetPosition(pos_down_right);
00201         wait(waittime);
00202         pos_down_right = pos_down_right + step_down_right;
00203         if(pos_down_right >= pos_down_end_right + step_down_right and pos_up_right == pos_up_start) {
00204             state_count_right = 2;
00205         }
00206         /*pc.printf("RAD");
00207         pc.printf("%f\n",pos_down_right);
00208         pc.printf("RAP");
00209         pc.printf("%f\n",pos_up_right);*/
00210     } else if(state_count_right == 2) {
00211         Servo4.SetPosition(pos_up_right);
00212         wait(waittime);
00213         pos_up_right = pos_up_right + step_up_right;
00214         if(pos_down_right >= pos_down_end_right + step_down_right and pos_up_right >= pos_up_end_right + step_up_right) {
00215             state_count_right = 3;
00216         }
00217         /*pc.printf("RBD");
00218         pc.printf("%f\n",pos_down_right);
00219         pc.printf("RBP");
00220         pc.printf("%f\n",pos_up_right);*/
00221     } else if(state_count_right == 3) {
00222         Servo3.SetPosition(pos_down_right);
00223         wait(waittime);
00224         pos_down_right = pos_down_right - step_down_right;
00225         if(pos_down_right <= pos_down_start - step_down_right and pos_up_right >= pos_up_end_right + step_up_right) {
00226             state_count_right = 4;
00227         }
00228         /*pc.printf("RCD");
00229         pc.printf("%f\n",pos_down_right);
00230         pc.printf("RCP");
00231         pc.printf("%f\n",pos_up_right);*/
00232     } else if(state_count_right == 4) {
00233         Servo4.SetPosition(pos_up_right);
00234         wait(waittime);
00235         pos_up_right = pos_up_right - step_up_right;
00236         if(pos_down_right <= pos_down_start - step_down_right and pos_up_right <= pos_up_start - step_up_right) {
00237             state_count_right = 0;
00238         }
00239         /*pc.printf("RDD");
00240         pc.printf("%f\n",pos_down_right);
00241         pc.printf("RDP");
00242         pc.printf("%f\n",pos_up_right);*/
00243     } else if (state_count_right == 0 and round_count_right < round) {
00244         round_count_right = round_count_right+1;
00245         state_count_right = 1;
00246         pos_down_right = pos_down_start;
00247         pos_up_right = pos_up_start;
00248     }
00249 }
00250             
00251     
00252 void servo() {
00253     attitude_setup();
00254     getvalue();
00255     pc.printf("start\n"); 
00256     //pc.printf("%f \n",down_degree );
00257     //pc.printf("%f \n",up_degree );
00258     cal_step_down();
00259     cal_step_up();
00260     //pc.printf("%f \n",down_degree );
00261     //pc.printf("%f \n",up_degree );
00262     move();
00263 }
00264 
00265 void getvalue() {
00266     pc.printf("case 1 = 90-15 \n");
00267     pc.printf("case 2 = 90-30 \n");
00268     pc.printf("case 3 = 90-45 \n");
00269     pc.printf("case 4 = 90-60 \n");
00270     pc.printf("case 5 = 80-45 \n");
00271     pc.printf("case 6 = 85-45 \n");
00272     pc.printf("case 7 = 95-45 \n");
00273     pc.printf("case 8 = 100-45 \n");
00274         value = pc.getc() ; 
00275         switch (value) {
00276          case '1': { 
00277             down_degree = 90.00 ;
00278             up_degree = 15.00 ; 
00279             break;
00280             }
00281         case '2': {
00282             down_degree = 90.00 ;
00283             up_degree = 30.00 ; 
00284             break;
00285             }
00286         case '3': {
00287             down_degree = 90.00 ;
00288             up_degree = 45.00 ; 
00289             break;
00290             }
00291         case '4': {
00292             down_degree = 90.00 ;
00293             up_degree = 60.00 ; 
00294             break;
00295             }
00296         case '5': {
00297             down_degree = 80.00 ;
00298             up_degree = 45.00 ; 
00299             break;
00300             }
00301         case '6': {
00302             down_degree = 85.00 ;
00303             up_degree = 45.00 ; 
00304             break;
00305             }
00306         case '7': {
00307             down_degree = 95.00 ;
00308             up_degree = 45.00 ; 
00309             break;
00310             }
00311         case '8': {
00312             down_degree = 100.00 ;
00313             up_degree = 45.00 ; 
00314             break;
00315             }
00316         break;
00317 }
00318     //pc.printf("%f \n",down_degree );
00319     //pc.printf("%f \n",up_degree );
00320     
00321     }