Diff: main.cpp

Revision:

5:07bbe020eb65

Parent:

4:b37fd183e46a

--- a/main.cpp	Sat Jul 09 20:41:49 2016 +0000
+++ b/main.cpp	Tue Jul 26 17:22:33 2016 +0000
@@ -4,13 +4,32 @@
 
 int main()
 {  
+    Timer t;
+    
+    wait(3);
     Vessel seagoat; //Starts the seagoat
-    seagoat.SetYawPID(1,0,0);    
-    seagoat.SetRollPID(1,0,0);    
-    seagoat.SetPitchPID(1,0,0);    
+    seagoat.SetYawPID(4,0,0);  
+    seagoat.SetRollPID(4,0,0);    
+    seagoat.SetPitchPID(4,0,0);    
+    seagoat.SetXPID(0,0,0);    
+    seagoat.SetYPID(0,0,0);    
+    seagoat.SetZPID(0,0,0);    
+    
+    t.start();
+    while(t.read() < 5){
+         seagoat.calibrate();
+         
+         //pc.printf("%f \n", t.read());
+    }
+    t.stop();
+    pc.printf("Seagoat Ready to Go\n\r");
+    //seagoat.motorTest();
     
     while(1) {
+        
+        //seagoat.motorTest();
         seagoat.update();
-        wait(0.1);
+        
+        wait(0.01);
     }
 }
\ No newline at end of file