Diff: main.cpp

Revision:

6:c930555fd872

Parent:

4:8ead8ada8a8b

Child:

7:27516a2b504b

--- a/main.cpp	Sun Apr 17 01:01:17 2016 +0000
+++ b/main.cpp	Sun Apr 17 01:05:16 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 feedback, 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)
@@ -182,12 +212,45 @@
 }
 /* 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.                                                      */