def waitUntilClosed(MotorController):
mc.goto(3)
while not MotorController.machineState == 4:
sleep(0.1)
pass
print('[+] Stopping MotorController.')
MotorController.stop()
def __init__(self):
self._left_motor_controller = MotorController(
0, left_pins, left_motor_pid_constants, 0)
self._right_motor_controller = MotorController(
1, right_pins, right_motor_pid_constants, 1)
self._speed_filter = IrregularDoubleExponentialFilter(
*robot_speed_smoothing_factors)
self.pos_heading = (ZERO_POS, 0.0)
self.robot_angle = 0.0
self.robot_speed = 0.0
def setUp(self):
p = MockPwm()
self.p_output = MockPidOutput()
p_controller = MockPid(self.p_output)
q = MockQep()
self.m = MotorController(p, p_controller, self.p_output, q)
self.m_pid_disabled = MotorController(p, p_controller, self.p_output, q, False)
pass
def __init__(self, database_ip, database_user, database_password, database_name):
self.ph_measurement = []
self.buffer_length = rospy.get_param('buffer_length')
self.ph_limit_low = rospy.get_param('ph_limit_low')
self.ph_limit_high = rospy.get_param('ph_limit_high')
self.cooldown_minutes = rospy.get_param('cooldown_minutes')
self.client = InfluxDBClient(database_ip, 8086, database_user, database_password, database_name)
self.ph_sub = rospy.Subscriber("/sensors/alkalinity", Alkalinity, self.OnAlkalinity)
self.dispenser = FluidDispenser()
self.dispenser.AddMotor(MotorController(36, 38, 40), 0.8827, "pump1") # PH DOWN PUMP
self.dispenser.AddMotor(MotorController(35, 37, 33), 0.8156, "pump2") # PH UP PUMP
self.last_action_time = 0
def __init__(self):
self.wheelBase = 0.24
self.encDist1000MM = 6586
self.lastEncoders = [0,0,0,0]
self.hardware = RoboHWRealMob()
self.motorController = MotorController(self.wheelBase,NUMBER_OF_MOTORS)
self.lastTimer = 0
self.lastMasterTime = 0
#self.lastCompass = 0
cmd_vel = Twist()
self.lastSpeeds = None
self.wheelSpeeds = None
self.update(cmd_vel)
def _setup_controllers(self, robot):
self.log.debug("Setup controllers")
self.left_wheel = MotorController(robot, type='wheel', id='LEFT')
self.right_wheel = MotorController(robot, type='wheel', id='RIGHT')
self.camera = VisionController(robot)
self.grabber = ServoController(robot, type='grabber')
self.arm = ServoController(robot, type='arm')
self._setup_switch_sources(robot)
#self.bump_FrL = RuggeduinoController(robot, type='bump', id='FRONT_L')
#self.bump_FrR = RuggeduinoController(robot, type='bump', id='FRONT_R')
#self.bump_BkL = RuggeduinoController(robot, type='bump', id='BACK_L')
#self.bump_BkR = RuggeduinoController(robot, type='bump', id='BACK_R')
self.token_sensL = RuggeduinoController(robot, type='sensor', id='TOKEN_L')
self.token_sensR = RuggeduinoController(robot, type='sensor', id='TOKEN_R')
def main():
logging.basicConfig(filename='/var/log/executioner/executioner.log',
level=logging.INFO,
format='%(asctime)s %(message)s')
#not handling signals because python can't reconcile them with multithreading. supposedly Py3.3 does though.
#signal.signal(signal.SIGINT, handle_signal)
#signal.signal(signal.SIGTERM, handle_signal)
sys.stdout = LoggerWriter(logging.info)
sys.stderr = LoggerWriter(logging.error)
MotorController.init()
SwitchController.init()
listener = Listener()
listener.start()
async def main():
application = make_app()
application.listen(80)
http_server = HTTPServer(application,
ssl_options = {
"certfile":"/etc/ssl/certs/valence.crt",
"keyfile":"/etc/ssl/certs/valence.key"
}
)
http_server.listen(443)
main_loop = IOLoop.current()
await mc = MotorController()
await mc.start()
def signalHandler(signum, frame):
print('[!] Caught termination signal: ', signum)
print('[*] Waiting for Valence to Close.')
waitUntilClosed(mc)
print('[*] Shutting down server.')
main_loop.stop()
sys.exit()
signal.signal(signal.SIGINT, signalHandler)
signal.signal(signal.SIGTERM, signalHandler)
signal.signal(signal.SIGHUP, signalHandler)
print ("[+] Valence Server started")
main_loop.start()
class MotorControllerNode(object):
def __init__(self):
rospy.init_node('motor_controller_node')
self.motor = MotorController()
sub = rospy.Subscriber('motor/speed_motors', Int32MultiArray, self.adjust_motor)
def adjust_motor(self, data):
speed_values = np.array(data.data)
right_speed = speed_values[0]
left_speed = speed_values[1]
self.motor.cmd_move_motors(right_speed, left_speed)
def stop(self):
self.motor.brake()
class MotorControllerNode(object):
def __init__(self):
rospy.init_node('motor_controller_node')
self.motor = MotorController()
sub = rospy.Subscriber('motor/speed_motors', Int32MultiArray,
self.adjust_motor)
def adjust_motor(self, data):
speed_values = np.array(data.data)
right_speed = speed_values[0]
left_speed = speed_values[1]
self.motor.cmd_move_motors(right_speed, left_speed)
def stop(self):
self.motor.brake()
def _setup_controllers(self, robot):
self.log.debug("Setup controllers")
self.left_wheel = MotorController(robot, type='wheel', id='LEFT')
self.right_wheel = MotorController(robot, type='wheel', id='RIGHT')
self.camera = VisionController(robot)
self.grabber = ServoController(robot, type='grabber')
self.arm = ServoController(robot, type='arm')
self._setup_switch_sources(robot)
#self.bump_FrL = RuggeduinoController(robot, type='bump', id='FRONT_L')
#self.bump_FrR = RuggeduinoController(robot, type='bump', id='FRONT_R')
#self.bump_BkL = RuggeduinoController(robot, type='bump', id='BACK_L')
#self.bump_BkR = RuggeduinoController(robot, type='bump', id='BACK_R')
self.token_sensL = RuggeduinoController(robot,
type='sensor',
id='TOKEN_L')
self.token_sensR = RuggeduinoController(robot,
type='sensor',
id='TOKEN_R')
def main():
try:
motor_controller = MotorController()
sonar = Sonar(18, 17)
controller = RobotController(sonar, motor_controller)
server = Server(controller, "192.168.99.11", 8080)
server.listen()
finally:
sonar.stop()
def main():
if len(sys.argv) != 5:
print('Usage: python main.py [left_motor_pin_number_forward] [right_motor_pin_number_forward] [left_motor_pin_number_reverse] [right_motor_pin_number_reverse] ')
else:
try:
global controller
controller = MotorController(int(sys.argv[1]), int(sys.argv[2]), int(sys.argv[3]), int(sys.argv[4]))
#controller = MockMotorController(int(sys.argv[1]), int(sys.argv[2]))
run(host='0.0.0.0', port=8000, debug=True)
except ValueError:
print('Unable to parse command line arguments.')
def start_motor():
mc = MotorController()
if mc.connect():
print "in connect"
mc.set_motor_speed(1700)
mc.sync_speed(5)
return mc
else:
return None
class MotorControllerTest(unittest.TestCase):
def setUp(self):
p = MockPwm()
self.p_output = MockPidOutput()
p_controller = MockPid(self.p_output)
q = MockQep()
self.m = MotorController(p, p_controller, self.p_output, q)
self.m_pid_disabled = MotorController(p, p_controller, self.p_output, q, False)
pass
def test_motor_controller_init(self):
self.assertTrue(isinstance(self.m, Thread))
self.assertTrue(isinstance(self.m.pwm, MockPwm))
self.assertTrue(isinstance(self.m.qep, MockQep))
self.assertTrue(isinstance(self.m.pid, MockPid))
# Test default values
self.assertEqual(self.m.pid_enabled, True)
def test_motor_controlling(self):
self.m.set_speed(1.0)
self.assertEqual(1.0, self.m.pid.set_point)
time.sleep(0.1)
self.assertEqual(self.p_output.value, 1.0)
self.m_pid_disabled.set_speed(1.0)
time.sleep(0.1)
self.assertEqual(1.0, self.m_pid_disabled.speed_to_command)
def setup_lid_controller(loop):
"""Sets up the devices for the lid controller"""
encoder_pair = RotaryEncoderPair(top_ccw=ROT_TOP_CCW,
top_cw=ROT_TOP_CW,
btm_ccw=ROT_BTM_CCW,
btm_cw=ROT_BTM_CW,
top_gear_ratio=TOP_GEAR_RATIO,
btm_gear_ratio=BTM_GEAR_RATIO)
encoder_pair.calibrate()
top_mc = MotorController(encoder_pair.encoder_top,
fwd_pin=MTR_1_FWD,
rev_pin=MTR_1_REV)
btm_mc = MotorController(encoder_pair.encoder_btm,
fwd_pin=MTR_2_FWD,
rev_pin=MTR_2_REV)
lid = LidController(top_mc, btm_mc)
lid_fut = asyncio.ensure_future(lid.close())
loop.run_until_complete(lid_fut)
return lid
def __init__(self):
"""
Instantiate the AUV object
"""
self.mc = MotorController()
# Connection to onboard radio.
try:
# Jack Silberman's radio
#self.radio = Radio('/dev/serial/by-id/usb-FTDI_FT230X_Basic_UART_DN038PQU-if00-port0')
# Yonder's radio
self.radio = Radio(
'/dev/serial/by-id/usb-Silicon_Labs_CP2102_USB_to_UART_Bridge_Controller_0001-if00-port0')
except Exception, e:
print("Radio not found. Exception is: ", e)
print("Exiting")
exit(1)
def set_status(newStatus):
#switch = SwitchSimulator(newStatus)
#switch.start()
if newStatus == DoorStatus.Closed:
MotorController.energize_down()
currentTime = time.time()
while DoorController.get_status() == DoorStatus.Open and time.time(
) < currentTime + Config.MaxDownDuration:
sleep(0.01)
else:
MotorController.energize_up()
sleep(Config.MaxUpDuration)
MotorController.halt()
from motor_controller import MotorController
from get_coordinates import GetCoordinates
from time import sleep
from datetime import datetime
__author__ = 'Karl'
motor_controller = MotorController()
'''
print("sending" + str(datetime.now().time()))
motor_controller.move_left_wheel(10)
motor_controller.move_right_wheel(10)
motor_controller.move_back_wheel(10)
print("back" + str(datetime.now().time()))
sleep(0.2)
motor_controller.move_left_wheel(10)
motor_controller.move_right_wheel(10)
motor_controller.move_back_wheel(10)
sleep(0.2)
motor_controller.move_left_wheel(10)
motor_controller.move_right_wheel(10)
motor_controller.move_back_wheel(10)
sleep(0.2)
motor_controller.move_left_wheel(-40)
motor_controller.move_right_wheel(40)
sleep(0.2)
motor_controller.move_left_wheel(-40)
motor_controller.move_right_wheel(40)
sleep(0.2)
motor_controller.move_left_wheel(-10)
motor_controller.move_right_wheel(-10)
#!/usr/bin/python
from motor_controller import MotorController
from fluid_dispenser import FluidDispenser
if __name__ == '__main__':
dispenser = FluidDispenser()
dispenser.AddMotor(MotorController(36, 38, 40), 0.8827, "pump1")
dispenser.AddMotor(MotorController(35, 37, 33), 0.8156, "pump2")
dispenser.AddMotor(MotorController(12, 16, 18), 0.7804, "pump3")
dispenser.AddMotor(MotorController(13, 15, 11), 0.8585, "pump4")
dispenser.DispenseFluid("pump1", 30)
dispenser.DispenseFluid("pump2", 30)
dispenser.DispenseFluid("pump3", 30)
#dispenser.DispenseFluid("pump4", 20)
def str_to_tuple(s):
point_str = s[1:-1].split(',')
point = (int(point_str[0].trim()), int(point_str[0].trim()))
return point
# Converts a move command to an Arduino Command
def convertMoveCmd(move):
x_move_str = ('xf' if move[0] > 0 else 'xb') + (str(abs(move[0])))
y_move_str = ('yf' if move[1] > 0 else 'yb') + (str(abs(move[1])))
return x_move_str, y_move_str
# initilize everything
curr_point = None
controller = MotorController(const.COM_PORT, const.ARDUINO_BPS)
controller.start()
sio = socketio.Client()
@sio.on(const.CON_ROUTE)
def on_connect():
print('connected')
# move handeler (TODO @rdiersing1: respond to the server)
@sio.on(const.MOVE_ROUTE)
def on_move(data):
global curr_point
# parse the move data recieved from the web-server
def main():
# Start motor controller
motor_ctrl = MotorController()
motor_ctrl.start()
def __init__(self):
rospy.init_node('motor_controller_node')
self.motor = MotorController()
sub = rospy.Subscriber('motor/speed_motors', Int32MultiArray,
self.adjust_motor)
class IOInterface(object):
def __init__(self, Robot):
self.log = logging.getLogger('Robot.IO')
self.log.info("Initializing")
# Aliases for English readability
self.drive = self.turn = self._move
try:
self._setup_controllers(Robot)
except:
self.log.critical("Could not set-up hardware controller")
raise
try:
self.running = True
tr = ThreadRegister(self)
#self.bump_thread = BumpThread()
self._bump_callback = lambda s: None
self._marker_callback = lambda m: None
#tr.add_thread(self.bump_thread)
#self.bump_thread.start()
except Exception as e:
self.log.critical("Unable to start threads")
self.log.exception(e)
raise
self.log.info("Done")
def _setup_controllers(self, robot):
self.log.debug("Setup controllers")
self.left_wheel = MotorController(robot, type='wheel', id='LEFT')
self.right_wheel = MotorController(robot, type='wheel', id='RIGHT')
self.camera = VisionController(robot)
self.grabber = ServoController(robot, type='grabber')
self.arm = ServoController(robot, type='arm')
self._setup_switch_sources(robot)
#self.bump_FrL = RuggeduinoController(robot, type='bump', id='FRONT_L')
#self.bump_FrR = RuggeduinoController(robot, type='bump', id='FRONT_R')
#self.bump_BkL = RuggeduinoController(robot, type='bump', id='BACK_L')
#self.bump_BkR = RuggeduinoController(robot, type='bump', id='BACK_R')
self.token_sensL = RuggeduinoController(robot, type='sensor', id='TOKEN_L')
self.token_sensR = RuggeduinoController(robot, type='sensor', id='TOKEN_R')
def _setup_switch_sources(self, robot):
#for id in ['FRONT_L', 'FRONT_R', 'BACK_L', 'BACK_R']:
# RuggeduinoController(robot, type='bump_src', id=id).write(False)
RuggeduinoController(robot, type='sensor_src', id='TOKEN_L').write(False)
RuggeduinoController(robot, type='sensor_src', id='TOKEN_R').write(False)
def _move(self, instruction):
if not isinstance(instruction, MoveInstruction):
raise TypeError("Invalid movement instruction")
self.log.debug("Do movement %s", instruction)
instruction.action(self)
def goto_marker(self, marker, speed, comparator=None, offset=0):
self.log.debug("goto marker %s %.1f", marker, speed)
if comparator is None or not hasattr(comparator, '__call__'):
comparator = lambda a, b: a.info.code == b.info.code
reached = False
blind_spot = 0.66
while True: # Alternatively, on each new find, call goto_marker within
horiz_dist = Marker(marker).horizontal_dist - offset
travel_dist = horiz_dist / 2 # set dist to travel half the measured
if marker.rot_y < 0:
Dir = Left # negative rot_y indicates a left turn
else:
Dir = Right # positive rot_y means right turn
if not reached: # if reached flag is true then should not rotate
degree = marker.rot_y
if degree < 1: degree += 0.7 # rotation < 1 is too subtle
if degree < 2: degree *= 2 # amplify rotation
self.turn(Dir(degree, Speed(speed / 3))) # face marker
self.drive(Distance(travel_dist, speed)) # drive half way
if reached: # set below
self.log.info("Reached marker")
break
potential_markers = [] # new markers of the same type stored here
for new_marker in self.get_markers():
if comparator(new_marker, marker):
potential_markers.append(new_marker) # append based on comparator
closest = self.get_closest_marker(potential_markers) # closest new
if closest is None: # there are no markers found
if travel_dist < blind_spot: # we have traveled beyond blind_spot
self.log.info("Distance to go in blind_spot (%.4f)",
travel_dist)
reached = True # break on next loop
else:
self.log.warning("Lost the marker")
break # lost the marker
else:
self.log.debug("Re-found marker")
c = Marker(closest)
expected = horiz_dist - min(0.1, travel_dist / 3.0)
if c.horizontal_dist >= expected:
self.log.warning("Did not travel far enough (%f >= %f) (%f, %f)" % (
c.horizontal_dist, expected, closest.dist, marker.dist))
return 0
marker = closest # the new marker to target
if not reached:
if marker.info.marker_type in MarkerFilter.include("TOKENS"):
reached = self.is_holding_token()
self.log.debug("Not reached, holding:%s", reached)
if reached: break
return reached
def face_marker(self, marker, speed):
self.log.debug("Going to face marker %s", marker)
m_orient = abs(marker.orientation.rot_y)
if m_orient < 0:
Dir1, Dir2 = Left, Right
self.log.debug("Going left then right")
else:
Dir1, Dir2 = Right, Left
self.log.debug("Going right then left")
self.turn(Dir1(abs(marker.rot_y), Speed(speed)))
self.drive(Distance((0.5 * marker.dist) /
abs(math.cos(math.radians(m_orient))), speed))
self.turn(Dir2(3 * m_orient, Speed(speed)))
def face_marker_2(self, marker, speed):
theta = abs(marker.orientation.rot_y)
length = marker.dist
if theta < 0:
Dir1, Dir2 = Left, Right
self.log.debug("Going left then right")
else:
Dir1, Dir2 = Right, Left
self.log.debug("Going right then left")
self.turn(Dir1(2 * theta, Speed(speed)))
x = (length / 2) * math.cos(math.radians(theta))
self.drive(Distance(x, speed))
angle = 2 * theta
self.turn(Dir2(angle, Speed(speed)))
self.drive(Distance(x, speed))
def face_marker_3(self, marker, speed):
self.log.debug("Going to face marker %s", marker)
m_orient = abs(marker.orientation.rot_y)
if m_orient < 0:
Dir1, Dir2 = Left, Right
self.log.debug("Going left then right")
else:
Dir1, Dir2 = Right, Left
self.log.debug("Going right then left")
self.turn(Dir1(abs(marker.rot_y), Speed(speed)))
self.drive(Distance((0.7 * marker.dist) /
abs(math.cos(math.radians(m_orient))), speed))
self.turn(Dir2(2 * m_orient, Speed(speed)))
def navigate_to_marker(self, marker, speed, comparator=None):
self.log.debug("navigating to marker")
self.face_marker_3(marker, speed) # face the marker
markers = self.get_markers() # rescan for the marker
old_marker = marker
for new_marker in markers:
if new_marker.info.code == marker.info.code:
marker = new_marker
self.log.debug("Found marker, continuing")
break
if marker is old_marker: # the marker got lost
self.log.warning("Could not find marker")
return False
if self.goto_marker(marker, speed, comparator):
return True
markers = self.get_markers() # rescan for the marker
for new_marker in markers:
if new_marker.info.code == marker.info.code:
marker = new_marker
self.log.debug("Found marker again, continuing")
break
if marker is old_marker: # the marker got lost
self.log.warning("Could not find marker")
return False
self.navigate_to_marker(marker, speed, comparator)
def is_holding_token(self):
return self.token_sensL.read() or self.token_sensR.read()
def move_arm(self, direction, delay=False):
if direction not in [UP, DOWN, MIDDLE]:
raise TypeError("Invalid arm movement")
if direction == UP:
angle = self.arm.MIN
elif direction == DOWN:
angle = self.arm.MAX
elif direction == MIDDLE:
angle = self.arm.MAX / 2
self.log.debug("Move arm %s (angle = %d)", direction, angle)
self.arm.set_angle(angle)
if delay:
self.wait(0.5)
def open_grabber(self, delay=False):
self.log.debug("OPEN grabber (angle = %d)", self.grabber.MIN)
self.grabber.set_angle(self.grabber.MIN)
if delay:
self.wait(0.7)
def close_grabber(self, delay=False):
self.log.debug("CLOSE grabber (angle = %d)", self.grabber.MAX)
self.grabber.set_angle(self.grabber.MAX)
if delay:
self.wait(0.7)
def get_markers(self, _filter=None):
markers = self.camera.find_markers()
self._marker_callback(markers)
self.log.debug("get_markers %s", str(self.camera.fmt_markers(markers)))
if _filter is not None:
markers = filter(lambda m: m.info.marker_type in _filter, markers)
return markers
def get_closest_marker(self, markers):
self.log.debug("Get closest marker to robot")
return self.camera.get_closest(markers)
def get_closest_marker_rotation(self, markers, degree=0):
self.log.debug("Get closest marker from degree (%f)", degree)
return self.camera.get_rotation_nearest(markers, degree)
def _stop_operation(self, filter=None):
self.log.info("Stopping current action")
try:
self.right_wheel.stop()
self.left_wheel.stop()
except Exception as e:
self.log.exception(e)
raise StateInterrupt('stop', 'operation.stop')
def wait(self, seconds):
self.log.debug("Wait %.4f seconds", seconds)
time.sleep(seconds)
def set_bump_handler(self, callback):
self._bump_callback = callback
def set_marker_handler(self, callback):
self._marker_callback = callback
def bumped(self, sensor):
if sensor is self.bump_FrL:
self._bump_callback('Front.Left')
elif sensor is self.bump_FrR:
self._bump_callback('Front.Right')
elif sensor is self.bump_BkL:
self._bump_callback('Back.Left')
elif sensor is self.bump_BkR:
self._bump_callback('Back.Right')
def create_token_obj(self, markers):
return Token(markers)
def stop(self):
self.log.info("Stopping all communications")
self.running = False
class TankController():
""" タンクコントローラー """
def __init__(self):
self.mc = MotorController()
self.gt = getTargetThread()
self.excutor = concurrent.futures.ThreadPoolExecutor(max_workers=1)
self.excutor.submit(self.turret_move)
self.lock = threading.Lock()
#
# 以下戦車の動作
#
def tank_go_forward(self):
# 前進
print("exec: ", sys._getframe().f_code.co_name)
self.mc.forwardMotor1A(50)
self.mc.forwardMotor1B(50)
def tank_go_backward(self):
# 後退
print("exec: ", sys._getframe().f_code.co_name)
self.mc.reverseMotor1A(50)
self.mc.reverseMotor1B(50)
def tank_stop(self):
# 停止
print("exec: ", sys._getframe().f_code.co_name)
self.mc.breakMotor1A()
self.mc.breakMotor1B()
def tank_turn_right(self):
# 右旋回
print("exec: ", sys._getframe().f_code.co_name)
self.mc.reverseMotor1A(50)
self.mc.forwardMotor1B(50)
def tank_turn_left(self):
# 左旋回
print("exec: ", sys._getframe().f_code.co_name)
self.mc.forwardMotor1A(50)
self.mc.reverseMotor1B(50)
#
# 以下砲台
#
def turret_follow(self):
# 標的追尾開始
print("exec: ", sys._getframe().f_code.co_name)
self.gt.FollowStart()
def turret_unfollow(self):
# 標的追尾停止
print("exec: ", sys._getframe().f_code.co_name)
self.gt.FollowStop()
def turret_shoot(self):
# BB弾発射
print("exec: ", sys._getframe().f_code.co_name)
self.mc.forwardMotor2A(100)
time.sleep(5)
self.mc.breakMotor2A()
def turret_move(self):
# 砲台の旋回
# getTargetThreadから標的の位置情報をもらって砲台を旋回させる。
while True:
TurretLeft = 0
TurretRight = 0
TurretLeft = self.gt.GetInfoTurretTurnLeft()
TurretRight = self.gt.GetInfoTurretTurnRight()
# print('turret left %1.2f right %1.2f' %(TurretLeft,TurretRight))
# 時間計測開始
start = time.time()
print('tank_ctrl:logtime1 start')
if TurretLeft != 0:
self.lock.acquire()
self.mc.forwardMotor2B(40)
# print("turret:turnning left")
time.sleep(TurretLeft)
self.mc.breakMotor2B()
self.gt.ClearInfoTurretTurnLeft()
self.lock.release()
elif TurretRight != 0:
self.lock.acquire()
self.mc.reverseMotor2B(40)
# print("turret:turnning Right")
time.sleep(TurretRight)
self.mc.breakMotor2B()
self.gt.ClearInfoTurretTurnRight()
self.lock.release()
else:
# print("turret:No Action")
time.sleep(0.5)
time2 = time.time() - start
print('tank_ctrl:logtime2 %1.2f sec' % (time2))
class RoboconfigCar:
def __init__(self):
self.wheelBase = 0.235
self.encDist1000MM = 13133
self.lastEncoders = [0, 0, 0, 0]
self.hardware = RoboHWRealCar()
self.motorController = MotorController(self.wheelBase,
NUMBER_OF_MOTORS)
self.lastTimer = 0
self.lastMasterTime = 0
#self.lastCompass = 0
cmd_vel = Twist()
self.lastSpeeds = None
self.wheelSpeeds = None
self.lastCompass = 0
self.redSwitch = False
self.update(cmd_vel)
def update(self, cmd_vel):
#send cmd to robot and return current status
if self.hardware.getReboot():
#hardware was rebooted
self.hardware.setReboot(False)
cmd_vel.linear.x = 0
cmd_vel.angular.z = 0
if self.redSwitch:
cmd_vel.linear.x = 0
cmd_vel.angular.z = 0
newWheelSpeed = self.motorController.update(cmd_vel, self.wheelSpeeds)
speeds = self.convertSpeedToRobot(newWheelSpeed)
direction = self.convertDirectionToRobot(cmd_vel)
executeAt = 0xFFFF & self.convertTimeToRobot(self.lastMasterTime +
TICK_DELAY)
timer, encoders, self.redSwitch, compass = self.hardware.synchronize(
executeAt, speeds.frontLeft, direction)
#############
#status.setRedSwitch(hwStatus.getRedSwitch()) #how to represent red switch in ROS?
actualTickDelay = self.convertTimeFromRobot(0xFFFF
& (timer - self.lastTimer))
speeds = self.convertSpeedFromRobot(encoders, self.lastEncoders,
actualTickDelay)
speeds.ang = self.convertAngularSpeedFromRobot(
self.convertCompassFromRobot(compass), actualTickDelay)
#newWheelSpeed = self.convertWheelSpeedFromRobot(hwStatus.getEncoders(),self.lastEncoders,actualTickDelay)
#status.setWheelSpeed(newWheelSpeed)
masterTime = self.lastMasterTime + actualTickDelay
heading = self.convertCompassFromRobot(compass)
#print("Compass:"******"Compass not working")
angleInRad = -math.pi / 180 * float(compass - COMPASS_VALUE_ZERO) / 10
normalized = robomath.normalizeAngle(angleInRad)
return normalized
def convertSpeedFromRobot(self, encoders, lastEncoders, time):
encDiff = [0, 0, 0, 0]
#print "Encoders=",encoders
for i in range(0, len(encoders)):
encDiff[i] = encoders[i] - lastEncoders[i]
unpacked = struct.unpack(
"h", "%c%c" % (encDiff[i] % 256, 0xFF & (encDiff[i] >> 8)))
encDiff[i] = float(unpacked[0])
#print "EncDiff=",encDiff, " Time=",time
self.wheelSpeeds = Object
self.wheelSpeeds.frontRight = 0
self.wheelSpeeds.rearRight = 0
self.wheelSpeeds.frontLeft = (-encDiff[0]) / self.encDist1000MM / time
self.wheelSpeeds.rearLeft = 0
speeds = Object
speeds.fwd = (-encDiff[0]) / self.encDist1000MM / time
speeds.ang = 0
return speeds
def convertSpeedToRobot(self, speed):
speeds = Object
speeds.frontLeft = 128 + speed.frontLeft * FORWARD_FACTOR
if speeds.frontLeft > 170:
speeds.frontLeft = 170
if speeds.frontLeft < 90:
speeds.frontLeft = 90
return speeds
def convertTimeToRobot(self, seconds):
return int(seconds * TIMER_TICKS_PER_SECOND)
def convertTimeFromRobot(self, timer):
return float(timer) / TIMER_TICKS_PER_SECOND
def convertDirectionToRobot(self, cmd):
if cmd.linear.x >= 0:
direction = cmd.angular.z
else:
direction = -cmd.angular.z
newDirection = ANGULAR_CENTER - direction * ANGULAR_FACTOR
if newDirection > 205:
newDirection = 205
if newDirection < 50:
newDirection = 50
return newDirection
class EarthRoverChassis:
def __init__(self):
rospy.init_node("earth_rover_chassis",
# log_level=rospy.DEBUG
)
# rospy.on_shutdown(self.shutdown)
# robot dimensions
self.wheel_radius = rospy.get_param("~wheel_radius_meters", 1.0)
self.wheel_distance = rospy.get_param("~wheel_distance_meters", 1.0)
self.ticks_per_rotation = rospy.get_param("~ticks_per_rotation", 1.0)
# speed parameters
self.left_min_speed_meters_per_s = rospy.get_param(
"~left_min_speed_meters_per_s", -1.0)
self.left_max_speed_meters_per_s = rospy.get_param(
"~left_max_speed_meters_per_s", 1.0)
self.right_min_speed_meters_per_s = rospy.get_param(
"~right_min_speed_meters_per_s", -1.0)
self.right_max_speed_meters_per_s = rospy.get_param(
"~right_max_speed_meters_per_s", 1.0)
self.left_min_usable_command = rospy.get_param(
"~left_min_usable_command", 0.10)
self.right_min_usable_command = rospy.get_param(
"~right_min_usable_command", 0.10)
self.min_measurable_speed = rospy.get_param("~min_measurable_speed",
0.0001)
# cmd_vel parameters
# self.min_cmd_lin_vel = rospy.get_param("~min_cmd_lin_vel", 0.0)
# self.min_cmd_ang_vel = rospy.get_param("~min_cmd_lin_vel", 0.0)
# PID parameters
self.enable_pid = rospy.get_param("~enable_pid", True)
self.min_command = rospy.get_param("~min_command", -1.0)
self.max_command = rospy.get_param("~max_command", 1.0)
self.kp = rospy.get_param("~kp", 1.0)
self.ki = rospy.get_param("~ki", 0.0)
self.kd = rospy.get_param("~kd", 0.0)
self.speed_smooth_k = rospy.get_param("~speed_smooth_k", 1.0)
# self.output_deadzone = rospy.get_param("~output_deadzone", 0.1)
self.output_noise = rospy.get_param("~output_noise", 0.01)
# TF parameters
self.child_frame = rospy.get_param("~odom_child_frame", "base_link")
self.parent_frame = rospy.get_param("~odom_parent_frame", "odom")
self.tf_broadcaster = tf.TransformBroadcaster()
# Ultrasonic parameters
self.stopping_distances_cm = rospy.get_param("~stopping_distances_cm",
None)
self.easing_offset_cm = rospy.get_param("~easing_offset_cm", 5.0)
self.easing_offset_cm = rospy.get_param("~min_tracking_lin_vel", 0.07)
self.easing_offset_cm = rospy.get_param("~min_tracking_ang_vel", 5.0)
self.front_sensors = rospy.get_param("~front_sensors", None)
self.right_sensors = rospy.get_param("~right_sensors", None)
self.left_sensors = rospy.get_param("~left_sensors", None)
self.back_sensors = rospy.get_param("~back_sensors", None)
assert self.stopping_distances_cm is not None
assert self.front_sensors is not None
assert self.right_sensors is not None
assert self.left_sensors is not None
assert self.back_sensors is not None
# Motor controller objects
left_motor_info = MotorInfo(
"left motor", self.kp, self.ki, self.kd, self.speed_smooth_k,
self.wheel_radius, self.ticks_per_rotation,
self.left_min_usable_command, self.left_min_speed_meters_per_s,
self.left_max_speed_meters_per_s, self.min_command,
self.max_command, self.output_noise, self.min_measurable_speed)
right_motor_info = MotorInfo(
"right motor", self.kp, self.ki, self.kd, self.speed_smooth_k,
self.wheel_radius, self.ticks_per_rotation,
self.right_min_usable_command, self.right_min_speed_meters_per_s,
self.right_max_speed_meters_per_s, self.min_command,
self.max_command, self.output_noise, self.min_measurable_speed)
self.left_motor = MotorController(left_motor_info)
self.right_motor = MotorController(right_motor_info)
self.linear_speed_mps = 0.0
self.rotational_speed_mps = 0.0
# Ultrasonic state tracking objects
self.num_sensors = len(self.stopping_distances_cm)
self.sensor_directions = [[Direction.FRONT, self.front_sensors],
[Direction.BACK, self.back_sensors],
[Direction.LEFT, self.left_sensors],
[Direction.RIGHT, self.right_sensors]]
self.trackers_indexed = [None for _ in range(self.num_sensors)]
self.trackers_directed = {
Direction.FRONT: TrackerCollection(),
Direction.BACK: TrackerCollection(),
Direction.LEFT: TrackerCollection(),
Direction.RIGHT: TrackerCollection(),
}
for direction, sensor_indices in self.sensor_directions:
for sensor_index in sensor_indices:
stop_dist = self.stopping_distances_cm[sensor_index]
ease_dist = stop_dist + self.easing_offset_cm
tracker = UltrasonicTracker(stop_dist, ease_dist)
self.trackers_indexed[sensor_index] = tracker
self.trackers_directed[direction].append(tracker)
# prev state tracking
self.prev_left_output = 0.0
self.prev_right_output = 0.0
# odometry state
self.odom_x = 0.0
self.odom_y = 0.0
self.odom_t = 0.0
self.odom_vx = 0.0
self.odom_vy = 0.0
self.odom_vt = 0.0
# Odometry message
self.odom_msg = Odometry()
self.odom_msg.header.frame_id = self.parent_frame
self.odom_msg.child_frame_id = self.child_frame
# Subscribers
self.twist_sub = rospy.Subscriber("cmd_vel",
Twist,
self.twist_callback,
queue_size=5)
self.left_encoder_sub = rospy.Subscriber("left/left_encoder/ticks",
Int64,
self.left_encoder_callback,
queue_size=50)
self.right_encoder_sub = rospy.Subscriber("right/right_encoder/ticks",
Int64,
self.right_encoder_callback,
queue_size=50)
self.ultrasonic_sub = rospy.Subscriber(
"earth_rover_teensy_bridge/ultrasonic",
Float32MultiArray,
self.ultrasonic_callback,
queue_size=15)
# Publishers
self.left_dist_pub = rospy.Publisher("left/left_encoder/distance",
Float64,
queue_size=5)
self.right_dist_pub = rospy.Publisher("right/right_encoder/distance",
Float64,
queue_size=5)
self.left_speed_pub = rospy.Publisher("left/left_encoder/speed",
Float64,
queue_size=5)
self.right_speed_pub = rospy.Publisher("right/right_encoder/speed",
Float64,
queue_size=5)
self.left_command_pub = rospy.Publisher("left/command_speed",
Float64,
queue_size=5)
self.right_command_pub = rospy.Publisher("right/command_speed",
Float64,
queue_size=5)
self.odom_pub = rospy.Publisher("odom", Odometry, queue_size=5)
# Services
self.tuning_service = rospy.Service("tune_motor_pid", TuneMotorPID,
self.tune_motor_pid)
def tune_motor_pid(self, request):
kp = request.kp
ki = request.ki
kd = request.kd
rospy.loginfo("Updated PID constants to kp=%s, ki=%s, kd=%s" %
(kp, ki, kd))
self.left_motor.tune_pid(kp, ki, kd)
self.right_motor.tune_pid(kp, ki, kd)
return TuneMotorPIDResponse()
def twist_callback(self, twist_msg):
self.linear_speed_mps = twist_msg.linear.x # m/s
angular_speed_radps = twist_msg.angular.z # rad/s
# if abs(self.linear_speed_mps) < self.min_cmd_lin_vel and self.linear_speed_mps != 0.0:
# # assign linear_speed_mps the minimum speed with direction sign applied
# self.linear_speed_mps = math.copysign(self.min_cmd_lin_vel, self.linear_speed_mps)
# if abs(angular_speed_radps) < self.min_cmd_ang_vel and angular_speed_radps != 0.0:
# angular_speed_radps = math.copysign(self.min_cmd_ang_vel, angular_speed_radps)
# arc = angle * radius
# rotation speed at the wheels
self.rotational_speed_mps = angular_speed_radps * self.wheel_distance / 2
def left_encoder_callback(self, enc_msg):
self.left_motor.enc_tick = -enc_msg.data
dt = self.left_motor.get_dt(rospy.Time.now())
left_output = self.left_motor.update(dt)
if left_output is not None:
self.command_left_motor(left_output)
def right_encoder_callback(self, enc_msg):
self.right_motor.enc_tick = enc_msg.data
dt = self.right_motor.get_dt(rospy.Time.now())
right_output = self.right_motor.update(dt)
if right_output is not None:
self.command_right_motor(right_output)
def ultrasonic_callback(self, ultrasonic_msg):
for index, distance in enumerate(ultrasonic_msg.data):
self.trackers_indexed[index].update(distance)
def scale_targets(self, lin_vel, ang_vel):
if lin_vel > 0:
lin_vel = self.trackers_directed[Direction.FRONT].scale_v(lin_vel)
elif lin_vel < 0:
lin_vel = self.trackers_directed[Direction.BACK].scale_v(lin_vel)
elif ang_vel < 0: # if moving right, check the left side
ang_vel = self.trackers_directed[Direction.LEFT].scale_v(ang_vel)
elif ang_vel > 0: # if moving left, check the right side
ang_vel = self.trackers_directed[Direction.RIGHT].scale_v(ang_vel)
return lin_vel, ang_vel
def run(self):
clock_rate = rospy.Rate(30)
prev_ultrasonic_report_t = rospy.Time.now()
while not rospy.is_shutdown():
self.compute_odometry(self.left_motor.get_delta_dist(),
self.right_motor.get_delta_dist(),
self.left_motor.get_speed(),
self.right_motor.get_speed())
self.publish_chassis_data()
linear_speed_mps, rotational_speed_mps = self.scale_targets(
self.linear_speed_mps, self.rotational_speed_mps)
if linear_speed_mps != self.linear_speed_mps or rotational_speed_mps != self.rotational_speed_mps:
current_time = rospy.Time.now()
if current_time - prev_ultrasonic_report_t > rospy.Duration(
0.5):
prev_ultrasonic_report_t = current_time
print "Scaling speed based on distance sensors"
for direction in self.trackers_directed:
print "\t%s, dist: %s, scale: %s" % (
Direction.name(direction),
self.trackers_directed[direction].get_dists(),
self.trackers_directed[direction].get_scale())
self.left_motor.set_target(linear_speed_mps - rotational_speed_mps)
self.right_motor.set_target(linear_speed_mps +
rotational_speed_mps)
clock_rate.sleep()
def command_left_motor(self, command):
if self.enable_pid and self.prev_left_output != command:
self.left_command_pub.publish(command)
self.prev_left_output = command
def command_right_motor(self, command):
if self.enable_pid and self.prev_right_output != command:
self.right_command_pub.publish(command)
self.prev_right_output = command
def publish_chassis_data(self):
self.left_dist_pub.publish(self.left_motor.get_dist())
self.right_dist_pub.publish(self.right_motor.get_dist())
self.left_speed_pub.publish(self.left_motor.get_speed())
self.right_speed_pub.publish(self.right_motor.get_speed())
odom_quaternion = tf.transformations.quaternion_from_euler(
0.0, 0.0, self.odom_t)
self.tf_broadcaster.sendTransform((self.odom_x, self.odom_y, 0.0),
odom_quaternion, rospy.Time.now(),
self.child_frame, self.parent_frame)
self.odom_msg.header.stamp = rospy.Time.now()
self.odom_msg.pose.pose.position.x = self.odom_x
self.odom_msg.pose.pose.position.y = self.odom_y
self.odom_msg.pose.pose.position.z = 0.0
self.odom_msg.pose.pose.orientation.x = odom_quaternion[0]
self.odom_msg.pose.pose.orientation.y = odom_quaternion[1]
self.odom_msg.pose.pose.orientation.z = odom_quaternion[2]
self.odom_msg.pose.pose.orientation.w = odom_quaternion[3]
self.odom_msg.twist.twist.linear.x = self.odom_vx
self.odom_msg.twist.twist.linear.y = self.odom_vy
self.odom_msg.twist.twist.linear.z = 0.0
self.odom_msg.twist.twist.angular.x = 0.0
self.odom_msg.twist.twist.angular.y = 0.0
self.odom_msg.twist.twist.angular.z = self.odom_vt
self.odom_pub.publish(self.odom_msg)
def compute_odometry(self, delta_left, delta_right, left_speed,
right_speed):
delta_dist = (delta_right + delta_left) / 2
# angle = arc / radius
delta_angle = (delta_right - delta_left) / self.wheel_distance
self.odom_t += delta_angle
dx = delta_dist * math.cos(self.odom_t)
dy = delta_dist * math.sin(self.odom_t)
self.odom_x += dx
self.odom_y += dy
speed = (right_speed + left_speed) / 2
self.odom_vx = speed * math.cos(self.odom_t)
self.odom_vy = speed * math.sin(self.odom_t)
self.odom_vt = (right_speed - left_speed) / (self.wheel_distance / 2)
def serv1(port):
try:
from servo_controller import ServoController
servo = ServoController()
except Exception as e:
log.debug("Failed to use ServoController: {0}".format(e))
from standins import StandinServoController
servo = StandinServoController()
try:
from motor_controller import MotorController
motors = MotorController()
except Exception as e:
log.debug("failed to use MotorController: {0}".format(e))
from standins import StandinMotorController, StandinMotorDefinition
motors = StandinMotorController()
motors.defineMotor("right", (14, 15), servo, 0)
motors.defineMotor("left", (17, 18), servo, 1)
motors.setSignedPWM("left", 0)
motors.setSignedPWM("right", 0)
time.sleep(0.1)
motors.setDirection("left", "B")
time.sleep(0.1)
motors.setDirection("left", "A")
time.sleep(0.1)
motors.setDirection("right", "B")
time.sleep(0.1)
motors.setDirection("right", "A")
time.sleep(0.1)
addr = ('', port)
server = HTTPServer(addr, PyServ)
server.servo = servo
server.motors = motors
server.autodriver = None
os.chdir("content")
log.debug("Serving on port {0} in {1}".format(port, os.getcwd()))
server.serve_forever()
class DriveController:
"""
Tracks the robot's position via dead reckoning and handles the motor's
kinematics.
The robot uses skid steering which has simple forward and reverse
kinematics. DriveController is used to bridge the abstraction between a
point model of the robot and the robot's actual layout."""
def __init__(self):
self._left_motor_controller = MotorController(
0, left_pins, left_motor_pid_constants, 0)
self._right_motor_controller = MotorController(
1, right_pins, right_motor_pid_constants, 1)
self._speed_filter = IrregularDoubleExponentialFilter(
*robot_speed_smoothing_factors)
self.pos_heading = (ZERO_POS, 0.0)
self.robot_angle = 0.0
self.robot_speed = 0.0
def read_encoders(self, time_elapsed):
"""Reads the encoders and updates the robot's position, heading,
and wheel velocities"""
# get movement of left and right wheels
left_dist_traveled = self._left_motor_controller.read_encoder(
time_elapsed)
right_dist_traveled = self._right_motor_controller.read_encoder(
time_elapsed)
dist, angle = forward_kin(left_dist_traveled, right_dist_traveled)
# dead reckon pos and angle
self.robot_angle = correct_angle(self.robot_angle + angle)
# self.dead_reckon(dist, angle)
self.robot_speed = self._speed_filter.filter(dist / time_elapsed,
time_elapsed)
def dead_reckon(self, dist, angle):
"""This method was tested in a previous version of the robot's software
and it tracked the position of the robot very well when the tires didn't
slip. Regrettably there wasn't enough time to integrate position system
with the navigation system, so keeping track of the robot's position
wouldn't yield any advantages.
This method uses the point and shoot method. The point and shoot method
estimates movement as a turn followed by a movement forward. Obviously
the robot doesn't move like this but it is a good estimation when the
total turn is small. If the turn is too large (>small_angle), then the
movement is split into two consecutive point and shoots. This is done
recursively while the angle is too large."""
if abs(angle) <= small_angle:
pos = self.pos_heading[0].pos_from_dist_angle(dist, angle)
theta = correct_angle(self.pos_heading[1] + angle)
self.pos_heading = (pos, theta)
return
self.dead_reckon(dist / 2, angle / 2)
self.dead_reckon(dist / 2, angle / 2)
def update_motors(self, forward_vel, angular_vel, time_elapsed):
# Transform forward and angular to left and right and send to the
# motor controllers
left_target, right_target = reverse_kin(forward_vel, angular_vel)
self._left_motor_controller.adjust_motor_speed(left_target,
time_elapsed)
self._right_motor_controller.adjust_motor_speed(
right_target, time_elapsed)
def check_current(self):
# the current sensor don't work so this is never called in this version
self._left_motor_controller.current_sensor.check_current()
self._right_motor_controller.current_sensor.check_current()
def reset(self):
self._left_motor_controller.reset()
self._right_motor_controller.reset()
self.pos_heading = (ZERO_POS, 0.0)
self.robot_angle = 0.0
self.robot_speed = 0.0
def stop(self):
self._left_motor_controller.stop()
self._right_motor_controller.stop()
#!/usr/bin/env python3
import pygame
import time
import os
import sys
from motor_controller import MotorController
from controller import Joystick
if __name__ == '__main__':
os.environ["SDL_VIDEODRIVER"] = "dummy"
pygame.init()
pygame.joystick.init()
motoCon = MotorController()
# if controllerJoystickIsUp:
# motoCon.forward()
with Joystick(0) as controller:
while True:
controller.update()
inputs = controller.get_all_vals()
if inputs['button_0']:
motoCon.forward()
elif inputs['button_1']:
motoCon.stop()
elif inputs['button_2']:
motoCon.reverse()
motoCon.set_steering(inputs['axis_0'])
motoCon.print_stat()
time.sleep(0.1)
timing["refresh_count"] = timing["refresh_count"] + 1
time.sleep(0.0001)
magnet.close()
else:
while end_time > timing["last_update"]:
timing["last_update"] = time.time()
self.strip.clear()
self.strip.close()
def stop(self):
self.strip.clear()
self.strip.close()
if __name__ == "__main__":
from motor_controller import MotorController
mc = MotorController()
mc.connect()
mc.set_motor_speed(1650)
mc.sync_speed(5)
fan = PovFan()
fan.load_sequence("endless", 1)
fan.play(400)
mc.stop_motor()
MotorController.stop()
if __name__ == "__main__":
try:
application = make_app()
application.listen(80)
http_server = HTTPServer(application,
ssl_options = {
"certfile":"/etc/ssl/certs/valence.crt",
"keyfile":"/etc/ssl/certs/valence.key"
}
)
http_server.listen(443)
main_loop = IOLoop.current()
mc = MotorController()
mc.start()
def signalHandler(signum, frame):
print('[!] Caught termination signal: ', signum)
print('[*] Waiting for Valence to Close.')
waitUntilClosed(mc)
print('[*] Shutting down server.')
main_loop.stop()
sys.exit()
signal.signal(signal.SIGINT, signalHandler)
signal.signal(signal.SIGTERM, signalHandler)
signal.signal(signal.SIGHUP, signalHandler)
print ("[+] Valence Server started")
main_loop.start()
def __init__(self):
rospy.init_node('motor_controller_node')
self.motor = MotorController()
sub = rospy.Subscriber('motor/speed_motors', Int32MultiArray, self.adjust_motor)
import time
from motor_controller import MotorController
motor_controller = MotorController()
for x in range(5):
if not motor_controller.is_working():
motor_controller.start_motor()
time.sleep(5)
import socket
# get outward facing ip (credit: someone on SO)
host = [
l for l in (
[
ip for ip in socket.gethostbyname_ex(socket.gethostname())[2]
if not ip.startswith("127.")
][:1],
[[(s.connect(('8.8.8.8', 53)), s.getsockname()[0], s.close())
for s in [socket.socket(socket.AF_INET, socket.SOCK_DGRAM)]][0][1]])
if l
][0][0]
port = 9939
mc = MotorController()
def movement_handler(command, args):
print("Command is: %s" % str(command))
print("Args are: %s" % str(args))
if command == Commands.FORWARD:
mc.forward(args['speed'])
elif command == Commands.BACKWARD:
mc.backward(args['speed'])
elif command == Commands.LEFT:
mc.turnLeft()
elif command == Commands.RIGHT:
mc.turnRight()
elif command == Commands.STOP:
def __init__(self):
rospy.init_node("earth_rover_chassis",
# log_level=rospy.DEBUG
)
# rospy.on_shutdown(self.shutdown)
# robot dimensions
self.wheel_radius = rospy.get_param("~wheel_radius_meters", 1.0)
self.wheel_distance = rospy.get_param("~wheel_distance_meters", 1.0)
self.ticks_per_rotation = rospy.get_param("~ticks_per_rotation", 1.0)
# speed parameters
self.left_min_speed_meters_per_s = rospy.get_param(
"~left_min_speed_meters_per_s", -1.0)
self.left_max_speed_meters_per_s = rospy.get_param(
"~left_max_speed_meters_per_s", 1.0)
self.right_min_speed_meters_per_s = rospy.get_param(
"~right_min_speed_meters_per_s", -1.0)
self.right_max_speed_meters_per_s = rospy.get_param(
"~right_max_speed_meters_per_s", 1.0)
self.left_min_usable_command = rospy.get_param(
"~left_min_usable_command", 0.10)
self.right_min_usable_command = rospy.get_param(
"~right_min_usable_command", 0.10)
self.min_measurable_speed = rospy.get_param("~min_measurable_speed",
0.0001)
# cmd_vel parameters
# self.min_cmd_lin_vel = rospy.get_param("~min_cmd_lin_vel", 0.0)
# self.min_cmd_ang_vel = rospy.get_param("~min_cmd_lin_vel", 0.0)
# PID parameters
self.enable_pid = rospy.get_param("~enable_pid", True)
self.min_command = rospy.get_param("~min_command", -1.0)
self.max_command = rospy.get_param("~max_command", 1.0)
self.kp = rospy.get_param("~kp", 1.0)
self.ki = rospy.get_param("~ki", 0.0)
self.kd = rospy.get_param("~kd", 0.0)
self.speed_smooth_k = rospy.get_param("~speed_smooth_k", 1.0)
# self.output_deadzone = rospy.get_param("~output_deadzone", 0.1)
self.output_noise = rospy.get_param("~output_noise", 0.01)
# TF parameters
self.child_frame = rospy.get_param("~odom_child_frame", "base_link")
self.parent_frame = rospy.get_param("~odom_parent_frame", "odom")
self.tf_broadcaster = tf.TransformBroadcaster()
# Ultrasonic parameters
self.stopping_distances_cm = rospy.get_param("~stopping_distances_cm",
None)
self.easing_offset_cm = rospy.get_param("~easing_offset_cm", 5.0)
self.easing_offset_cm = rospy.get_param("~min_tracking_lin_vel", 0.07)
self.easing_offset_cm = rospy.get_param("~min_tracking_ang_vel", 5.0)
self.front_sensors = rospy.get_param("~front_sensors", None)
self.right_sensors = rospy.get_param("~right_sensors", None)
self.left_sensors = rospy.get_param("~left_sensors", None)
self.back_sensors = rospy.get_param("~back_sensors", None)
assert self.stopping_distances_cm is not None
assert self.front_sensors is not None
assert self.right_sensors is not None
assert self.left_sensors is not None
assert self.back_sensors is not None
# Motor controller objects
left_motor_info = MotorInfo(
"left motor", self.kp, self.ki, self.kd, self.speed_smooth_k,
self.wheel_radius, self.ticks_per_rotation,
self.left_min_usable_command, self.left_min_speed_meters_per_s,
self.left_max_speed_meters_per_s, self.min_command,
self.max_command, self.output_noise, self.min_measurable_speed)
right_motor_info = MotorInfo(
"right motor", self.kp, self.ki, self.kd, self.speed_smooth_k,
self.wheel_radius, self.ticks_per_rotation,
self.right_min_usable_command, self.right_min_speed_meters_per_s,
self.right_max_speed_meters_per_s, self.min_command,
self.max_command, self.output_noise, self.min_measurable_speed)
self.left_motor = MotorController(left_motor_info)
self.right_motor = MotorController(right_motor_info)
self.linear_speed_mps = 0.0
self.rotational_speed_mps = 0.0
# Ultrasonic state tracking objects
self.num_sensors = len(self.stopping_distances_cm)
self.sensor_directions = [[Direction.FRONT, self.front_sensors],
[Direction.BACK, self.back_sensors],
[Direction.LEFT, self.left_sensors],
[Direction.RIGHT, self.right_sensors]]
self.trackers_indexed = [None for _ in range(self.num_sensors)]
self.trackers_directed = {
Direction.FRONT: TrackerCollection(),
Direction.BACK: TrackerCollection(),
Direction.LEFT: TrackerCollection(),
Direction.RIGHT: TrackerCollection(),
}
for direction, sensor_indices in self.sensor_directions:
for sensor_index in sensor_indices:
stop_dist = self.stopping_distances_cm[sensor_index]
ease_dist = stop_dist + self.easing_offset_cm
tracker = UltrasonicTracker(stop_dist, ease_dist)
self.trackers_indexed[sensor_index] = tracker
self.trackers_directed[direction].append(tracker)
# prev state tracking
self.prev_left_output = 0.0
self.prev_right_output = 0.0
# odometry state
self.odom_x = 0.0
self.odom_y = 0.0
self.odom_t = 0.0
self.odom_vx = 0.0
self.odom_vy = 0.0
self.odom_vt = 0.0
# Odometry message
self.odom_msg = Odometry()
self.odom_msg.header.frame_id = self.parent_frame
self.odom_msg.child_frame_id = self.child_frame
# Subscribers
self.twist_sub = rospy.Subscriber("cmd_vel",
Twist,
self.twist_callback,
queue_size=5)
self.left_encoder_sub = rospy.Subscriber("left/left_encoder/ticks",
Int64,
self.left_encoder_callback,
queue_size=50)
self.right_encoder_sub = rospy.Subscriber("right/right_encoder/ticks",
Int64,
self.right_encoder_callback,
queue_size=50)
self.ultrasonic_sub = rospy.Subscriber(
"earth_rover_teensy_bridge/ultrasonic",
Float32MultiArray,
self.ultrasonic_callback,
queue_size=15)
# Publishers
self.left_dist_pub = rospy.Publisher("left/left_encoder/distance",
Float64,
queue_size=5)
self.right_dist_pub = rospy.Publisher("right/right_encoder/distance",
Float64,
queue_size=5)
self.left_speed_pub = rospy.Publisher("left/left_encoder/speed",
Float64,
queue_size=5)
self.right_speed_pub = rospy.Publisher("right/right_encoder/speed",
Float64,
queue_size=5)
self.left_command_pub = rospy.Publisher("left/command_speed",
Float64,
queue_size=5)
self.right_command_pub = rospy.Publisher("right/command_speed",
Float64,
queue_size=5)
self.odom_pub = rospy.Publisher("odom", Odometry, queue_size=5)
# Services
self.tuning_service = rospy.Service("tune_motor_pid", TuneMotorPID,
self.tune_motor_pid)
#!/usr/bin/python
from motor_controller import MotorController
from time import sleep
if __name__ == '__main__':
#chip1 left motor (13,15,11) CONFIRMED
#chip1 right motor (12, 16, 18) CONFIRMED
#chip2 left motor (35,37,33) CONFIRMED
#chip2 right motor (36, 38, 40) CONFIRMED
motors = [
MotorController(13, 15, 11),
MotorController(12, 16, 18),
MotorController(35, 37, 33),
MotorController(36, 38, 40)
]
#for motor in motors:
# motor.RunForwards()
#sleep(5)
for motor in motors:
motor.RunBackwards()
sleep(20)
for motor in motors:
motor.Stop()