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 __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
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()
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 start_motor():
mc = MotorController()
if mc.connect():
print "in connect"
mc.set_motor_speed(1700)
mc.sync_speed(5)
return mc
else:
return None
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 __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_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 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()
#!/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()
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()
import busio
import board
from aux_terminal import AuxPipe
#initialize
sampleFreq = 0.02
terminal = AuxPipe('angle_values')
db = DBManager()
i2c_bus = busio.I2C(board.SCL, board.SDA)
fulcrumValues = db.retrieve_fulcrum_values(id=1)
motorStartupValue = fulcrumValues[1]+500
angleSensor = Sensor(i2c_bus, fulcrumValues[0], terminal)
motorController = MotorController(i2c_bus, 7, 500, sampleFreq, motorStartupValue, angleSensor, terminal)
#routines
def options(step):
if step == 0:
print('''
Select your choice:
(1): Quit
(2): Calibrate Sensor
(3): Set Throttle Range
(4): Fulcrum Prop Values
(5): Startup Motor
(6): Kill Motor
'''
)
optionSwitch(0)
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/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)
def stop_motor(mc):
if (mc):
mc.set_motor_speed(1500)
else:
mc = MotorController()
mc.connect()
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()
#!/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 __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()
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)
def __init__(self):
rospy.init_node('motor_controller_node')
self.motor = MotorController()
sub = rospy.Subscriber('motor/speed_motors', Int32MultiArray,
self.adjust_motor)
