def main(): global speed, direction, timeout, ser rospy.init_node('ros_servo') ipcon = IPConnection() # Create IP connection servo = BrickServo(UID, ipcon) # Create device object ipcon.connect(HOST, PORT) rospy.Subscriber("/cmd_vel", Twist, callback) r = rospy.Rate(100) # Hz vel_tp = [0] * 50 # 50 sample low-pass for speed dir_tp = [0] * 10 # 10 sample low-pass for steering while not rospy.is_shutdown(): vel_tp[len(vel_tp) - 1] = speed #if not timeout > TIMEOUT else 0 vel_tp[:-1] = vel_tp[1:] dir_tp[len(dir_tp) - 1] = direction dir_tp[:-1] = dir_tp[1:] tx_speed = (sum(vel_tp) / len(vel_tp)) * 18000 tx_dir = (sum(dir_tp) / len(dir_tp)) * -90000 rospy.loginfo("Speed: %f", tx_speed) rospy.loginfo("Steering: %f", tx_dir) #motorR = tx_speed + tx_dir #motorL= tx_speed - tx_dir servo.set_degree(0, -9000, 9000) servo.set_velocity(0, 65535) servo.set_position(0, tx_dir) servo.enable(0) servo.set_degree(1, -9000, 9000) servo.set_velocity(1, 65535) servo.set_position(1, tx_speed) servo.enable(1) timeout += 1 r.sleep()
ipcon.connect(HOST, PORT) # Connect to brickd # Don't use device before ipcon is connected # Configure two servos with voltage 5.5V # Servo 1: Connected to port 0, period of 19.5ms, pulse width of 1 to 2ms # and operating angle -100 to 100° # # Servo 2: Connected to port 5, period of 20ms, pulse width of 0.95 # to 1.95ms and operating angle -90 to 90° servo.set_output_voltage(5500) servo.set_degree(0, -10000, 10000) servo.set_pulse_width(0, 1000, 2000) servo.set_period(0, 19500) servo.set_acceleration(0, 1000) # Slow acceleration servo.set_velocity(0, 65535) # Full speed servo.set_degree(5, -9000, 9000) servo.set_pulse_width(5, 950, 1950) servo.set_period(5, 20000) servo.set_acceleration(5, 65535) # Full acceleration servo.set_velocity(5, 65535) # Full speed servo.set_position(0, 10000) # Set to most right position servo.enable(0) servo.set_position(5, -9000) # Set to most left position servo.enable(5) raw_input("Press key to exit\n") # Use input() in Python 3 servo.disable(0)
ipcon.connect(HOST, PORT) # Connect to brickd # Don't use device before ipcon is connected # Configure two servos with voltage 5.5V # Servo 1: Connected to port 0, period of 19.5ms, pulse width of 1 to 2ms # and operating angle -100 to 100° # # Servo 2: Connected to port 5, period of 20ms, pulse width of 0.95 # to 1.95ms and operating angle -90 to 90° servo.set_output_voltage(5500) servo.set_degree(0, -10000, 10000) servo.set_pulse_width(0, 1000, 2000) servo.set_period(0, 19500) servo.set_acceleration(0, 1000) # Slow acceleration servo.set_velocity(0, 65535) # Full speed servo.set_degree(5, -9000, 9000) servo.set_pulse_width(5, 950, 1950) servo.set_period(5, 20000) servo.set_acceleration(5, 65535) # Full acceleration servo.set_velocity(5, 65535) # Full speed servo.set_position(0, 10000) # Set to most right position servo.enable(0) servo.set_position(5, -9000) # Set to most left position servo.enable(5) raw_input("Press key to exit\n") # Use input() in Python 3 servo.disable(0)
servo.set_position(servo_num, -9000) elif position == -9000: print('Position: -90°, going to 90°') servo.set_position(servo_num, 9000) else: print('Error') # Can only happen if another program sets position if __name__ == "__main__": ipcon = IPConnection() # Create IP connection servo = BrickServo(UID, ipcon) # Create device object ipcon.connect(HOST, PORT) # Connect to brickd # Don't use device before ipcon is connected # Register position reached callback to function cb_position_reached servo.register_callback(servo.CALLBACK_POSITION_REACHED, cb_position_reached) # Enable position reached callback servo.enable_position_reached_callback() # Set velocity to 100°/s. This has to be smaller or equal to the # maximum velocity of the servo you are using, otherwise the position # reached callback will be called too early servo.set_velocity(0, 10000) servo.set_position(0, 9000) servo.enable(0) raw_input("Press key to exit\n") # Use input() in Python 3 servo.disable(0) ipcon.disconnect()