main.cpp

00001 #include "mbed.h"
00002 #include "Servo.h"
00003 #include "rtos.h"
00004 
00005 Serial pc(USBTX, USBRX);
00006 
00007 Thread thread;
00008 
00009 Servo Servo1(D6);
00010 Servo Servo2(D8);
00011 Servo Servo3(D9);
00012 Servo Servo4(D10);
00013 
00014 void servo_Right();
00015 void move();
00016 void cal_step_down();
00017 void cal_step_up();
00018 
00019 float pos_down_start = 1400;
00020 float pos_up_start = 1000;
00021 float stepmin = 5;
00022 float round = 5;
00023 
00024 float pos_down_left = 1400;
00025 float pos_up_left = 1000;
00026 float pos_down_end_left = 1700; //left down
00027 float pos_up_end_left = 1350; //left up
00028 float state_count_left = 1;
00029 float round_count_left = 1;
00030 float step_down_left;
00031 float step_up_left;
00032 
00033 float pos_down_right = 1400;
00034 float pos_up_right = 1000;
00035 float pos_down_end_right = 1650; //right down
00036 float pos_up_end_right = 1700; //right up
00037 float state_count_right = 1;
00038 float round_count_right = 1;
00039 float step_up_right;
00040 float step_down_right;
00041 
00042 int main()
00043 {
00044     wait(5);
00045     cal_step_down();
00046     cal_step_up();
00047     move();
00048 }
00049 
00050 void cal_step_down(){
00051     if (pos_down_end_right > pos_down_end_left){
00052         step_down_right = (pos_down_end_right - pos_down_start)*stepmin/(pos_down_end_left - pos_down_start);
00053         step_down_left = stepmin;
00054     } else if (pos_down_end_right < pos_down_end_left){
00055         step_down_right = stepmin;
00056         step_down_left = (pos_down_end_left - pos_down_start)*stepmin/(pos_down_end_right - pos_down_start);
00057     } else{
00058         step_down_right = stepmin;
00059         step_down_left = stepmin;
00060     }
00061     /*pc.printf("pos_down_right");
00062     pc.printf("%f\n",pos_down_end_right);
00063     pc.printf("pos_down_left");
00064     pc.printf("%f\n",pos_down_end_left);
00065     pc.printf("step_down_right");
00066     pc.printf("%f\n",step_down_right);
00067     pc.printf("step_down_left");
00068     pc.printf("%f\n",step_down_left);*/
00069 }
00070 
00071 void cal_step_up(){    
00072     if (pos_up_end_right > pos_up_end_left){
00073         step_up_right = (pos_up_end_right - pos_up_start)*stepmin/(pos_up_end_left - pos_up_start);
00074         step_up_left = stepmin;
00075     } else if (pos_up_end_right < pos_up_end_left){
00076         step_up_right = stepmin;
00077         step_up_left = (pos_up_end_left - pos_up_start)*stepmin/(pos_up_end_right - pos_up_start);
00078     } else{
00079         step_up_right = stepmin;
00080         step_up_left = stepmin;
00081     }
00082     /*pc.printf("pos_up_right");
00083     pc.printf("%f\n",pos_up_end_right);
00084     pc.printf("pos_up_left");
00085     pc.printf("%f\n",pos_up_end_left);
00086     pc.printf("step_up_right");
00087     pc.printf("%f\n",step_up_right);;
00088     pc.printf("step_up_left");
00089     pc.printf("%f\n",step_up_left);*/
00090 }
00091 
00092 void move(){
00093     pc.baud(9600);
00094     Servo1.Enable(1000,2000);
00095     Servo2.Enable(1000,2000);
00096     Servo3.Enable(1000,2000);
00097     Servo4.Enable(1000,2000);
00098     pc.printf("Start\n");
00099     while(1) {
00100         servo_Right();
00101         if(state_count_left == 1) {
00102             Servo1.SetPosition(pos_down_left);
00103             wait(0.005);
00104             pos_down_left = pos_down_left + step_down_left;
00105             if(pos_down_left >= pos_down_end_left + step_down_left and pos_up_left == pos_up_start) {
00106                 state_count_left = 2;
00107             }
00108             /*pc.printf("LAD");
00109             pc.printf("%f\n",pos_down_left);
00110             pc.printf("LAP");
00111             pc.printf("%f\n",pos_up_left);*/
00112         } else if(state_count_left == 2) {
00113             Servo2.SetPosition(pos_up_left);
00114             wait(0.005);
00115             pos_up_left = pos_up_left + step_up_left;
00116             if(pos_down_left >= pos_down_end_left + step_down_left and pos_up_left >= pos_up_end_left + step_up_left) {
00117                 state_count_left = 3;
00118             }
00119             /*pc.printf("LBD");
00120             pc.printf("%f\n",pos_down_left);
00121             pc.printf("LBP");
00122             pc.printf("%f\n",pos_up_left);*/
00123         } else if(state_count_left == 3) {
00124             Servo1.SetPosition(pos_down_left);
00125             wait(0.005);
00126             pos_down_left = pos_down_left - step_down_left;
00127             if(pos_down_left <= pos_down_start - step_down_left and pos_up_left >= pos_up_end_left + step_up_left) {
00128                 state_count_left = 4;
00129             }
00130             /*pc.printf("LCD");
00131             pc.printf("%f\n",pos_down_left);
00132             pc.printf("LCP");
00133             pc.printf("%f\n",pos_up_left);*/
00134         } else if(state_count_left == 4) {
00135             Servo2.SetPosition(pos_up_left);
00136             wait(0.005);
00137             pos_up_left = pos_up_left - step_up_left;
00138             if(pos_down_left <= pos_down_start - step_down_left and pos_up_left <= pos_up_start - step_up_left) {
00139                 state_count_left = 0;
00140             }
00141             /*pc.printf("LDD");
00142             pc.printf("%f\n",pos_down_left);
00143             pc.printf("LDP");
00144             pc.printf("%f\n",pos_up_left);*/
00145         } else if (state_count_left == 0 and round_count_left < round) {
00146             round_count_left = round_count_left+1;
00147             state_count_left = 1;
00148             pos_down_left = pos_down_start;
00149             pos_up_left = pos_up_start;
00150         } else if (state_count_left == 0 and round_count_left == round and state_count_right == 0 and round_count_right < round){
00151             pc.printf("Finish");
00152             break;        
00153         }
00154     }
00155 }
00156 
00157 void servo_Right()
00158 {
00159     if(state_count_right == 1) {
00160         Servo3.SetPosition(pos_down_right);
00161         wait(0.005);
00162         pos_down_right = pos_down_right + step_down_right;
00163         if(pos_down_right >= pos_down_end_right + step_down_right and pos_up_right == pos_up_start) {
00164             state_count_right = 2;
00165         }
00166         /*pc.printf("RAD");
00167         pc.printf("%f\n",pos_down_right);
00168         pc.printf("RAP");
00169         pc.printf("%f\n",pos_up_right);*/
00170     } else if(state_count_right == 2) {
00171         Servo4.SetPosition(pos_up_right);
00172         wait(0.005);
00173         pos_up_right = pos_up_right + step_up_right;
00174         if(pos_down_right >= pos_down_end_right + step_down_right and pos_up_right >= pos_up_end_right + step_up_right) {
00175             state_count_right = 3;
00176         }
00177         /*pc.printf("RBD");
00178         pc.printf("%f\n",pos_down_right);
00179         pc.printf("RBP");
00180         pc.printf("%f\n",pos_up_right);*/
00181     } else if(state_count_right == 3) {
00182         Servo3.SetPosition(pos_down_right);
00183         wait(0.005);
00184         pos_down_right = pos_down_right - step_down_right;
00185         if(pos_down_right <= pos_down_start - step_down_right and pos_up_right >= pos_up_end_right + step_up_right) {
00186             state_count_right = 4;
00187         }
00188         /*pc.printf("RCD");
00189         pc.printf("%f\n",pos_down_right);
00190         pc.printf("RCP");
00191         pc.printf("%f\n",pos_up_right);*/
00192     } else if(state_count_right == 4) {
00193         Servo4.SetPosition(pos_up_right);
00194         wait(0.005);
00195         pos_up_right = pos_up_right - step_up_right;
00196         if(pos_down_right <= pos_down_start - step_down_right and pos_up_right <= pos_up_start - step_up_right) {
00197             state_count_right = 0;
00198         }
00199         /*pc.printf("RDD");
00200         pc.printf("%f\n",pos_down_right);
00201         pc.printf("RDP");
00202         pc.printf("%f\n",pos_up_right);*/
00203     } else if (state_count_right == 0 and round_count_right < round) {
00204         round_count_right = round_count_right+1;
00205         state_count_right = 1;
00206         pos_down_right = pos_down_start;
00207         pos_up_right = pos_up_start;
00208     }
00209 }