Diff: main.cpp

Revision:

11:69a364e391dc

Parent:

10:4b88be251088

diff -r 4b88be251088 -r 69a364e391dc main.cpp
--- a/main.cpp	Thu Sep 15 18:40:03 2016 +0000
+++ b/main.cpp	Wed May 31 13:57:55 2017 +0000
@@ -1,20 +1,145 @@
 #include "stm32f103c8t6.h"
 #include "mbed.h"
-  
-int main() {
+I2C i2c(PB_9, PB_8);
+const int address = 0x08 << 1;
+Timer t;
+int Now = 0 , LastUpdate = 0, deltat = 0;
+int main()
+{
     confSysClock();     //Configure system clock (72MHz HSE clock, 48MHz USB clock)
-    
+
     Serial      pc(PA_2, PA_3);
-    DigitalOut  myled(LED1);
-    
+    pc.baud(115200);
+    //AnalogIn    nub1(PA_0);
+    PwmOut      servo1(PA_6);
+    PwmOut      servo2(PA_7);
+    PwmOut      servo3(PB_0);
+    PwmOut      servo4(PB_1);
+    servo1.period_us(20000);
+    servo2.period_us(20000);
+    servo3.period_us(20000);
+    servo4.period_us(20000);
+
+    float pitchrig = 0.0164;//41*8/20000;
+    float pitchlft = 0.0276;//69*8/20000;
+    float pitchout = 0.0224;//56*8/20000;
+
+
+    float rollrig = 0.014;//35
+    float rolllft = 0.0308;//77
+    float rollout = 0.0224;//56
+
+    float Motr_up = 0.0448;//112
+    float MotorA  = 0;
+    float MotorB  = 0;
+
+    float MotorAi = 0;
+//    float MotorBi = 0;
+
+    float diff = 0;
+
+    servo1 = 0.05f + pitchout;
+    servo2 = 0.05f + rollout;
+    servo3 = 0.05f + MotorA;
+    servo4 = 0.05f + MotorB;
+    t.start();
+
     while(1) {
-        // The on-board LED is connected, via a resistor, to +3.3V (not to GND). 
-        // So to turn the LED on or off we have to set it to 0 or 1 respectively
-        myled = 0;      // turn the LED on
-        wait_ms(200);   // 200 millisecond
-        myled = 1;      // turn the LED off
-        wait_ms(1000);  // 1000 millisecond
-        pc.printf("Blink\r\n");
+        char data[5];
+        i2c.read(address,data,5);
+        //pc.printf("%d,%d,%d,%d,%d\r\n", data[0], data[1], data[2], data[3], data[4]);
+//        if (pc.readable()) {
+//            char channel = pc.getc();
+//            if (channel>128) {
+//                char control = pc.getc();
+//                if (channel == 129 && control<=128) {
+                    Now = t.read_us();
+                    deltat = Now - LastUpdate; // set integration time by time elapsed since last filter update
+                    LastUpdate = Now;
+                    
+                    pitchout = float(data[0]*4)/20000;
+
+                    if(pitchout>pitchlft) {
+                        pitchout = pitchlft;
+                    }
+                    if(pitchout<pitchrig) {
+                        pitchout = pitchrig;
+                    }
+                    if(deltat > 4000){
+                        servo1 = 0.05f + pitchout;
+                    }
+
+//                }
+//                if (channel == 130 && control<=128) {
+                    rollout = float(data[1]*4)/20000;
+                    //pc.printf("rollout = %f\n\r", rollout);
+                    if(rollout>rolllft) {
+                        rollout = rolllft;
+                    }
+                    if(rollout<rollrig) {
+                        rollout = rollrig;
+                    }
+                    //servo2 = 0.05f + rollout;
+                    //servo2 = 0.05f+float(control*8)/20000;
+
+//                }
+//                if (channel == 131 && control<=128) {
+                    MotorAi = float(data[2]*4)/20000;
+                    //pc.printf("MotorAi = %f\n\r", MotorAi);
+                    diff = float((int16_t(data[3]/2)-64)*0.0004);
+                    pc.printf("%f,%f,%f,%f\n\r", pitchout,rollout,MotorAi,diff);
+                    MotorA = MotorAi+diff;
+                    if(MotorA>Motr_up) {
+                        MotorA = Motr_up;
+                    }
+                    if(MotorA<0) {
+                        MotorA = 0;
+                    }
+
+                    MotorB = MotorAi - diff;
+                    if(MotorB>Motr_up) {
+                        MotorB = Motr_up;
+                    }
+                    if(MotorB<0) {
+                        MotorB = 0;
+                    }
+                    //servo3 = 0.05f + MotorA;
+                    //servo4 = 0.05f + MotorB;
+                    //servo2 = 0.05f+float(control*8)/20000;
+
+//                }
+/*                if (channel == 132 && control<=128) {
+                    diff = float((int16_t(control)-64)*0.0004);
+                    //pc.printf("diff = %f\n\r", diff);
+                    
+                    MotorA = MotorAi+diff;
+                    if(MotorA>Motr_up) {
+                        MotorA = Motr_up;
+                    }
+                    if(MotorA<0) {
+                        MotorA = 0;
+                    }
+
+                    MotorB = MotorAi-diff;
+                    if(MotorB>Motr_up) {
+                        MotorB = Motr_up;
+                    }
+                    if(MotorB<0) {
+                        MotorB = 0;
+                    }
+                    servo3 = 0.05f + MotorA;
+                    servo4 = 0.05f + MotorB;
+                }
+            }*/ 
+            //servo = float(1000+(c*4))/20000;
+        //}
     }
 }
- 
\ No newline at end of file
+
+
+
+
+
+
+
+