Diff: main.cpp

Revision:

4:3040d0f9e8c6

Parent:

3:828e82089564

Child:

5:f007542f1dab

--- a/main.cpp	Thu Mar 31 22:05:57 2016 +0000
+++ b/main.cpp	Fri Apr 01 18:55:21 2016 +0000
@@ -13,12 +13,21 @@
 
 Serial pc(USBTX, USBRX);
 
+// pwm outputs for the 4 motors
+PwmOut motor_1(p21);
+PwmOut motor_2(p23);
+PwmOut motor_3(p23);
+PwmOut motor_4(p24);
+
+
+
+
 
 
 int main()
 {
     control result;
-    offset offset_gyro; 
+    offset offset_gyro;
 
     initSensors(accel, mag, gyro,offset_gyro);
 
@@ -26,10 +35,44 @@
     sensors_event_t mag_event;
     sensors_event_t gyro_event;
     sensors_vec_t   orientation;
-    
+
+    motor_1.period(0.002);          // servo requires a 2ms period
+    motor_2.period(0.002);          // servo requires a 2ms period
+    motor_3.period(0.002);          // servo requires a 2ms period
+    motor_4.period(0.002);          // servo requires a 2ms period
+
+// pwm duty cycles for the 4 motors
+    motor_1 = 0;
+    motor_2 = 0;
+    motor_3 = 0;
+    motor_4 = 0;
+
+    // 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;
+    };
+    pc.printf("Starting up ESCs\n\r");
+    motor_1 = 0.5;
+    motor_2 = 0.5;
+    motor_3 = 0.5;
+    motor_4 = 0.5;
+
+    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("Entering control loop\n\r");
+
+    while (1) {
 
 
-    while (1) {
 
         /* Calculate pitch and roll from the raw accelerometer data */
         accel.getEvent(&accel_event);
@@ -48,7 +91,7 @@
             /* 'orientation' should have valid .heading data now */
             //s_com->print(("Heading: "));
             //s_com->print(orientation.heading);
-          //  s_com->print((";\r\n"));
+            //  s_com->print((";\r\n"));
         }
         /* Get angular rate data from gyroscope */
         gyro.getEvent(&gyro_event);
@@ -63,6 +106,18 @@
         // call controller
         controller(gyro_event, orientation, &result);
 
+        // compute PWM singals
+        // assumption for low angles the computed moments are between -10...10
+        // since I dont want to risk, i set PWM to 60% duty cycle if deflection =10.
+        // USB connection of quadcopter points into x direction.
+        
+        
+        // Set duty cycle
+        // continue looking what pwm is. 
+        motor_2=50+result.M_x;
+        motor_4=50-result.M_x;
+
+
         // plot
 
         //pc.printf("F: %f M_x: %f M_y: %f M_z: %f\n\r",  F, M_x, M_y, M_z);