def __init__(self, ):
rospy.init_node("pololu_simple_motor_controller",
# log_level=rospy.DEBUG
)
rospy.on_shutdown(self.shutdown)
rospy.loginfo("Connecting to pololu simple motor controller")
self.port = rospy.get_param("~serial_port", "/dev/ttyUSB0")
self.baud = rospy.get_param("~serial_baud", 115200)
self.speed_topic_name = rospy.get_param("~speed_topic_name",
"command_speed")
self.brake_topic_name = rospy.get_param("~brake_topic_name",
"command_brake")
self.reverse_motor = rospy.get_param("~reverse_motor", False)
rospy.loginfo("pololu simple motor controller serial_port: '%s'",
self.port)
rospy.loginfo("pololu simple motor controller serial_baud: '%s'",
self.baud)
if self.reverse_motor:
rospy.loginfo(
"reversing motor direction for pololu simple motor controller")
self.motor_controller = SMC(self.port, self.baud)
self.motor_controller.init()
self.speed_sub = rospy.Subscriber(self.speed_topic_name,
Float64,
self.speed_command_callback,
queue_size=1)
self.brake_sub = rospy.Subscriber(self.brake_topic_name,
Int16,
self.brake_command_callback,
queue_size=1)
self.motor_controller.speed(0)
async def parse_cmd():
"""
Parse the message for a command.
Ensure its not a message from the bot and that its preceded by a bang
"""
if not message.content.startswith('!') or message.author == client.user:
return
global current_smc
check_smc()
try:
command_name, duration, *topic = message.content.split()
command_name = command_name.replace('!', '').strip()
if command_name == 'smc':
if current_smc is not None and current_smc.ongoing():
await message.channel.send(f"~ error: Cannot start an SMC while another is ongoing ~")
return
valid, validate_msg = SMC.validate_command(parts)
if not valid:
await message.channel.send(f"~ error: {validate_msg} ~")
await message.channel.send(SMC._help)
else:
current_smc = SMC(
owner=message.author,
channel=message.channel,
topic=' '.join(parts[2:]),
duration=int(parts[1])
)
await current_smc._announce()
else:
# Delegate to the SMC class
if current_smc is None:
await message.channel.send('~ error: No currently running SMC ~')
else:
await delegate_cmd(current_smc, command_name, message)
def __init__(self):
print("Starting lamp_base driver!")
self.sub = rospy.Subscriber(rospy.get_param('/driver_params/effort_topic'), Float64, self.callback)
self.mc_tty = rospy.get_param('/driver_params/smc_tty')
self.minspeed = rospy.get_param('/driver_params/smc_speed_min')
self.maxspeed = rospy.get_param('/driver_params/smc_speed_max')
self.invert = rospy.get_param('/driver_params/smc_invert')
self.power = 0
self.last_cmd = None
self.timeout = 0.2
self.mc = None
print("Connecting to motor controller at {}".format(self.mc_tty))
while True:
try:
self.mc = SMC(self.mc_tty, 115200)
self.mc.init()
except SerialException as se:
print("Could not configure motor controller! Trying again in 3 seconds.")
print(se.message)
rospy.sleep(3)
continue
break
print("Successfully initialized lamp base motor driver!")
def __init__(self, enabled=True):
self.set_logger(write=True)
super(MotorControllerBridge, self).__init__(enabled)
if enabled:
self.mc = SMC('/dev/serial/by-id/usb-Pololu_Corporation_Pololu_Simple_High-Power_Motor_Controller_18v15_33FF-6806-4D4B-3731-5147-1543-if00', 115200)
else:
self.mc = None
self.queue_active_event = asyncio.Event()
self.queue_lock = Lock()
self.command_queue = Queue()
self.pause_timestamp = None
class LampBase:
def __init__(self):
print("Starting lamp_base driver!")
self.sub = rospy.Subscriber(rospy.get_param('/driver_params/effort_topic'), Float64, self.callback)
self.mc_tty = rospy.get_param('/driver_params/smc_tty')
self.minspeed = rospy.get_param('/driver_params/smc_speed_min')
self.maxspeed = rospy.get_param('/driver_params/smc_speed_max')
self.invert = rospy.get_param('/driver_params/smc_invert')
self.power = 0
self.last_cmd = None
self.timeout = 0.2
self.mc = None
print("Connecting to motor controller at {}".format(self.mc_tty))
while True:
try:
self.mc = SMC(self.mc_tty, 115200)
self.mc.init()
except SerialException as se:
print("Could not configure motor controller! Trying again in 3 seconds.")
print(se.message)
rospy.sleep(3)
continue
break
print("Successfully initialized lamp base motor driver!")
def timerUpdate(self, timerEvent):
if self.last_cmd is None:
return
if rospy.Time.now() - self.last_cmd > rospy.Duration(self.timeout):
return
speedcmd = float(self.maxspeed - self.minspeed) / 2 * self.power
if self.invert:
speedcmd = -speedcmd
try:
self.mc.speed(int(speedcmd))
except SerialException:
try:
self.mc = SMC(self.mc_tty, 115200)
self.mc.init()
except SerialException as se:
print("Could not configure motor controller after connection loss!.")
print(se.message)
def callback(self, msg):
self.power = msg.data
self.last_cmd = rospy.Time.now()
def start(self):
rospy.Timer(rospy.Duration(1.0/50), self.timerUpdate)
rospy.spin()
self.mc.speed(0)
def timerUpdate(self, timerEvent):
if self.last_cmd is None:
return
if rospy.Time.now() - self.last_cmd > rospy.Duration(self.timeout):
return
speedcmd = float(self.maxspeed - self.minspeed) / 2 * self.power
if self.invert:
speedcmd = -speedcmd
try:
self.mc.speed(int(speedcmd))
except SerialException:
try:
self.mc = SMC(self.mc_tty, 115200)
self.mc.init()
except SerialException as se:
print("Could not configure motor controller after connection loss!.")
print(se.message)
class PololuSimpleMotorController:
def __init__(self, ):
rospy.init_node("pololu_simple_motor_controller",
# log_level=rospy.DEBUG
)
rospy.on_shutdown(self.shutdown)
rospy.loginfo("Connecting to pololu simple motor controller")
self.port = rospy.get_param("~serial_port", "/dev/ttyUSB0")
self.baud = rospy.get_param("~serial_baud", 115200)
self.speed_topic_name = rospy.get_param("~speed_topic_name",
"command_speed")
self.brake_topic_name = rospy.get_param("~brake_topic_name",
"command_brake")
self.reverse_motor = rospy.get_param("~reverse_motor", False)
rospy.loginfo("pololu simple motor controller serial_port: '%s'",
self.port)
rospy.loginfo("pololu simple motor controller serial_baud: '%s'",
self.baud)
if self.reverse_motor:
rospy.loginfo(
"reversing motor direction for pololu simple motor controller")
self.motor_controller = SMC(self.port, self.baud)
self.motor_controller.init()
self.speed_sub = rospy.Subscriber(self.speed_topic_name,
Float64,
self.speed_command_callback,
queue_size=1)
self.brake_sub = rospy.Subscriber(self.brake_topic_name,
Int16,
self.brake_command_callback,
queue_size=1)
self.motor_controller.speed(0)
def shutdown(self):
self.speed_sub.unregister()
self.brake_sub.unregister()
# self.motor_controller.stop()
# self.motor_controller.close()
def speed_command_callback(self, command):
motor_command = max(-1.0, min(command.data, 1.0))
motor_command = int(motor_command * 3200)
if self.reverse_motor:
motor_command = -motor_command
rospy.logdebug("Sending: %s" % motor_command)
self.motor_controller.speed(motor_command)
def brake_command_callback(self, command):
self.motor_controller.mbreak(int(command.data))
def run(self):
smc = SMC('/dev/tty.usbserial0')
smc.init()
smc.stop() # make sure we are stopped?
saber = Sabertooth('/dev/tty.usbserial1')
saber.stop() # make sure we are stopped?
# pub = zmq.Pub() # do i need to feedback motor errors?
sub = zmq.Sub(['cmd', 'dome'])
while True:
topic, msg = sub.recv()
if msg:
if topic == 'cmd':
print('got cmd')
elif topic == 'dome':
print('got dome')
else:
time.sleep(0.5)
def extend_pause_restract_arm():
mc = SMC('/dev/ttyACM0', 115200)
# drive using 12b mode
print("Arm extending...")
mc.init()
mc.speed(-1000)
print_counter(15)
print("Arm stopped...")
mc.stop()
print_counter(10)
print("Arm retracting...")
mc.init()
mc.speed(1000)
print_counter(15)
mc.stop()
def PRE(pivot_angle, rotate_angle, extend_length):
mc = SMC('/dev/ttyACM0', 115200)
mc.init()
pivot_rotate_kit = ServoKit(channels=16)
# The pivot servo should be connected to channel 1
# pivot_servo = adafruit_motor.servo.Servo(0)
# The rotation servo should be connected to channel 2
# rotation_servo = pivot_rotate_kit.servo.Servo(1)
#
# # Set the actuation range, min_pulse, and max_pulse of the pivot servo
pivot_rotate_kit.servo[0].actuation_range = 600 # degrees
pivot_rotate_kit.servo[0].min_pulse = (1000)
pivot_rotate_kit.servo[0].max_pulse = (2000)
# Set the actuation range, min_pulse, and max_pulse of the rotation servo
pivot_rotate_kit.servo[1].actuation_range = 600 # degrees
pivot_rotate_kit.servo[1].min_pulse = (1000)
pivot_rotate_kit.servo[1].max_pulse = (2000)
# Command pivot to the required angle
# Command rotation to the required angle
pivot_rotate_kit.servo[0].angle = pivot_angle * 4 # degrees
pivot_rotate_kit.servo[1].angle = rotate_angle # degrees
#########################TEST EXTENSION############################
# Extension line equation: y = 10x + 20
time_on = (extend_length - 20) / 10
current_time = time.time()
while time.time() - current_time < time_on:
mc.speed(-1100)
mc.stop()
time.sleep(30)
current_time = time.time()
mc.init()
while time.time() - current_time < time_on:
mc.speed(1100)
mc.stop()
#!/usr/bin/env python
from __future__ import print_function, division
from smc import SMC
import time
port = '/dev/serial/by-id/usb-Pololu_Corporation_Pololu_Simple_Motor_Controller_18v7_50FF-6D06-7085-5652-2323-2267-if00'
mc = SMC(port, 115200)
while 1:
mc.init()
choice = input("Enter speed: ")
mc.speed7b(choice)
time.sleep(3)
## mc.speed7b(-choice)
## time.sleep(3)
mc.stop()
from smc import SMC
import time
mc = SMC('/dev/ttyACM0', 115200)
# open serial port and exit safe mode
mc.init()
# drive using 12b mode
mc.speed(1000)
time.sleep(30)
mc.speed(-1000)
time.sleep(30)
# drive using 7b mode
mc.speed7b(100)
time.sleep(30)
mc.speed7b(-100)
time.sleep(30)
# and stop motor
mc.stop()
def main(argv):
parser = argparse.ArgumentParser(prog='flash-dump',
formatter_class=argparse.RawDescriptionHelpFormatter,
description=textwrap.dedent('''\
360 Flash Tool - acabey
--------------------------------
Load and dump information from flash dumps and shadowboot ROMs
Should be able to detect type of file as well as partial files
'''),
epilog=textwrap.dedent('''\
Valid sections are:
keyvault, smc, smcconfig, sb, cb, sc, sd, cd, se, ce, cf, cg, cf1, cg1, kernel, hv
''')
)
parser.add_argument('target', type=str, metavar='/path/to/working/file', help='Working file - NAND/Shadowboot/NAND section or output file name')
parser.add_argument('-e', '--enumerate', nargs='+', metavar='section', type=str, help='Enumerate section(s)')
parser.add_argument('-d', '--decrypt', nargs='+', metavar='section', type=str, help='Decrypt section(s)')
parser.add_argument('-x', '--extract', nargs='+', metavar='section', type=str, help='Extract section(s)')
parser.add_argument('-r', '--replace', nargs=2, type=str, metavar=('section', '/path/to/replacement'), help='Replace decrypted section')
parser.add_argument('-s', '--sign', nargs=1, type=str, metavar='section', help='Sign section (SD)')
parser.add_argument('-k', '--keyfile', nargs=1, type=str, metavar='/path/to/keyfile', help='Key file')
parser.add_argument('-c', '--cpukey', nargs=1, type=str, metavar='cpukey', help='CPU key')
parser.add_argument('--debug', action='store_true', help='Verbose debug output')
parser.add_argument('-v', '--version', action='version', version='%(prog)s 0.5', help='Version')
args = parser.parse_args()
# Load input metadata and populate available sections
"""
============================================================================
Input metadata
Load NAND/shadowboot structures
============================================================================
"""
try:
Image.identifyAvailableStructures(args.target)
availablesections = Image.getAvailableStructures()
except Exception as e:
failprint('Failed to identify available structures: ' + str(e))
"""
============================================================================
Manipulate input
============================================================================
"""
Constants.DEBUG = args.debug
dbgprint('args: ' + str(args))
# CPU key if available
if not args.cpukey == None:
try:
Constants.CPUKEY = bytes.fromhex(args.cpukey[0])
except Exception as e:
Constants.CPUKEY = None
failprint('Failed to set given CPU key: ' + str(e))
# BL1 key if available
if not args.keyfile == None:
try:
with open(args.keyfile[0], 'rb') as keyfile:
Constants.SECRET_1BL = keyfile.read()
except Exception as e:
Constants.SECRET_1BL = None
failprint('Failed to set 1BL key from keyfile' + str(e))
# Enumerate if available
if not args.enumerate == None:
for section in args.enumerate:
if section == 'all':
for availablesection in availablesections:
print('==== ' + availablesection + ' ====')
print(availablesections[availablesection].enumerate())
print('')
break
if section in availablesections.keys():
print('==== ' + section + ' ====')
print(availablesections[section].enumerate())
else:
warnprint('Section: ' + section + ' is not available in input file')
# Extract if available
if not args.extract == None:
for section in args.extract:
if section == 'all':
for availablesection in availablesections:
availablesections[availablesection].extract()
break
if section in availablesections.keys():
availablesections[section].extract()
else:
warnprint('Section: ' + section + ' is not available in input file')
# Replace if available
if not args.replace == None:
section = args.replace[0]
replacementpath = args.replace[1]
dbgprint(availablesections.keys())
if section in availablesections.keys():
try:
availablesections[section].replace(replacementpath)
availablesections[section].write(Image.outputpath.path)
except Exception as e:
failprint('Failed to replace ' + section + ': ' + str(e))
else:
warnprint('Section: ' + section + ' is not available in input file')
# Sign if available
if not args.sign== None:
for section in args.sign:
if section == 'all':
for availablesection in availablesections:
availablesections[availablesection].sign()
break
if section in availablesections.keys():
availablesections[section].sign()
else:
warnprint('Section: ' + section + ' is not available in input file')
"""
============================================================================
DEPRECATED
This is code from prior revisions that I have not refactored.
It will be kept (inaccesible) as a reference until implemented in new structure
============================================================================
"""
"""
============================================================================
NAND Extras
============================================================================
"""
if False:
nandsections = []
# Check for SMC
if not nandheader.smcoffset == 0:
image.seek(nandheader.smcoffset,0)
smcdata = image.read(nandheader.smclength)
# Make sure SMC is not null
if not all(b == 0 for b in smcdata):
smc = SMC(smcdata, nandheader.smcoffset)
# nandsections.append(smc)
print('Found valid SMC at ' + str(hex(smc.offset)))
else:
print('SMC is null, skipping SMC')
else:
print('SMC offset is null, skipping SMC')
# Check for Keyvault
# Because the keyvault's length is stored in its header, we first create the header object
if not nandheader.kvoffset == 0:
image.seek(nandheader.kvoffset,0)
keyvaultheaderdata = image.read(KeyvaultHeader.HEADER_SIZE)
# Make sure KV header is not null
if not all(b == 0 for b in keyvaultheaderdata):
keyvaultheader = KeyvaultHeader(keyvaultheaderdata, nandheader.kvoffset)
image.seek(nandheader.kvoffset,0)
keyvaultdata = image.read(keyvaultheader.length)
# Make sure KV is not null
if not all(b == 0 for b in keyvaultdata):
keyvault = Keyvault(keyvaultheader, keyvaultdata)
nandsections.append(keyvault)
print('Found valid keyvault at ' + str(hex(keyvault.offset)))
else:
print('Keyvault data is null, skipping keyvault')
else:
print('Keyvault header is null, skipping keyvault')
else:
print('Keyvault offset is null, skipping keyvault')
sys.exit(0)
class MotorControllerBridge(Node):
def __init__(self, enabled=True):
self.set_logger(write=True)
super(MotorControllerBridge, self).__init__(enabled)
if enabled:
self.mc = SMC('/dev/serial/by-id/usb-Pololu_Corporation_Pololu_Simple_High-Power_Motor_Controller_18v15_33FF-6806-4D4B-3731-5147-1543-if00', 115200)
else:
self.mc = None
self.queue_active_event = asyncio.Event()
self.queue_lock = Lock()
self.command_queue = Queue()
self.pause_timestamp = None
async def setup(self):
self.logger.debug("Initializing...")
self.mc.init()
self.mc.speed(0)
self.logger.debug("done!")
def set_speed(self, command):
command = int(-command)
self.logger.debug("command: %s" % command)
self.mc.speed(command)
def queue_speed(self, command):
self.command_queue.put(int(command))
def run_queue(self):
self.queue_active_event.set()
def write_pause(self, timestamp):
self.command_queue.put(float(timestamp))
def clear_write_queue(self):
self.command_queue.put(None)
async def loop(self):
while True:
await self.queue_active_event.wait()
self.logger.info("Executing motor command queue backlog")
with self.queue_lock:
while not self.command_queue.empty():
if self.pause_timestamp is not None:
# if pause timer has expired, reset the timer and continue sending commands
if time.time() > self.pause_timestamp:
self.pause_timestamp = None
await asyncio.sleep(0.0)
continue
command = self.command_queue.get()
if type(command) == int:
self.set_speed(command)
elif type(command) == float:
self.pause_timestamp = command
else:
# for any other type, cancel the queue
while not self.command_queue.empty():
self.command_queue.get()
break
await asyncio.sleep(0.0)
self.logger.info("Command queue backlog finished!")
self.queue_active_event.clear()
async def teardown(self):
self.logger.debug("Tearing down")
self.mc.speed(0)