Diff: main.cpp

Revision:

37:a983eb9fd9c5

Parent:

36:40b134328376

Child:

38:14bf11115f9f

--- a/main.cpp	Fri Apr 29 18:24:15 2016 +0000
+++ b/main.cpp	Sun May 01 18:45:02 2016 +0000
@@ -9,6 +9,9 @@
 MRF24J40 mrf(p11, p12, p13, p14, p21);
 Timer timer; // timer ;)
 Mutex desired_mutex;
+
+int started = 0;
+
 Quadcopter myQuadcopter(&pc, &mrf, &timer, &desired_mutex); // instantiate Quadcopter object
 
 // pwm outputs for the 4 motors (motor 1 points into x direction, 2 into y direction, 3 -x direction, -y direction
@@ -26,6 +29,14 @@
 //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 emergencyShutdown()
 {
     emergencyOff = 1;
@@ -38,6 +49,7 @@
 int getLowThrust(double threshold)
 {
     double force = myQuadcopter.getForce();
+    //printf("%f\r\n", force);
     if (force < threshold) { // if low thrust signal is detected
         nLowThrust++;
         printf("Negative thrust! %f, nLowThrust %d\r\n",myQuadcopter.getForce(),nLowThrust);
@@ -54,7 +66,40 @@
 {
     while(emergencyOff != 1) {
        // printf(" thrust: %f\r\n",myQuadcopter.getForce());
+        myQuadcopter.readSensorValues();
 
+        myQuadcopter.controller();
+        static int count = 0;
+        if (count % 100 == 0) {
+            printf("%s\t%d\r\n", "controller", count);
+        }
+        count++;
+        motors motorsPwm = myQuadcopter.getPwm();
+
+        motor_1 = motorsPwm.m1;
+        motor_2 = motorsPwm.m2;
+        motor_3 = motorsPwm.m3;
+        motor_4 = motorsPwm.m4;
+
+        Thread::wait(10);
+        Thread::yield();
+        // 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;
+    while(true) {
+        if (count % 1000 == 0) {
+            printf("%s\t%d\r\n", "rc", count);
+        }
+        count++; 
+        myQuadcopter.readRc();
+
+        if (!started) {
+            continue;   
+        }
         int shutdown = getLowThrust(-0.3);
         if (shutdown==1) {
             emergencyShutdown();
@@ -62,25 +107,8 @@
             shutDownLed = 1;
             break;
         }
-
-        myQuadcopter.readSensorValues();
-
-        myQuadcopter.controller();
-        motors motorsPwm = myQuadcopter.getPwm();
-
-        motor_1 = motorsPwm.m1;
-        motor_2 = motorsPwm.m2;
-        motor_3 = motorsPwm.m3;
-        motor_4 = motorsPwm.m4;
-
-        // 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)
-{
-    while(true) {
-        myQuadcopter.readRc();
+        Thread::wait(10);
+        Thread::yield();
     }
 }
 
@@ -94,7 +122,12 @@
     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);
+        }
+        count++; 
         if (battery.read() < threshold_adc) {
             printf("low battery! %f\r\n", battery.read() * saturating_voltage);
             batteryLed = 1;
@@ -107,6 +140,19 @@
     }
 }
 
+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()
 {
     // ESCs requires a 100Hz frequency
@@ -115,13 +161,17 @@
     motor_3.period(0.01);
     motor_4.period(0.01);
 
-    Thread thread_rc(rc_thread);
-
-    int startLoop = 0;
+    //Thread counting1(counting_thread1);
+    //Thread counting2(counting_thread2);
+    //counting1.set_priority(osPriorityHigh);
+    //counting2.set_priority(osPriorityHigh);
+    //Thread thread_rc(rc_thread);
 
-    while (!startLoop) { // wait until Joystick is in starting position
-        startLoop = getLowThrust(-0.3);
+    while (!started) { // wait until Joystick is in starting position
+        myQuadcopter.readRc();
+        started = getLowThrust(-0.3);
     }
+    printf("Started!\r\n");
     nLowThrust = 0; // reset for later use in controller.
 
     motor_1 = 0.1;
@@ -132,8 +182,32 @@
     wait(3); // hold startup duty cycle for 2 seconds
 
     // TODO assign priorities to threads, test if it really works as we expect
-    Thread battery(battery_thread);
-    Thread controller(controller_thread);
+    //Thread battery(battery_thread);
+    //Thread controller(controller_thread);
     // TODO is this needed?
-    while (1);
+    
+    //controller.set_priority(osPriorityRealtime);
+    //thread_rc.set_priority(osPriorityRealtime);
+    int i =0;
+    while (1) {
+        i++;
+        if (i % 100 == 0) {
+            printf("%d\r\n", i);
+        }
+        myQuadcopter.readRc();
+        int shutdown = getLowThrust(-0.3);
+        if (shutdown == 1) {
+            emergencyShutdown();
+            printf("too long negative thrust! %f\r\n",myQuadcopter.getForce());
+            shutDownLed = 1;
+            break;
+        }
+        myQuadcopter.readSensorValues();
+        myQuadcopter.controller();
+        motors motorsPwm = myQuadcopter.getPwm();
+        motor_1 = motorsPwm.m1;
+        motor_2 = motorsPwm.m2;
+        motor_3 = motorsPwm.m3;
+        motor_4 = motorsPwm.m4;
+    }
 }