Diff: main.cpp

Revision:

42:7fa713d5d1af

Parent:

41:8ce8a3a47a92

Child:

43:0627a2245b9d

--- a/main.cpp	Wed Jun 15 16:00:48 2016 +0000
+++ b/main.cpp	Wed Jun 15 18:52:43 2016 +0000
@@ -96,18 +96,8 @@
     wait(0.1f);
 }
 
-int main()
-{   
-    pc.printf("MAIN\n");
-    bt.rise(&rise);
-    bt.fall(&fall);
-    
-    //ms[active]->drive(0.25);
-    
-    ticker.attach(&tick, tickTime);
-    
-    //unsigned char volatile rfget;
-    
+void calibrateLegs()
+{
     for (int i = 0; i < MOTORS; i++)
     {
         if (i != 3)
@@ -133,12 +123,12 @@
     
     for (int i = 0; i < MOTORS; i++)
         ms[i]->getEncoder().reset();
+    
     pc.printf("done");
-    
-    for (int i = 0; i < MOTORS; i++)
-        pc.printf("%ld ", ms[i]->encoder.getCount());
-    pc.printf("\n");
-    
+}
+
+void standUp()
+{
     ms[0]->rotate(0.5f + 0.125f, 0.75f);
     ms[1]->rotate(0.5f - 0.125f, 0.75f);
     ms[2]->rotate(0.5f + 0.125f, 0.75f);
@@ -146,12 +136,27 @@
     ms[4]->rotate(0.5f + 0.125f, 0.75f);
     ms[5]->rotate(0.5f - 0.125f, 0.75f);
     waitAllRotate();
+}
+
+int main()
+{   
+    pc.printf("MAIN\n");
+    bt.rise(&rise);
+    bt.fall(&fall);
+    
+    //ms[active]->drive(0.25);
+    
+    ticker.attach(&tick, tickTime);
+    
+    //unsigned char volatile rfget;
+    
+    calibrateLegs();
+    standUp();
     
     float volatile speed;
     
     float volatile sum;
     
-    
     while(1)
     {