Diff: main.cpp

Revision:

7:27516a2b504b

Parent:

6:c930555fd872

Child:

8:a1067fcde341

--- a/main.cpp	Sun Apr 17 01:05:16 2016 +0000
+++ b/main.cpp	Sun Apr 17 01:07:25 2016 +0000
@@ -58,55 +58,23 @@
 void debug();
 
 int main(){
-    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);
+    printf("Initializing controller....\r\n\r\n");
+    initializeController();
+    printf("Controller Initialized. \r\n");
+    magCal();
+    gyro_angle = processMagAngle();
+    velocity = MINIMUM_VELOCITY;
+    setMotorPWM(velocity,motor);
+    startGyro();
+        while (true){
+        processGyroAngle();
+        controlAnglePID(P,I,D,N);
+        debug();
+        if(t.read_us() < Ts*1000000)
+            wait_us(Ts*1000000 - t.read_us());
+        if(pc.readable())
+            readProtocol();
+        }
 }
 void readProtocol(){
     char msg = ser.getc();