Fixed #239; Constrain motor signals to allowed range

Tested on hardware.
OpenROV · Jun 23, 2014 · 9b78e84 · 9b78e84
1 parent 720cc55
commit 9b78e84
Showing 1 changed file with 6 additions and 6 deletions.
diff --git a/arduino/OpenROV/Motors.cpp b/arduino/OpenROV/Motors.cpp
@@ -23,24 +23,24 @@ void Motors::goport(int p){
   if (p>MIDPOINT) modifier = motor_positive_modifer[port_motor];
   if (p<MIDPOINT) modifier = motor_negative_modifer[port_motor];
   int delta = p-MIDPOINT;
-  port.writeMicroseconds(MIDPOINT+delta*modifier);
+  port.writeMicroseconds(constrain(MIDPOINT+delta*modifier,1000,2000));
 
 }
 
 void Motors::govertical(int v){
-  int modifier = 1;  
+  int modifier = 1;
   if (v>MIDPOINT) modifier = motor_positive_modifer[vertical_motor];
   if (v<MIDPOINT) modifier = motor_negative_modifer[vertical_motor];
   int delta = v-MIDPOINT;
-  vertical.writeMicroseconds(MIDPOINT+delta*modifier); 
+  vertical.writeMicroseconds(constrain(MIDPOINT+delta*modifier,1000,2000));
 }
 
 void Motors::gostarbord(int s){
-  int modifier = 1;  
+  int modifier = 1;
   if (s>MIDPOINT) modifier = motor_positive_modifer[starbord_motor];
-  if (s<MIDPOINT) modifier = motor_negative_modifer[starbord_motor];  
+  if (s<MIDPOINT) modifier = motor_negative_modifer[starbord_motor];
   int delta = s-MIDPOINT;
-  starbord.writeMicroseconds(MIDPOINT+delta*modifier); 
+  starbord.writeMicroseconds(constrain(MIDPOINT+delta*modifier,1000,2000));
 }
 
 void Motors::stop(){