Diff: main.cpp

Revision:

11:5c54826d23a7

Parent:

10:e7d1801e966a

Child:

12:422963993df5

--- a/main.cpp	Sat Apr 02 17:41:37 2016 +0000
+++ b/main.cpp	Sat Apr 02 18:01:36 2016 +0000
@@ -43,16 +43,16 @@
 // myQuadcopter.setDes(...)
 
 
-    control result;
-    offset offset_gyro;
+//    control result;
+    //offset offset_gyro;
 
-    initSensors(accel, mag, gyro,offset_gyro);
+    //initSensors(accel, mag, gyro,offset_gyro);
 
-    sensors_event_t accel_event;
-    sensors_event_t mag_event;
-    sensors_event_t gyro_event;
-    sensors_vec_t   orientation;
-
+    /*  sensors_event_t accel_event;
+      sensors_event_t mag_event;
+      sensors_event_t gyro_event;
+      sensors_vec_t   orientation;
+    */
     motor_1.period(0.002);          // motor requires a 2ms period
     motor_2.period(0.002);          // motor requires a 2ms period
     motor_3.period(0.002);          // motor requires a 2ms period
@@ -87,10 +87,12 @@
     pc.printf("Entering control loop\n\r");
 
     while (1) {
-        /* Calculate pitch and roll from the raw accelerometer data */
+        myQuadcopter.readSensorValues();
+
+        myQuadcopter.controller();
+        /*
         accel.getEvent(&accel_event);
         if (dof.accelGetOrientation(&accel_event, &orientation)) {
-            /* 'orientation' should have valid .roll and .pitch fields */
             //s_com->print(("Roll: "));
             //s_com->print(orientation.roll);
             //s_com->print(("; "));
@@ -98,26 +100,28 @@
             //s_com->print(orientation.pitch);
             //s_com->print((";\t"));
         }
+        */
+
         /* Calculate the heading using the magnetometer */
-        mag.getEvent(&mag_event);
+        /*mag.getEvent(&mag_event);
         if (dof.magGetOrientation(SENSOR_AXIS_Z, &mag_event, &orientation)) {
-            /* 'orientation' should have valid .heading data now */
             //s_com->print(("Heading: "));
             //s_com->print(orientation.heading);
             //  s_com->print((";\r\n"));
         }
         /* Get angular rate data from gyroscope */
-        gyro.getEvent(&gyro_event);
+
+        /*gyro.getEvent(&gyro_event);
         gyro_event.gyro.x -= offset_gyro.x;
         gyro_event.gyro.y -= offset_gyro.y;
         gyro_event.gyro.z -= offset_gyro.z;
-
+        */
         wait(0.01);
 
         // get sensor values
 
         // call controller
-        controller(gyro_event, orientation, &result);
+        //   controller(gyro_event, orientation, &result);
 
         // compute PWM singals
         // assumption for low angles the computed moments are between -10...10
@@ -127,14 +131,15 @@
 
         // Set duty cycle
         // continue looking what pwm is.
+        motors motorsPwm=myQuadcopter.getPwm();
 
-        motor_2=0.6+result.M_x/100;
-        motor_4=0.6-result.M_x/100;
+        motor_2=motorsPwm.m2;
+        motor_4=motorsPwm.m4;
 
 
         // plot
 
         //pc.printf("F: %f M_x: %f M_y: %f M_z: %f\n\r",  F, M_x, M_y, M_z);
-        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);
+        //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);
     }
 }