Browse Source

更新上下转动舵机控制范围0-180度

corvin 4 years ago
parent
commit
74a8457f7e
1 changed files with 4 additions and 4 deletions
  1. 4 4
      src/teleop_twist_keyboard/teleop_twist_keyboard.py

+ 4 - 4
src/teleop_twist_keyboard/teleop_twist_keyboard.py

@@ -31,8 +31,8 @@ anything else : stop
 q/z : increase/decrease max speeds by 10%
 w/x : increase/decrease only linear speed by 10%
 e/c : increase/decrease only angular speed by 10%
-1/2 : increase/decrease up/down servo angle by 5°
-3/4 : increase/decrease left/right servo angle by 5°
+1/2 : increase/decrease up/down servo angle(0-180°) by 5°
+3/4 : increase/decrease left/right servo angle(0-180°) by 5°
 
 1 : Camera UP
 2 : Camera Down
@@ -138,7 +138,7 @@ if __name__=="__main__":
                 z = 0
                 th = 0
                 print(angles(upDownAngle))
-                if (upDownAngle < 90 and upDownAngle >=0):
+                if (upDownAngle < 180 and upDownAngle >=0):
                     upDownAngle = upDownAngle + cameraUpBindings[key]
                     client_srv(0, upDownAngle)
             elif key in cameraDownBindings.keys():
@@ -147,7 +147,7 @@ if __name__=="__main__":
                 z = 0
                 th = 0
                 print(angles(upDownAngle))
-                if (upDownAngle <= 90 and upDownAngle >0):
+                if (upDownAngle <= 180 and upDownAngle >0):
                     upDownAngle = upDownAngle + cameraDownBindings[key]
                     client_srv(0, upDownAngle)
             elif key in cameraLeftBindings.keys():