mian.cpp

00001 #include "mbed.h"
00002 
00003 PwmOut servo_x(D5);
00004 PwmOut servo_y(D6);
00005 DigitalOut led1(D13);
00006 
00007 Serial python(D8, D2, 9600);
00008 Serial pc(USBTX, USBRX, 9600);
00009 
00010 int MIN = 510;
00011 int MAX = 2350;
00012 int MID = (MAX + MIN)/2;
00013 int OFFSET = 300;
00014 
00015 int STEP = 5;
00016 int CSTEP_x = MID; 
00017 int CSTEP_y = MID;    
00018   
00019 int state = 0;
00020 char rx_line;
00021 
00022 
00023 void Rx_interrupt();  
00024 void calibrate();
00025 void move_x(int dir, int max, int min, int step);
00026 void move_y(int dir, int max, int min, int step);
00027 
00028 
00029 
00030 int main() {
00031     
00032     // toda vez q chegar(Rx) info pela serial, execura a funcao interupt
00033     python.attach(&Rx_interrupt, Serial::RxIrq);
00034     
00035     servo_x.period_us(20000); //20ms period, typical for analog RC servo
00036     servo_y.period_us(20000);
00037  
00038     calibrate();
00039  
00040     while(1){
00041 
00042         if(state == 1){
00043             pc.printf("state = 1\n");
00044             wait(0.1);
00045             while(state == 1){
00046                 if(CSTEP_x < (MAX-OFFSET)){
00047                     servo_x.pulsewidth_us(CSTEP_x + STEP);
00048                     CSTEP_x = CSTEP_x + STEP;
00049                     wait(0.01);
00050                     }
00051                 }
00052         }
00053         if(state == 2){
00054             pc.printf("state = 2\n");
00055             wait(0.1);
00056             while(state == 2){
00057                 if(CSTEP_x > (MIN+OFFSET)){
00058                     servo_x.pulsewidth_us(CSTEP_x - STEP);
00059                     CSTEP_x = CSTEP_x - STEP;
00060                     wait(0.01);
00061                     }
00062                 }
00063         }
00064         if(state == 3){
00065             pc.printf("state = 3\n");
00066             wait(0.1);
00067             while(state == 3){
00068                 if(CSTEP_y < (MAX-OFFSET)){
00069                 servo_y.pulsewidth_us(CSTEP_y + STEP);
00070                 CSTEP_y = CSTEP_y + STEP;
00071                 wait(0.01);
00072                 }
00073             }
00074         }
00075         if(state == 4){
00076             pc.printf("state = 4\n");
00077             wait(0.1);
00078             while(state == 4){
00079                 if(CSTEP_y > (MIN+OFFSET)){
00080                     servo_y.pulsewidth_us(CSTEP_y - STEP);
00081                     CSTEP_y = CSTEP_y - STEP;
00082                     wait(0.01);
00083                     }
00084                 }
00085         }
00086         if(state == 5){
00087             //pc.printf("state = 5\n");
00088             wait(1);
00089             //break;
00090         }
00091         
00092     }
00093     
00094 
00095 }
00096 
00097 
00098 
00099 
00100 
00101 void move_x(int dir, int max, int min, int step){ 
00102 
00103     if(dir == 0){   
00104     pc.printf("dir x = 0\n");
00105     char txt[10];
00106     sprintf(txt,"CSTEP_x=%d",CSTEP_x); 
00107     pc.printf(txt);
00108     pc.printf("\n");
00109         for(int pw_x=CSTEP_x; pw_x<max; pw_x+=step){
00110             servo_x.pulsewidth_us(pw_x);
00111             CSTEP_x = pw_x;
00112             wait(0.01);
00113             }
00114         }
00115         
00116     else if(dir == 1){
00117     pc.printf("dir x = 1\n");
00118     char txt[10];
00119     sprintf(txt,"CSTEP_x=%d",CSTEP_x); 
00120     pc.printf(txt);
00121     pc.printf("\n");
00122         for(int pw_x=CSTEP_x; pw_x>min; pw_x-=step){
00123             servo_x.pulsewidth_us(pw_x);
00124             CSTEP_x = pw_x;
00125             wait(0.01);
00126             } 
00127         }
00128     }
00129 
00130 
00131 
00132 
00133     
00134     
00135     
00136 void calibrate(){
00137     servo_x.pulsewidth_us(MID);
00138     wait(0.5);
00139     servo_x.pulsewidth_us(MID + 460);
00140     wait(0.5);
00141     servo_x.pulsewidth_us(MID - 460);
00142     wait(0.5);
00143     servo_x.pulsewidth_us(MID);
00144     wait(0.5);
00145     
00146     servo_y.pulsewidth_us(MID);
00147     wait(0.5);
00148     servo_y.pulsewidth_us(MID + 460);
00149     wait(0.5);
00150     servo_y.pulsewidth_us(MID - 460);
00151     wait(0.5);
00152     servo_y.pulsewidth_us(MID);
00153     wait(0.5);
00154     
00155     
00156     
00157     }
00158 
00159 
00160 
00161 
00162 void Rx_interrupt() // funcao que recebe os outputs do script python 
00163 {
00164     led1=1;
00165     while(python.readable())
00166     
00167 
00168     // Recebe o char do buffer usado pelo dispositivo "python"
00169     rx_line = python.getc();
00170     
00171     switch(rx_line) 
00172     {
00173 
00174         case '1':
00175             state = 1;
00176             rx_line = 0x00;
00177             break;
00178 
00179         case '2':
00180             state = 2;
00181             rx_line = 0x00;
00182             break;
00183             
00184         case '3':
00185             state = 3;
00186             rx_line = 0x00;
00187             break;
00188             
00189         case '4':
00190             state = 4;
00191             rx_line = 0x00;
00192             break;
00193             
00194         case '5':
00195             state = 5;
00196             rx_line = 0x00;
00197             break;
00198             
00199             
00200             
00201             default: rx_line=0;
00202     led1=0;
00203     }
00204     return;
00205 }