Diff: main.cpp

Revision:

14:64b06476d943

Parent:

13:291ba30c7806

Child:

15:90e07946186f

--- a/main.cpp	Sat Apr 02 22:06:46 2016 +0000
+++ b/main.cpp	Thu Apr 07 02:07:33 2016 +0000
@@ -1,15 +1,12 @@
 #include "mbed.h"
+#include "rtos.h"
 #define _MBED_
-#include "Adafruit_9DOF.h"
-#include "Serial_base.h"
 //#include "controller.h"
 #include "sensor.h"
-
 #include "quadcopter.h"
 
-// initialze serial connection
 Serial pc(USBTX, USBRX);
-Serial *pcPntr=&pc; // pointer used to enable serial connection with print method in Quadcopter class
+Quadcopter myQuadcopter(&pc); // 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(p21);
@@ -17,14 +14,26 @@
 PwmOut motor_3(p23);
 PwmOut motor_4(p24);
 
-int main()
-{
-    Quadcopter myQuadcopter; // initialze Quadcopter object
-    myQuadcopter.setSerial(pcPntr); // give Quadcopter object pc Pointer in order to print
-    myQuadcopter.initAllSensors(); // initialize sensors
+void controller_thread(void const *args) {
+  while(true){
+  myQuadcopter.controller();
+  Thread::wait(10);
+  }
+}
 
-// get desired values from joystick (to be implemented)
-// myQuadcopter.setDes(...)
+void rc_thread(void const *args) {
+  while(true){
+      myQuadcopter.readRc();
+      Thread::wait(100);
+  }
+}
+
+
+int main() {
+    //Thread thread(controller_thread);
+
+    // get desired values from joystick (to be implemented)
+    // myQuadcopter.setDes(...)
 
     motor_1.period(0.002);          // motor requires a 2ms period
     motor_2.period(0.002);          // motor requires a 2ms period
@@ -47,18 +56,25 @@
     motor_4 = 0.5;
 
     pc.printf("Type 'c' to enter control loop, or anything else to abort.\n\r");
-    char b= pc.getc();
+    char b = pc.getc();
     if (b!='c') {
         pc.printf("Aborting");
         return 0;
     };
+    
+    //
+    Thread threadR(rc_thread);
+
+    //check if remote controll is working with if statement
+    
+    Thread threadC(controller_thread);
 
     pc.printf("Entering control loop\n\r");
 
     while (1) {
         myQuadcopter.readSensorValues();
 
-        myQuadcopter.controller();
+//        myQuadcopter.controller();
 
 
         wait(0.01);
@@ -68,8 +84,9 @@
 
         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("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);
         //pc.printf("M_x: %f\tM_y: %f\tM_z: %f\tF: %f\n\r", result.M_x, result.M_y, result.M_z, result.F);
     }
 }