Fixed some maths.

emb/servos.cpp

@@ -5,6 +5,8 @@
 #include <stdint.h>
 
 #define NUM_SERVOS 3
+#define MIN_PULSE 999
+#define MAX_PULSE 4999
 
 static void reorderServos();
 static void initTimer1();
@@ -41,7 +43,7 @@ void initServos()
 void setServoAngle(uint8_t servo, int16_t arcMin)
 {
     if (0 <= servo && servo < NUM_SERVOS)
-        servos[servo].tempValue = 2999 + (int32_t) arcMin * 1000 / (180 * 60);
+        servos[servo].tempValue = MIN_PULSE + (int32_t) (arcMin + 180 * 60)  * (MAX_PULSE - MIN_PULSE) / (360 * 60);
 }
 
 void updateServos()