![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
New Self Balancing Code
Dependencies: mbed BMI160 max32630fthr_pitch USBDevice Math
Diff: main.cpp
- Revision:
- 7:2c48702d99e3
- Parent:
- 5:f6727800e43f
--- 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);