Diff: main.cpp

Revision:

27:11116aa69f32

Parent:

26:7f50323c0c0d

Child:

28:61f7356325c3

--- a/main.cpp	Thu Apr 14 19:58:42 2016 +0000
+++ b/main.cpp	Thu Apr 14 22:32:30 2016 +0000
@@ -82,6 +82,8 @@
 
 int main()
 {
+
+
     // get desired values from joystick (to be implemented)
     // myQuadcopter.setDes(...)
 
@@ -90,6 +92,23 @@
     motor_3.period(0.01);          // motor requires a 2ms period
     motor_4.period(0.01);          // motor requires a 2ms period
 
+    Thread threadR(rc_thread);
+    double forceThreshold = -0.3;
+    double forceRc = myQuadcopter.getForce();
+    
+    while (forceRc > forceThreshold)  // wait until Joystick is in starting position
+    {
+        forceRc = myQuadcopter.getForce();
+    }
+    
+        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();
@@ -111,7 +130,7 @@
         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;
 
@@ -125,10 +144,9 @@
 
 // TODO assign priorities to threads, test if it really works as we expect
     Thread thread(controller_thread);
-    Thread threadR(rc_thread);
     Thread battery(battery_thread);
 
-    pc.printf("Entering control loop\n\r");
+    //pc.printf("Entering control loop\n\r");
 
     while (1) {
         //myQuadcopter.readSensorValues();