Diff: main.cpp

Revision:

28:61f7356325c3

Parent:

27:11116aa69f32

Child:

29:ae765492fa8b

--- a/main.cpp	Thu Apr 14 22:32:30 2016 +0000
+++ b/main.cpp	Fri Apr 15 19:08:56 2016 +0000
@@ -19,9 +19,7 @@
 AnalogIn battery(p20);
 DigitalOut batteryLed(LED1);
 
-
-//
-int emergencyOff=0;
+int emergencyOff = 0;
 
 void controller_thread(void const *args)
 {
@@ -29,14 +27,12 @@
         myQuadcopter.readSensorValues();
 
         myQuadcopter.controller();
-        motors motorsPwm=myQuadcopter.getPwm();
+        motors motorsPwm = myQuadcopter.getPwm();
 
-        motor_1=motorsPwm.m1;
-        motor_2=motorsPwm.m2;
-        motor_3=motorsPwm.m3;
-        motor_4=motorsPwm.m4;
-
-
+        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);
 
@@ -82,8 +78,6 @@
 
 int main()
 {
-
-
     // get desired values from joystick (to be implemented)
     // myQuadcopter.setDes(...)
 
@@ -95,71 +89,30 @@
     Thread threadR(rc_thread);
     double forceThreshold = -0.3;
     double forceRc = myQuadcopter.getForce();
-    
-    while (forceRc > forceThreshold)  // wait until Joystick is in starting position
-    {
+
+   // pc.printf("forceRc %f\n\r", forceRc);
+    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;
-    motor_2 = 0.1;
-    motor_3 = 0.1;
-    motor_4 = 0.1;
-    
-    wait(2); // hold startup duty cycle for 2 seconds
 
-/*
-    // startup procedure
-    pc.printf("Type 's' to start up Motors, or anything else to abort.\n\r");
-    char a= pc.getc();
-    if (a!='s') {
-        pc.printf("Aborting");
-        return 0;
-    };
-
-    // Duty cycle at beginning must be  50%
-    pc.printf("Starting up ESCs\n\r");
     motor_1 = 0.1;
     motor_2 = 0.1;
     motor_3 = 0.1;
     motor_4 = 0.1;
 
-    pc.printf("Type 'c' to enter control loop, or anything else to abort.\n\r");
-    char b = pc.getc();
-    if (b!='c') {
-        pc.printf("Aborting");
-        return 0;
-    };
-*/
-    /*     pc.printf("Outputting duty cycle specified below now press all but b to abort.\n\r");
-             motor_1 = 0.07;
+    wait(2); // hold startup duty cycle for 2 seconds
 
 
-     char c = pc.getc();
-     if (c!='c') {
-         pc.printf("Aborting");
-         return 0;
-     };
-     */
-
 // TODO assign priorities to threads, test if it really works as we expect
+  //     Thread battery(battery_thread);
     Thread thread(controller_thread);
-    Thread battery(battery_thread);
 
     //pc.printf("Entering control loop\n\r");
 
     while (1) {
-        //myQuadcopter.readSensorValues();
-
-        // myQuadcopter.controller();
-
-        // wait(0.01);
-
-        // Set duty cycle
-        // motors motorsPwm=myQuadcopter.getPwm();
-
-        //motor_2=motorsPwm.m2;
-        //motor_4=motorsPwm.m4;
 
         //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);