File content as of revision 0:e687ed46d0dc:

#include "mbed.h"

PwmOut servo_x(D5);
PwmOut servo_y(D6);
DigitalOut led1(D13);

Serial python(D8, D2, 9600);
Serial pc(USBTX, USBRX, 9600);

int MIN = 510;
int MAX = 2350;
int MID = (MAX + MIN)/2;
int OFFSET = 300;

int STEP = 5;
int CSTEP_x = MID; 
int CSTEP_y = MID;    
  
int state = 0;
char rx_line;


void Rx_interrupt();  
void calibrate();
void move_x(int dir, int max, int min, int step);
void move_y(int dir, int max, int min, int step);



int main() {
    
    // toda vez q chegar(Rx) info pela serial, execura a funcao interupt
    python.attach(&Rx_interrupt, Serial::RxIrq);
    
    servo_x.period_us(20000); //20ms period, typical for analog RC servo
    servo_y.period_us(20000);
 
    calibrate();
 
    while(1){

        if(state == 1){
            pc.printf("state = 1\n");
            wait(0.1);
            while(state == 1){
                if(CSTEP_x < (MAX-OFFSET)){
                    servo_x.pulsewidth_us(CSTEP_x + STEP);
                    CSTEP_x = CSTEP_x + STEP;
                    wait(0.01);
                    }
                }
        }
        if(state == 2){
            pc.printf("state = 2\n");
            wait(0.1);
            while(state == 2){
                if(CSTEP_x > (MIN+OFFSET)){
                    servo_x.pulsewidth_us(CSTEP_x - STEP);
                    CSTEP_x = CSTEP_x - STEP;
                    wait(0.01);
                    }
                }
        }
        if(state == 3){
            pc.printf("state = 3\n");
            wait(0.1);
            while(state == 3){
                if(CSTEP_y < (MAX-OFFSET)){
                servo_y.pulsewidth_us(CSTEP_y + STEP);
                CSTEP_y = CSTEP_y + STEP;
                wait(0.01);
                }
            }
        }
        if(state == 4){
            pc.printf("state = 4\n");
            wait(0.1);
            while(state == 4){
                if(CSTEP_y > (MIN+OFFSET)){
                    servo_y.pulsewidth_us(CSTEP_y - STEP);
                    CSTEP_y = CSTEP_y - STEP;
                    wait(0.01);
                    }
                }
        }
        if(state == 5){
            //pc.printf("state = 5\n");
            wait(1);
            //break;
        }
        
    }
    

}





void move_x(int dir, int max, int min, int step){ 

    if(dir == 0){   
    pc.printf("dir x = 0\n");
    char txt[10];
    sprintf(txt,"CSTEP_x=%d",CSTEP_x); 
    pc.printf(txt);
    pc.printf("\n");
        for(int pw_x=CSTEP_x; pw_x<max; pw_x+=step){
            servo_x.pulsewidth_us(pw_x);
            CSTEP_x = pw_x;
            wait(0.01);
            }
        }
        
    else if(dir == 1){
    pc.printf("dir x = 1\n");
    char txt[10];
    sprintf(txt,"CSTEP_x=%d",CSTEP_x); 
    pc.printf(txt);
    pc.printf("\n");
        for(int pw_x=CSTEP_x; pw_x>min; pw_x-=step){
            servo_x.pulsewidth_us(pw_x);
            CSTEP_x = pw_x;
            wait(0.01);
            } 
        }
    }




    
    
    
void calibrate(){
    servo_x.pulsewidth_us(MID);
    wait(0.5);
    servo_x.pulsewidth_us(MID + 460);
    wait(0.5);
    servo_x.pulsewidth_us(MID - 460);
    wait(0.5);
    servo_x.pulsewidth_us(MID);
    wait(0.5);
    
    servo_y.pulsewidth_us(MID);
    wait(0.5);
    servo_y.pulsewidth_us(MID + 460);
    wait(0.5);
    servo_y.pulsewidth_us(MID - 460);
    wait(0.5);
    servo_y.pulsewidth_us(MID);
    wait(0.5);
    
    
    
    }




void Rx_interrupt() // funcao que recebe os outputs do script python 
{
    led1=1;
    while(python.readable())
    

    // Recebe o char do buffer usado pelo dispositivo "python"
    rx_line = python.getc();
    
    switch(rx_line) 
    {

        case '1':
            state = 1;
            rx_line = 0x00;
            break;

        case '2':
            state = 2;
            rx_line = 0x00;
            break;
            
        case '3':
            state = 3;
            rx_line = 0x00;
            break;
            
        case '4':
            state = 4;
            rx_line = 0x00;
            break;
            
        case '5':
            state = 5;
            rx_line = 0x00;
            break;
            
            
            
            default: rx_line=0;
    led1=0;
    }
    return;
}