def _handle_motion_timer(): global _set_speed, _velocityhat if _set_speed: motion.drive(*_velocities, smooth=_smooth, theta=_thetahat) _set_speed = False (vx, vy, w, s1, s2, s3) = motion.get_velocities(theta=_thetahat) _velocityhat = (vx, vy, w, s1, s2, s3)
def test_square_calibration(velocity=0.6, sleep_time=1.5, M1QPPS=None, M2QPPS=None, M3QPPS=None): global wheelbase_configured # Make sure everything is init'd if not wheelbase_configured: w.init() wheelbase_configured = True _m1qpps = M1QPPS if M1QPPS is not None else qppsM1 _m2qpps = M2QPPS if M2QPPS is not None else qppsM2 _m3qpps = M3QPPS if M3QPPS is not None else qppsM3 if _m1qpps is not None and _m2qpps is not None and _m3qpps is not None: w.SetVelocityPID(w.M1, kp, ki, kd, _m1qpps) w.SetVelocityPID(w.M2, kp, ki, kd, _m2qpps) w.SetVelocityPID(w.M3, kp, ki, kd, _m3qpps) # Right motion.drive(velocity, 0, 0) time.sleep(sleep_time) motion.stop() time.sleep(sleep_time) # Up motion.drive(0, velocity, 0) time.sleep(sleep_time) motion.stop() time.sleep(sleep_time) # Left motion.drive(-velocity, 0, 0) time.sleep(sleep_time) motion.stop() time.sleep(sleep_time) # Down motion.drive(0, -velocity, 0) time.sleep(sleep_time) motion.stop()
def square(): motion.drive(.5, 0, 0) time.sleep(1) motion.stop() time.sleep(0.5) motion.drive(0, .5, 0) time.sleep(1) motion.stop() time.sleep(0.5) motion.drive(-.5, 0, 0) time.sleep(1) motion.stop() time.sleep(0.5) motion.drive(0, -.5, 0) time.sleep(1) motion.stop()
def _go_home(): # Get current robot position, rounded to 1 decimal place xhat = round(_xhat, 1) yhat = round(_yhat, 1) thetahat = round(_thetahat, 1) # Set a tolerance tolerance = 0.1 if abs(xhat) > tolerance: dt = abs(xhat / _vx) sign = -1 if xhat > 0 else 1 print("motion.drive({},{},{}) for {} s\r".format(sign*_vx, 0, 0, dt)) motion.drive(sign*_vx, 0, 0) time.sleep(dt) motion.stop() time.sleep(0.5) if abs(yhat) > tolerance: dt = abs(yhat / _vy) sign = -1 if yhat > 0 else 1 print("motion.drive({},{},{}) for {} s\r".format(0, sign*_vy, 0, dt)) motion.drive(0, sign*_vy, 0) time.sleep(dt) motion.stop() time.sleep(0.5) if abs(thetahat) > tolerance*100: # since it's periodic... # thetahat = round(thetahat%(360) ,2) dt = abs(thetahat / _w) sign = -1 if thetahat > 0 else 1 print("motion.drive({},{},{}) for {} s\r".format(0, 0, sign*_w, dt)) motion.drive(0, 0, sign*_w) time.sleep(dt) motion.stop() time.sleep(0.5)
def spin(): motion.drive(0, 0, 2 * np.pi) time.sleep(3) motion.stop()
def test_calibration(velocity=0.6, sleep_time=1.5, M1QPPS=None, M2QPPS=None, M3QPPS=None): global wheelbase_configured # Make sure everything is init'd if not wheelbase_configured: w.init() wheelbase_configured = True _m1qpps = M1QPPS if M1QPPS is not None else qppsM1 _m2qpps = M2QPPS if M2QPPS is not None else qppsM2 _m3qpps = M3QPPS if M3QPPS is not None else qppsM3 if _m1qpps is not None and _m2qpps is not None and _m3qpps is not None: w.UpdateVelocityPID(w.M1, q=_m1qpps) w.UpdateVelocityPID(w.M2, q=_m2qpps) w.UpdateVelocityPID(w.M3, q=_m3qpps) _print_stats() print("velocity: {}\r".format(velocity)) # Forward motion.drive(velocity, 0, 0) time.sleep(sleep_time) motion.stop() time.sleep(sleep_time) # Backward motion.drive(-velocity, 0, 0) time.sleep(sleep_time) motion.stop() time.sleep(sleep_time) # Left back motion.drive(velocity * np.cos(2 * np.pi / 3), velocity * np.sin(2 * np.pi / 3), 0) time.sleep(sleep_time) motion.stop() time.sleep(sleep_time) # Right forward motion.drive(-np.cos(2 * np.pi / 3) * velocity, -np.sin(2 * np.pi / 3) * velocity, 0) time.sleep(sleep_time) motion.stop() time.sleep(sleep_time) # Right back motion.drive( np.cos(-2 * np.pi / 3) * velocity, np.sin(-2 * np.pi / 3) * velocity, 0) time.sleep(sleep_time) motion.stop() time.sleep(sleep_time) # Right Forward motion.drive(-np.cos(-2 * np.pi / 3) * velocity, -np.sin(-2 * np.pi / 3) * velocity, 0) time.sleep(sleep_time) motion.stop()
def test_calibration(velocity=0.6, sleep_time=1.5, M1QPPS=None, M2QPPS=None, M3QPPS=None): global wheelbase_configured # Make sure everything is init'd if not wheelbase_configured: w.init() wheelbase_configured = True _m1qpps = M1QPPS if M1QPPS is not None else qppsM1 _m2qpps = M2QPPS if M2QPPS is not None else qppsM2 _m3qpps = M3QPPS if M3QPPS is not None else qppsM3 if _m1qpps is not None and _m2qpps is not None and _m3qpps is not None: w.UpdateVelocityPID(w.M1, q=_m1qpps) w.UpdateVelocityPID(w.M2, q=_m2qpps) w.UpdateVelocityPID(w.M3, q=_m3qpps) _print_stats() print ("velocity: {}\r".format(velocity)) # Forward motion.drive(velocity, 0, 0) time.sleep(sleep_time) motion.stop() time.sleep(sleep_time) # Backward motion.drive(-velocity, 0, 0) time.sleep(sleep_time) motion.stop() time.sleep(sleep_time) # Left back motion.drive(velocity * np.cos(2 * np.pi / 3), velocity * np.sin(2 * np.pi / 3), 0) time.sleep(sleep_time) motion.stop() time.sleep(sleep_time) # Right forward motion.drive(-np.cos(2 * np.pi / 3) * velocity, -np.sin(2 * np.pi / 3) * velocity, 0) time.sleep(sleep_time) motion.stop() time.sleep(sleep_time) # Right back motion.drive(np.cos(-2 * np.pi / 3) * velocity, np.sin(-2 * np.pi / 3) * velocity, 0) time.sleep(sleep_time) motion.stop() time.sleep(sleep_time) # Right Forward motion.drive(-np.cos(-2 * np.pi / 3) * velocity, -np.sin(-2 * np.pi / 3) * velocity, 0) time.sleep(sleep_time) motion.stop()
def spin(): motion.drive(0, 0, 2*np.pi) time.sleep(3) motion.stop()
def _handle_velocity_command(msg): velocities = (msg.linear.x, msg.linear.y, msg.angular.z) motion.drive(*velocities, smooth=_smooth, theta=_theta)