diff options
-rw-r--r-- | main.cpp | 26 |
1 files changed, 22 insertions, 4 deletions
@@ -15,6 +15,17 @@ inline void saturate(T& var, T absmax) { } } +template<class T> +inline T limit(T var, T min, T max) { + if(var < min) { + return min; + } else if(var > max) { + return max; + } else { + return var; + } +} + class PID { private: uint16_t Kp, Ki, Kd; @@ -95,9 +106,16 @@ int main() { saturate(roll, max); saturate(yaw, max); - TIM2.CCR1 = 1000 + throttle + pitch + roll + yaw; - TIM2.CCR2 = 1000 + throttle + pitch - roll - yaw; - TIM2.CCR3 = 1000 + throttle - pitch + roll - yaw; - TIM2.CCR4 = 1000 + throttle - pitch - roll + yaw; + int cmds[] = { + 1000 + throttle + pitch + roll + yaw, + 1000 + throttle + pitch - roll - yaw, + 1000 + throttle - pitch + roll - yaw, + 1000 + throttle - pitch - roll + yaw, + }; + + 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); } } |