diff options
| -rw-r--r-- | main.cpp | 24 |
1 files changed, 14 insertions, 10 deletions
@@ -64,6 +64,8 @@ ITG3200 gyro(i2c); BMA150 accel(i2c); volatile uint16_t dmabuf[2]; +volatile uint16_t motors[4]; + void threadmain() { ADC1.CR2 = 0x9; while(ADC1.CR2 & 0x8); @@ -90,6 +92,10 @@ void threadmain() { accel.x, accel.y, accel.z, + motors[0], + motors[1], + motors[2], + motors[3], dmabuf[0], dmabuf[1], }; @@ -193,16 +199,14 @@ int main() { saturate(roll, max); saturate(yaw, max); - int cmds[] = { - 1000 + throttle + pitch - roll + yaw, - 1000 + throttle - pitch - roll - yaw, - 1000 + throttle - pitch + roll + yaw, - 1000 + throttle + pitch + roll - yaw, - }; + motors[0] = limit(throttle + pitch - roll + yaw, 0, 1000); + motors[1] = limit(throttle - pitch - roll - yaw, 0, 1000); + motors[2] = limit(throttle - pitch + roll + yaw, 0, 1000); + motors[3] = limit(throttle + pitch + roll - yaw, 0, 1000); - TIM2.CCR1 = limit(cmds[0], 1000, 2000); - TIM2.CCR2 = limit(cmds[1], 1000, 2000); - TIM2.CCR3 = limit(cmds[2], 1000, 2000); - TIM2.CCR4 = limit(cmds[3], 1000, 2000); + TIM2.CCR1 = 1000 + motors[0]; + TIM2.CCR2 = 1000 + motors[1]; + TIM2.CCR3 = 1000 + motors[2]; + TIM2.CCR4 = 1000 + motors[3]; } } |