Revision 7:2c48702d99e3, committed 2018-12-21

Comitter:

CharlesMaxim

Date:

Fri Dec 21 19:49:34 2018 +0000

Parent:

6:ee03dafaa43f

Commit message:

Updated Self Balancing

Changed in this revision

--- a/main.cpp	Thu Dec 13 11:24:42 2018 +0000
+++ b/main.cpp	Fri Dec 21 19:49:34 2018 +0000
@@ -48,8 +48,8 @@
 DigitalOut gLED(P2_5, LED_ON);
 DigitalOut bLED(P2_6, LED_ON);
 
-DigitalOut M_1(P3_5);
-DigitalOut M_2(P3_4);
+PwmOut PWM(P3_5);
+DigitalOut Dir(P3_4);
 
 
 USBSerial pc(USBTX,USBRX);
@@ -169,6 +169,7 @@
         
         
         float imuTemperature;
+        float dutyCycle;
         
         double xDeviation, yDeviation, zDeviation;
         double prevGyroX, prevGyroY, prevGyroZ;
@@ -200,8 +201,28 @@
             
             time_2 = sensorTime.seconds;
             apitch = compFilter(k, apitch, gyroData.xAxis.scaled, accData.yAxis.scaled, accData.zAxis.scaled, time_2 - time_1);
-            daplink.printf("Forward: %s%4.3f\r\n", "\033[K", apitch);
-            time_1 = time_2;
+            pc.printf("Drift: %s%4.3f\r\n", "\033[K", apitch);
+          
+            
+            //dutyCycle = (apitch * (0.2 / 10)) + 0.8;
+          //  dutyCycle = fabsf(dutyCycle);
+          //  pc.printf("PWM: %s%4.3f\r\n", "\033[K", dutyCycle);
+            
+              time_1 = time_2;
+        /*    
+            if(dutyCycle < 0 ){
+                dutyCycle = 0 - dutyCycle;
+            }
+         */   
+            if(apitch <= 0){
+                PWM = 0.5;
+                Dir = 1;    
+            }
+            
+            if(apitch > 0){
+                PWM = 0.5;
+                Dir = 0;    
+            }
             
              //printRegister(imu, BMI160::GYR_CONF);