Diff: main.cpp

Revision:

46:4bcf2e679f96

Parent:

45:9f74298eee78

Child:

47:ae478c380136

--- a/main.cpp	Thu May 05 14:42:30 2016 +0000
+++ b/main.cpp	Thu May 05 15:58:49 2016 +0000
@@ -8,11 +8,11 @@
 Serial pc(USBTX, USBRX);
 MRF24J40 mrf(p11, p12, p13, p14, p21);
 Timer timer; // timer ;)
-Mutex desired_mutex;
+//Mutex desired_mutex;
 
 int started = 0;
 
-Quadcopter myQuadcopter(&pc, &mrf, &timer, &desired_mutex); // instantiate Quadcopter object
+Quadcopter myQuadcopter(&pc, &mrf, &timer); // instantiate Quadcopter object
 
 // pwm outputs for the 4 motors (motor 1 points into x direction, 2 into y direction, 3 -x direction, -y direction
 PwmOut motor_1(p23);
@@ -24,20 +24,19 @@
 AnalogIn battery(p20);
 DigitalOut batteryLed(LED1);
 DigitalOut shutDownLed(LED2);
-DigitalOut quicknessLed(LED3); // used for check delay in IMU measurement
+//DigitalOut quicknessLed(LED3); // used for check delay in IMU measurement
 
 int emergencyOff = 0;
-//int lowThrust= {0,0,0};
 int nLowThrust = 0;
 
-void print_count(int id)
-{
-    static int count = 0;
-    if (count % 1000 == 0) {
-        printf("%d\t%d\r\n", id, count);
-    }
-    count++;
-}
+//void print_count(int id)
+//{
+//    static int count = 0;
+//    if (count % 1000 == 0) {
+//        printf("%d\t%d\r\n", id, count);
+//    }
+//    count++;
+//}
 
 void emergencyShutdown()
 {
@@ -64,7 +63,7 @@
     return 0;
 }
 
-void controller_thread(void const *args)
+/*void controller_thread(void const *args)
 {
     while(emergencyOff != 1) {
         // printf(" thrust: %f\r\n",myQuadcopter.getForce());
@@ -88,7 +87,9 @@
         // pc.printf("m1: %f m2: %f m3: %f m4: %f \n\r",  motorsPwm.m1, motorsPwm.m2, motorsPwm.m3, motorsPwm.m4);
     }
 }
+*/
 
+/*
 void rc_thread(void const *args)
 {
     static int count = 0;
@@ -113,7 +114,7 @@
         Thread::yield();
     }
 }
-
+*/
 bool low_battery()
 {
     float threshold_voltage = 13.0; // desired lowest battery voltage
@@ -135,20 +136,20 @@
     return false;
 }
 
-void counting_thread1(void const *args)
-{
-    while (true) {
-        print_count(1);
-        Thread::wait(1);
-    }
-}
-
-void counting_thread2(void const *args)
-{
-    while (true) {
-        print_count(2);
-    }
-}
+//void counting_thread1(void const *args)
+//{
+//    while (true) {
+//        print_count(1);
+//        Thread::wait(1);
+//    }
+//}
+//
+//void counting_thread2(void const *args)
+//{
+//    while (true) {
+//        print_count(2);
+//    }
+//}
 
 int main()
 {
@@ -164,7 +165,7 @@
     //counting2.set_priority(osPriorityHigh);
     //Thread thread_rc(rc_thread);
 
-    printf("waiting for start signal!\r\n");
+    printf("Waiting for start signal!\r\n");
 
     while (!started) { // wait until Joystick is in starting position
         myQuadcopter.readRc();
@@ -172,26 +173,26 @@
     }
     printf("Started!\r\n");
     nLowThrust = 0; // reset for later use in controller.
+    // start ESCs up with 10% duty cycle
+    motor_1 = OFF_PWM; 
+    motor_2 = OFF_PWM; 
+    motor_3 = OFF_PWM; 
+    motor_4 = OFF_PWM; 
 
-    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(1); // hold startup duty cycle for 2 seconds
+    wait(1); // hold startup duty cycle for 1 second
     
+    // Idle thrust
     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
+    wait(1); // hold idle thrust for 1 second
 
     // TODO assign priorities to threads, test if it really works as we expect
     //Thread battery(battery_thread);
     //Thread controller(controller_thread);
     // TODO is this needed?
-
     //controller.set_priority(osPriorityRealtime);
     //thread_rc.set_priority(osPriorityRealtime);
 
@@ -199,15 +200,19 @@
     float previousTime = 0;
     float currentTime = 0;
     float checkBattery = 0.0;
+    int shutdown = 0;
+    
+    // Enter while loop with control
     while (1) {
+        // Check battery level
         i++;
         currentTime = timer.read();
         checkBattery += currentTime - previousTime;
         if (checkBattery > 1.0) {
             checkBattery = 0.0;
-            if (low_battery()) {
-                break;
-            }
+ //           if (low_battery()) {
+//                break;
+//            }
         }
         previousTime = currentTime;
 
@@ -220,30 +225,22 @@
 //
 //        }
         
-
         myQuadcopter.readRc();
 
-        int shutdown = getLowThrust(-0.3);
+        // shutdown
+        shutdown = getLowThrust(-0.3);
         if (shutdown == 1) {
             emergencyShutdown();
             printf("too long negative thrust! %f\r\n",myQuadcopter.getForce());
             shutDownLed = 1;
             break;
         }
+        // read sensor values
         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;
-        }
-        */
-
+        // compute controller and get desired pwm
         myQuadcopter.controller();
         motors motorsPwm = myQuadcopter.getPwm();
+        // set pwm
         motor_1 = motorsPwm.m1;
         motor_2 = motorsPwm.m2;
         motor_3 = motorsPwm.m3;