Limit commands.

1 files changed, 22 insertions, 4 deletions

diff --git a/main.cpp b/main.cpp
index c0deaf9..ed03845 100644
--- a/main.cpp
+++ b/main.cpp
@@ -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);
 	}
 }