Diff: main.cpp

Revision:

5:b0af0cfb678e

Parent:

3:e213c44a9f6c

Child:

12:273752f540be

--- a/main.cpp	Mon Apr 11 21:24:52 2016 +0000
+++ b/main.cpp	Sun Apr 17 00:58:06 2016 +0000
@@ -7,15 +7,18 @@
 
 #define PI 3.141592653589793238462
 #define Ts 0.02                         // Seconds
+#define PWM_PERIOD 13.5 // ms
 #define INITIAL_P 0.452531214933414
 #define INITIAL_I 5.45748932024049
 #define INITIAL_D 0.000233453623255507
 #define INITIAL_N 51.0605584484153
-#define BRAKE_CONSTANT 30
+#define BRAKE_CONSTANT 40
+#define BRAKE_WAIT 0.3
 #define GYRO_OFFSET 0.0152
 #define END_THRESH 4
 #define START_THRESH 10
 #define MINIMUM_VELOCITY 15
+
 Serial ser(USBTX, USBRX);    // Initialize Serial port
 PwmOut motor(PTD1);         // Motor connected to pin PTD1
 PwmOut servo(PTD3);         // Servo connected to pin PTD3
@@ -45,39 +48,66 @@
 void magCal();
 
 // State variables
-float sensor, velocity;
+float feedback, velocity = 0;
 void readProtocol();
 void brakeMotor();
+void reverseMotor(int speed);
+void setVelocity(int new_velocity);
+
 // Test functions
 void debug();
 
 int main(){
-    // Initializing serial communication
-    ser.baud(9600);
-    ser.format(8, SerialBase::None, 1);
-    // Initializing controller
-    printf("Initializing controller....\r\n\r\n");
-    initializeController();
-    printf("Controller Initialized. \r\n");
-    // Calibrating magnetometer and setting the initial position
-    magCal();
-    gyro_angle = processMagAngle();
-    // Start moving the robot and integrating the gyroscope
-    velocity = MINIMUM_VELOCITY;
-    setMotorPWM(velocity,motor);
-    startGyro();
-    // main loop
-        while (true){
-        processGyroAngle();
-        controlAnglePID(P,I,D,N);
-        //debug();
-        if(t.read_us() < Ts*1000000)
-            wait_us(Ts*1000000 - t.read_us());
-        if(ser.readable())
-            readProtocol();
-        }
+    int teste = 0;
+    setVelocity(0);
+    switch(teste){
+    case 0: // vai para tras duas vezes
+        brakeMotor();
+        wait(1);
+        setVelocity(-1);
+        setVelocity(0);
+        setVelocity(-30);
+        wait(2);
+        brakeMotor();
+        wait(2);
+        setVelocity(-1);
+        setVelocity(0);
+        setVelocity(-30);
+        break;
+    case 1: // vai para a frente e depois para tras
+        brakeMotor();
+        wait(1);
+        setVelocity(20);
+        wait(2);
+        brakeMotor();
+        wait(2);
+        setVelocity(-1);
+        setVelocity(0);
+        setVelocity(-30);
+        break;
+    case 2: // vai para tras e depois para frente
+        brakeMotor();
+        wait(1);
+        setVelocity(-1);
+        setVelocity(0);
+        setVelocity(-30);
+        wait(2);
+        brakeMotor();
+        wait(2);
+        setVelocity(20);
+        break;
+    case 3: // vai para frente duas vezes
+        brakeMotor();
+        wait(1);
+        setVelocity(20);
+        wait(2);
+        brakeMotor();
+        wait(2);
+        setVelocity(20);
+        break;
+    }
+    while(1);
 }
-
 void readProtocol(){
     char msg = ser.getc();
     switch(msg)
@@ -146,18 +176,18 @@
     gyro_angle = gyro_angle + (gyro_data[2] + GYRO_OFFSET)*(double)(t.read_us())/1000000;
     t.reset();
     t.start();
-    sensor = gyro_angle;
-    if(sensor > 180)
-        sensor = sensor - 360;
-    if(sensor < -180)
-        sensor = sensor + 360;
+    feedback = gyro_angle;
+    if(feedback > 180)
+        feedback = feedback - 360;
+    if(feedback < -180)
+        feedback = feedback + 360;
 }
 
 /* PID controller for angular position */
 void controlAnglePID(float P, float I, float D, float N){ 
     /* Getting error */
     e[1] = e[0];
-    e[0] = reference - (sensor*PI/180);
+    e[0] = reference - (feedback*PI/180);
     if(e[0] > PI)
       e[0]= e[0] - 2*PI;
     if(e[0] < -PI)
@@ -182,19 +212,52 @@
 }
 /* Brake function, braking while the gyroscope is still integrating will cause considerably error in the measurement. */
 void brakeMotor(){
-    if(velocity > 0)
+    if(velocity >= 0){
         setMotorPWM(-BRAKE_CONSTANT, motor);
-    else if(velocity < 0)
-        setMotorPWM(BRAKE_CONSTANT, motor);
-    wait(0.5);
-    setMotorPWM(0,motor);
+        wait(BRAKE_WAIT);
+        velocity = 0;
+        setMotorPWM(velocity,motor);
+    }
+    else {
+        setVelocity(0);
+    }
+}
+void reverseMotor(int speed){
+    for(int i=0 ; i >= -speed; i--){
+        setMotorPWM((float)i,motor);
+        wait_ms(13.5);    
+    }
+    for(int i=-speed ; i <= 0; i++){
+        setMotorPWM((float)i,motor);
+        wait_ms(13.5);    
+    }
+    for(int i=0 ; i >= -speed; i--){
+        setMotorPWM((float)i,motor);
+        wait_ms(13.5);    
+    }
+}
+void setVelocity(int new_velocity){
+    if( velocity > new_velocity){
+        for(; velocity >= new_velocity; velocity--){
+            setMotorPWM(velocity,motor);
+            wait_ms(PWM_PERIOD);
+            }
+        velocity++;
+    }
+    else if(velocity < new_velocity){
+        for(; velocity <= new_velocity; velocity++){
+            setMotorPWM(velocity,motor);
+            wait_ms(PWM_PERIOD);
+            }
+        velocity--;
+        }
 }
 /* Debug functions that prints the sensor and control inputs values. Since it's time consuming it should not be used    */
 /* in the main loop or the controller performance may be affected.                                                      */
 void debug(){
     //printf("ERROR: %f Up: %f Ui: %f Ud: %f U: %f \r\n", e[0]*180/3.1415, up[0]*100/(3.1415/8), ui[0]*100/(3.1415/8), ud[0]*100/(3.1415/8),u*100/(3.1415/8));
     //printf("Erro: %f  Sensor: %f Magnetometer: %f \r\n",e[0]*180/PI,sensor,processMagAngle(0)*180/PI);
-    printf(" %f \r\n",sensor);
+    printf(" %f \r\n",feedback);
 }
 
 /* Function to normalize the magnetometer reading */