Diff: mian.cpp

Revision:

0:e687ed46d0dc

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mian.cpp	Tue Aug 02 17:27:08 2022 +0000
@@ -0,0 +1,205 @@
+#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;
+}