Diff: main.cpp

Revision:

29:ae765492fa8b

Parent:

28:61f7356325c3

Child:

30:4820042e67b5

--- a/main.cpp	Fri Apr 15 19:08:56 2016 +0000
+++ b/main.cpp	Fri Apr 15 19:33:14 2016 +0000
@@ -35,8 +35,6 @@
         motor_4 = motorsPwm.m4;
 
         // pc.printf("m1: %f m2: %f m3: %f m4: %f \n\r",  motorsPwm.m1, motorsPwm.m2, motorsPwm.m3, motorsPwm.m4);
-
-        Thread::wait(10);
     }
 }
 
@@ -44,7 +42,6 @@
 {
     while(true) {
         myQuadcopter.readRc();
-        Thread::wait(50);  // wait for some number of miliseconds
     }
 }
 
@@ -61,13 +58,13 @@
     while(true) {
         if (battery.read() < threshold_adc) {
             printf("low battery! %f\r\n", battery.read() * saturating_voltage);
-            batteryLed=1;
+            batteryLed = 1;
             if (battery.read() < emergency_adc) {
-                emergencyOff=1;
-                motor_1=0.1;
-                motor_2=0.1;
-                motor_3=0.1;
-                motor_4=0.1;
+                emergencyOff = 1;
+                motor_1 = 0.1;
+                motor_2 = 0.1;
+                motor_3 = 0.1;
+                motor_4 = 0.1;
             }
         }
         Thread::wait(1000);  // wait for some number of miliseconds
@@ -78,24 +75,20 @@
 
 int main()
 {
-    // get desired values from joystick (to be implemented)
-    // myQuadcopter.setDes(...)
-
-    motor_1.period(0.01);          // motor requires a 2ms period
-    motor_2.period(0.01);          // motor requires a 2ms period
-    motor_3.period(0.01);          // motor requires a 2ms period
-    motor_4.period(0.01);          // motor requires a 2ms period
+    // ESCs requires a 100Hz frequency
+    motor_1.period(0.01);
+    motor_2.period(0.01);
+    motor_3.period(0.01);
+    motor_4.period(0.01);
 
     Thread threadR(rc_thread);
     double forceThreshold = -0.3;
     double forceRc = myQuadcopter.getForce();
 
-   // pc.printf("forceRc %f\n\r", forceRc);
+    // TODO figure out why forceRc occasionally goes below the threshold without input
     while (forceRc > forceThreshold) { // wait until Joystick is in starting position
         forceRc = myQuadcopter.getForce();
         wait(1);
-        //        pc.printf("forceRc %f\n\r", forceRc);
-
     }
 
     motor_1 = 0.1;
@@ -105,17 +98,10 @@
 
     wait(2); // hold startup duty cycle for 2 seconds
 
-
-// TODO assign priorities to threads, test if it really works as we expect
-  //     Thread battery(battery_thread);
+    // TODO assign priorities to threads, test if it really works as we expect
+    Thread battery(battery_thread);
     Thread thread(controller_thread);
 
-    //pc.printf("Entering control loop\n\r");
-
-    while (1) {
-
-        //pc.printf("F: %f M_x: %f M_y: %f M_z: %f\n\r",  F, M_x, M_y, M_z);
-        // pc.printf("m1: %f m2: %f \n\r",  motorsPwm.m2, motorsPwm.m4);
-        //pc.printf("M_x: %f\tM_y: %f\tM_z: %f\tF: %f\n\r", result.M_x, result.M_y, result.M_z, result.F);
-    }
+    // TODO is this needed?
+    while (1);
 }