Diff: main.cpp

Revision:

0:b63eda8e8952

Child:

1:a88edf25d893

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Mar 29 12:15:58 2017 +0000
@@ -0,0 +1,128 @@
+#include "mbed.h"
+#include "robot.h" // Inicializa o robô. Este include deverá ser usado em todos os main.cpp!
+#include "Pixy.h"
+
+Pixy pixy(Pixy::I2C,PTC11,PTC10);
+Timer t;
+
+int main() {
+    initRobot();
+    pixy.setAddress(0x21); //Camera Address 0x10
+    
+    
+    int Speed = 0;
+    int aux_Speed = 0;
+    int Coef = 0;
+    int aux_Coef = 0;
+    int left_speed = 0;
+    int left_Dir = 1;
+    int right_speed = 0;
+    int right_Dir = 1;
+    
+    int period = 50; //each loop takes 50ms
+    //////////width PI controller
+    int w_setpoint = 100;
+    float w_Kp = 15;
+    float w_Ki = 5; 
+    int w_e = 0;
+    int w_e2 = 0;
+    
+        //////////x PI controller
+    int x_setpoint = 160;
+    float x_Kp = 1;
+    float x_Ki = 0.1; 
+    int x_e = 0;
+    int x_e2 = 0;
+    
+        
+    q_led_red_fro = 1;  //Led Red Front
+    pwm_led_whi.pulsewidth_us(1000); //Led White Front
+    
+
+    uint16_t Block_width = 0,Block_x = 0;
+    uint16_t blocks;
+
+    t.start();
+    while(1) {        
+        if(q_led_red_fro == 1){
+            q_led_red_fro = 0;
+        }else{
+            q_led_red_fro = 1;
+        }
+                
+        blocks = 0;
+        blocks = (uint16_t) pixy.getBlocks();
+        
+        if (blocks > 0){
+            Block_width = pixy.blocks[0].width;
+            Block_x = pixy.blocks[0].x;
+        }
+        
+        w_e2 = (w_setpoint - Block_width) - w_e;
+        w_e = w_setpoint - Block_width;
+        Speed = Speed+ int(w_Kp*w_e2) + int ((w_Ki*w_e*period)/1000);
+        aux_Speed = Speed;
+        if (abs(Speed) >666)
+        { 
+            Speed = (int) 666 *Speed/abs(Speed) ;
+            //aux_Speed = (short int) 666 *Speed/abs(Speed) ;
+
+        }//else if (abs(Speed) <150)
+        else if (abs(w_e) <25)
+        {
+            Speed = 0;
+            //aux_Speed = 0;
+        }
+        
+        x_e2 = (x_setpoint - Block_x) - x_e;
+        x_e = x_setpoint - Block_x;
+        Coef = Coef+ int(x_Kp*x_e2) + int ((x_Ki*x_e*period)/1000);
+        aux_Coef = Coef;
+        if (abs(Coef) >334)
+        { 
+            Coef =(int) 334*Coef/abs(Coef);
+            //aux_Coef =(short int) 334*Coef/abs(Coef);
+        }//else if (abs(Coef) < 20 )
+        else if (abs(x_e) < 30 )
+        {
+            Coef = 0;
+            //aux_Coef = 0;
+        }
+        
+        
+        left_speed = Speed - Coef;
+        //left_speed = aux_Speed - aux_Coef;
+        if (left_speed >0)
+        {
+            left_Dir=1;
+            left_speed = abs(left_speed);
+        }else{
+            left_Dir=0;
+            left_speed = abs(left_speed);
+        }
+        
+        right_speed = Speed + Coef;        
+        //right_speed = aux_Speed + aux_Coef;
+        if (right_speed >0)
+        {
+            right_Dir=1;
+            right_speed = abs(right_speed);
+        }else{
+            right_Dir=0;
+            right_speed = abs(right_speed);
+        }
+        
+        leftMotor(left_Dir,left_speed);
+        rightMotor(right_Dir,right_speed);
+        while (t.read_ms () < period )
+        { 
+            //waste time
+        }
+        //pc.printf("Speed=%d , width=%d , w_e2=%d, w_e=%d \n",Speed , Block_width , w_e2 , w_e  );
+        //pc.printf("Coef=%d , x=%d, x_e2=%d, x_e=%d  \n",Coef, Block_x , x_e2 , x_e );
+        pc.printf("Coef=%d , x=%d, x_e2=%d, x_e=%d :: Speed=%d ,leftMotor=%d , rightMotor=%d \n",Coef, Block_x , x_e2 , x_e , Speed , left_speed , right_speed);
+
+    
+        t.reset();
+    }
+}
\ No newline at end of file