Diff: main.cpp

Revision:

42:d09dec5bb184

Parent:

40:09a59d5b7944

Child:

43:0e98d5488bf2

--- a/main.cpp	Wed May 04 18:46:06 2016 +0000
+++ b/main.cpp	Wed May 04 21:35:05 2016 +0000
@@ -42,10 +42,10 @@
 void emergencyShutdown()
 {
     emergencyOff = 1;
-    motor_1 = 0.1;
-    motor_2 = 0.1;
-    motor_3 = 0.1;
-    motor_4 = 0.1;
+    motor_1 = OFF_PWM;
+    motor_2 = OFF_PWM;
+    motor_3 = OFF_PWM;
+    motor_4 = OFF_PWM;
 }
 
 int getLowThrust(double threshold)
@@ -114,7 +114,7 @@
     }
 }
 
-void battery_thread(void const *args)
+bool low_battery()
 {
     float threshold_voltage = 13.0; // desired lowest battery voltage
     float emergencyVoltage = 12.5; // switch off motors below it
@@ -124,22 +124,15 @@
     float threshold_adc = max_adc * threshold_voltage / max_voltage;
     float emergency_adc = max_adc * emergencyVoltage / max_voltage;
 
-    static int count = 0;
-    while(true) {
-        if (count % 1 == 0) {
-            //printf("%s\t%d\r\n", "battery", count);
+    if (battery.read() < threshold_adc) {
+        printf("low battery! %f\r\n", battery.read() * saturating_voltage);
+        batteryLed = 1;
+        if (battery.read() < emergency_adc) {
+            emergencyShutdown();
+            return true;
         }
-        count++;
-        if (battery.read() < threshold_adc) {
-            printf("low battery! %f\r\n", battery.read() * saturating_voltage);
-            batteryLed = 1;
-            if (battery.read() < emergency_adc) {
-                emergencyShutdown();
-                break;
-            }
-        }
-        Thread::wait(1000);  // wait for some number of miliseconds
     }
+    return false;
 }
 
 void counting_thread1(void const *args)
@@ -180,12 +173,19 @@
     printf("Started!\r\n");
     nLowThrust = 0; // reset for later use in controller.
 
-    motor_1 = 0.1;
-    motor_2 = 0.1;
-    motor_3 = 0.1;
-    motor_4 = 0.1;
+    motor_1 = OFF_PWM; // starts at 11.72% duty cycle
+    motor_2 = OFF_PWM; // starts at 11.61% duty cycle
+    motor_3 = OFF_PWM; // starts at 11.61% duty cycle
+    motor_4 = OFF_PWM; // starts at 11.71% duty cycle
 
-    wait(3); // hold startup duty cycle for 2 seconds
+    wait(1); // hold startup duty cycle for 2 seconds
+    
+    motor_1 = MIN_PWM_1; // starts at 11.72% duty cycle
+    motor_2 = MIN_PWM_2; // starts at 11.61% duty cycle
+    motor_3 = MIN_PWM_3; // starts at 11.61% duty cycle
+    motor_4 = MIN_PWM_4; // starts at 11.71% duty cycle
+
+    wait(1); // hold startup duty cycle for 2 seconds
 
     // TODO assign priorities to threads, test if it really works as we expect
     //Thread battery(battery_thread);
@@ -200,9 +200,14 @@
     float currentTime = 0;
     while (1) {
         i++;
+        if (i % 1000 == 0) {
+            if (low_battery()) {
+                break;
+            }
+        }
 
         // can be used to measure frequency
-        /* 
+        /*
         if (i % 5000 == 0) {
             currentTime = timer.read();
             printf("%d, %f, %f\r\n", i, 5000.0 / (currentTime - previousTime), currentTime);
@@ -210,9 +215,9 @@
 
         }
         */
-        
+
         myQuadcopter.readRc();
-        
+
         int shutdown = getLowThrust(-0.3);
         if (shutdown == 1) {
             emergencyShutdown();
@@ -221,15 +226,15 @@
             break;
         }
         myQuadcopter.readSensorValues();
-        
+
         // can be used to test responsiveness of the IMU
-        /* 
+        /*
         state mystate = myQuadcopter.getState();
         if (mystate.phi > 0.0785) {
             quicknessLed = 1;
         } else {
             quicknessLed = 0;
-        } 
+        }
         */
 
         myQuadcopter.controller();