def __init__(self, cfg):
self.STEERING_NEUTRAL = cfg['servo_neutral_angle']
self.MIN_STEERING_ANGLE = cfg[
'servo_min_angle_limit'] # Steering left/right max angle.
self.MAX_STEERING_ANGLE = cfg[
'servo_max_angle_limit'] # Steering left/right max angle.
self.STEERING_RATE = cfg[
'steering_rate'] # Server steering angle is fomula angle. But car_client depends on car. Drift type steering, normal steering, etc. Therefore, use this rate for ajust good angle.
self.MOTOR_NEUTRAL_SPEED = cfg['motor_neutral_speed']
self.MOTOR_FORWARD_SPEED_RATE = float(
cfg['motor_max_speed_limit']
) / float(
100
) # Server max speed is 100. But car_client depends on config parameter.
self.MOTOR_BACK_SPEED_RATE = float(
cfg['motor_min_speed_limit']
) / float(
-100
) # Server min speed is -100. But car_client depends on config parameter.
self.steering = Servo(cfg)
self.motor = Motor(cfg)
self.steering.set_angle(self.STEERING_NEUTRAL, delay=0)
self.motor.set_speed(self.MOTOR_NEUTRAL_SPEED, delay=0)
self.speed = self.MOTOR_NEUTRAL_SPEED
self.back_in = False
self.back_start_time = None
return
def quit():
if _port_motor:
_port_motor.halt()
if _stbd_motor:
_stbd_motor.halt()
Motor.cancel()
sys.stderr = DevNull()
print('exit.')
sys.exit(0)
def __init__(self, config, ticker, tb, level):
super().__init__()
self._log = Logger('motors', level)
self._log.info('initialising motors...')
if config is None:
raise Exception('no config argument provided.')
if ticker is None:
raise Exception('no ticker argument provided.')
self._ticker = ticker
if tb is None:
tb = self._configure_thunderborg_motors(level)
if tb is None:
raise Exception('unable to configure thunderborg.')
self._tb = tb
self._set_max_power_ratio()
# config pigpio's pi and name its callback thread (ex-API)
try:
self._pi = pigpio.pi()
if self._pi is None:
raise Exception('unable to instantiate pigpio.pi().')
elif self._pi._notify is None:
raise Exception('can\'t connect to pigpio daemon; did you start it?')
self._pi._notify.name = 'pi.callback'
self._log.info('pigpio version {}'.format(self._pi.get_pigpio_version()))
from lib.motor import Motor
self._log.info('imported Motor.')
except Exception as e:
self._log.error('error importing and/or configuring Motor: {}'.format(e))
traceback.print_exc(file=sys.stdout)
sys.exit(1)
self._port_motor = Motor(config, self._ticker, self._tb, self._pi, Orientation.PORT, level)
self._port_motor.set_max_power_ratio(self._max_power_ratio)
self._stbd_motor = Motor(config, self._ticker, self._tb, self._pi, Orientation.STBD, level)
self._stbd_motor.set_max_power_ratio(self._max_power_ratio)
self._closed = False
self._enabled = False # used to be enabled by default
# a dictionary of motor # to last set value
self._msgIndex = 0
self._last_set_power = { 0:0, 1:0 }
self._log.info('motors ready.')
class RosCar():
def __init__(self, cfg):
self.STEERING_NEUTRAL = cfg['servo_neutral_angle']
self.MIN_STEERING_ANGLE = cfg[
'servo_min_angle_limit'] # Steering left/right max angle.
self.MAX_STEERING_ANGLE = cfg[
'servo_max_angle_limit'] # Steering left/right max angle.
self.STEERING_RATE = cfg[
'steering_rate'] # Server steering angle is fomula angle. But car_client depends on car. Drift type steering, normal steering, etc. Therefore, use this rate for ajust good angle.
self.MOTOR_NEUTRAL_SPEED = cfg['motor_neutral_speed']
self.MOTOR_FORWARD_SPEED_RATE = float(
cfg['motor_max_speed_limit']
) / float(
100
) # Server max speed is 100. But car_client depends on config parameter.
self.MOTOR_BACK_SPEED_RATE = float(
cfg['motor_min_speed_limit']
) / float(
-100
) # Server min speed is -100. But car_client depends on config parameter.
self.steering = Servo(cfg)
self.motor = Motor(cfg)
self.steering.set_angle(self.STEERING_NEUTRAL, delay=0)
self.motor.set_speed(self.MOTOR_NEUTRAL_SPEED, delay=0)
self.speed = self.MOTOR_NEUTRAL_SPEED
self.back_in = False
self.back_start_time = None
return
def __del__(self):
self.steering.set_angle(self.STEERING_NEUTRAL, delay=0)
self.motor.set_speed(self.MOTOR_NEUTRAL_SPEED, delay=0)
return
def listener(self):
"""
READ ROS TOPICS AND RUN FUNCTION BY HZ.
rosnode list
rosnode info /twist_filter
rostopic echo /cmd_vel
"""
rospy.init_node('ROSCarNano', anonymous=True)
rospy.Subscriber('/cmd_vel', Twist, self.cmd_vel)
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
return
def cmd_vel(self, values):
"""
ステアリング・速度のターゲット値の処理
ステアリング値のomegaはターゲット角速度である。現在の速度を加味してomegaを算出しなければならない
self.target_speed: target speed (m/s)
"""
"""
Autoware 1.9.1にバック機能は無い。
そのため速度は負の値は正の値として扱う。
"""
print("/cmd_vel: {}".format(values))
speed = abs(values.linear.x)
target_speed = map(speed, 0.0, 0.22, 0.0, 30.0)
"""
Autowareの角速度は右カーブがマイナス、左カーブがプラス。
omega: rad/s and 10 Hz
1 processing = 1 Hz = 0.1 * omega
left/right = -1 * omega
"""
omega = values.angular.z
"""
omegaはtarget_speedが時速2kmの時、ベストマッチ。
速度が上がった時、それに応じてomegaを減算させる。
"""
print("omega: {} speed: {}".format(omega, speed))
self.set_speed(target_speed)
self.set_angle_rad(omega)
return
def omega_to_angle(self, omega):
"""
角速度ωをサーボ可動域に変換する。
omegaは右カーブがマイナス、左カーブがプラス。
サーボは右カーブが90-180度、左カーブが0-90度となるため、
omegaの正負を逆にしてdegreeに変換してから90度加える。
ω(rad/s)
θ= ω*180/pi (deg/s)
rad = θ*pi/180
"""
omega = -1.0 * omega
theta = float(omega) * float(180) / math.pi
angle = 90.0 - (180.0 - theta) / 2.0
return angle
def set_angle_rad(self, omega):
"""
omega: rad/s
"""
steering_angle = self.omega_to_angle(omega)
#print("omega to angle: {} -> {}".format(omega, steering_angle))
steering_angle = int(float(steering_angle) * float(self.STEERING_RATE))
steering_angle = self.STEERING_NEUTRAL + steering_angle
"""
Adjust within operable angle
"""
if steering_angle > self.MAX_STEERING_ANGLE:
steering_angle = self.MAX_STEERING_ANGLE
if steering_angle < self.MIN_STEERING_ANGLE:
steering_angle = self.MIN_STEERING_ANGLE
print("steering: {}".format(steering_angle))
self.steering.set_angle(steering_angle)
def set_speed(self, speed):
"""
speed: Analog int value for PCA9685
"""
motor_speed = speed
if motor_speed > 0:
motor_speed = int(
float(motor_speed) * float(self.MOTOR_FORWARD_SPEED_RATE))
elif motor_speed < 0:
motor_speed = int(
float(motor_speed) * float(self.MOTOR_BACK_SPEED_RATE))
self.motor.set_speed(motor_speed)
def brake(self, value):
if value.data:
self.motor.set_speed(self.MOTOR_NEUTRAL_SPEED, delay=0)
def signal_handler(signal, frame):
print('Ctrl-C caught: exiting...')
Motor.cancel()
sys.stderr = DevNull()
print('exit.')
sys.exit(0)
class Motors():
'''
A dual motor controller with encoders.
'''
def __init__(self, config, ticker, tb, level):
super().__init__()
self._log = Logger('motors', level)
self._log.info('initialising motors...')
if config is None:
raise Exception('no config argument provided.')
if ticker is None:
raise Exception('no ticker argument provided.')
self._ticker = ticker
if tb is None:
tb = self._configure_thunderborg_motors(level)
if tb is None:
raise Exception('unable to configure thunderborg.')
self._tb = tb
self._set_max_power_ratio()
# config pigpio's pi and name its callback thread (ex-API)
try:
self._pi = pigpio.pi()
if self._pi is None:
raise Exception('unable to instantiate pigpio.pi().')
elif self._pi._notify is None:
raise Exception('can\'t connect to pigpio daemon; did you start it?')
self._pi._notify.name = 'pi.callback'
self._log.info('pigpio version {}'.format(self._pi.get_pigpio_version()))
from lib.motor import Motor
self._log.info('imported Motor.')
except Exception as e:
self._log.error('error importing and/or configuring Motor: {}'.format(e))
traceback.print_exc(file=sys.stdout)
sys.exit(1)
self._port_motor = Motor(config, self._ticker, self._tb, self._pi, Orientation.PORT, level)
self._port_motor.set_max_power_ratio(self._max_power_ratio)
self._stbd_motor = Motor(config, self._ticker, self._tb, self._pi, Orientation.STBD, level)
self._stbd_motor.set_max_power_ratio(self._max_power_ratio)
self._closed = False
self._enabled = False # used to be enabled by default
# a dictionary of motor # to last set value
self._msgIndex = 0
self._last_set_power = { 0:0, 1:0 }
self._log.info('motors ready.')
# ..........................................................................
def name(self):
return 'Motors'
# ..........................................................................
def _configure_thunderborg_motors(self, level):
'''
Import the ThunderBorg library, then configure the Motors.
'''
self._log.info('configure thunderborg & motors...')
global pi
try:
self._log.info('importing thunderborg...')
import lib.ThunderBorg3 as ThunderBorg
self._log.info('successfully imported thunderborg.')
TB = ThunderBorg.ThunderBorg(level) # create a new ThunderBorg object
TB.Init() # set the board up (checks the board is connected)
self._log.info('successfully instantiated thunderborg.')
if not TB.foundChip:
boards = ThunderBorg.ScanForThunderBorg()
if len(boards) == 0:
self._log.error('no thunderborg found, check you are attached.')
else:
self._log.error('no ThunderBorg at address {:02x}, but we did find boards:'.format(TB.i2cAddress))
for board in boards:
self._log.info('board {:02x} {:d}'.format(board, board))
self._log.error('if you need to change the I²C address change the setup line so it is correct, e.g. TB.i2cAddress = {:0x}'.format(boards[0]))
sys.exit(1)
TB.SetLedShowBattery(True)
return TB
except Exception as e:
self._log.error('unable to import thunderborg: {}'.format(e))
traceback.print_exc(file=sys.stdout)
sys.exit(1)
# ..........................................................................
def set_led_show_battery(self, enable):
self._tb.SetLedShowBattery(enable)
# ..........................................................................
def set_led_color(self, color):
self._tb.SetLed1(color.red/255.0, color.green/255.0, color.blue/255.0)
# ..........................................................................
def _set_max_power_ratio(self):
pass
# initialise ThunderBorg ...........................
self._log.info('getting battery reading...')
# get battery voltage to determine max motor power
# could be: Makita 12V or 18V power tool battery, or 12-20V line supply
voltage_in = self._tb.GetBatteryReading()
if voltage_in is None:
raise OSError('cannot continue: cannot read battery voltage.')
self._log.info('voltage in: {:>5.2f}V'.format(voltage_in))
# voltage_in = 20.5
# maximum motor voltage
voltage_out = 9.0
self._log.info('voltage out: {:>5.2f}V'.format(voltage_out))
if voltage_in < voltage_out:
raise OSError('cannot continue: battery voltage too low ({:>5.2f}V).'.format(voltage_in))
# Setup the power limits
if voltage_out > voltage_in:
self._max_power_ratio = 1.0
else:
self._max_power_ratio = voltage_out / float(voltage_in)
# convert float to ratio format
self._log.info('battery level: {:>5.2f}V; motor voltage: {:>5.2f}V;'.format( voltage_in, voltage_out) + Fore.CYAN + Style.BRIGHT \
+ ' maximum power ratio: {}'.format(str(Fraction(self._max_power_ratio).limit_denominator(max_denominator=20)).replace('/',':')))
# ..........................................................................
def get_motor(self, orientation):
if orientation is Orientation.PORT:
return self._port_motor
else:
return self._stbd_motor
# ..........................................................................
def is_in_motion(self):
'''
Returns true if either motor is moving.
'''
return self._port_motor.is_in_motion() or self._stbd_motor.is_in_motion()
# ..........................................................................
def get_steps(self):
'''
Returns the port and starboard motor step count.
'''
return [ self._port_motor.get_steps() , self._stbd_motor.get_steps() ]
# ..........................................................................
def get_current_power_level(self, orientation):
'''
Returns the last set power of the specified motor.
'''
if orientation is Orientation.PORT:
return self._port_motor.get_current_power_level()
else:
return self._stbd_motor.get_current_power_level()
# ..........................................................................
def interrupt(self):
'''
Interrupt any motor loops by setting the _interrupt flag.
'''
self._port_motor.interrupt()
self._stbd_motor.interrupt()
# ..........................................................................
def halt(self):
'''
Quickly (but not immediately) stops both motors.
'''
self._log.info('halting...')
if self.is_stopped():
_tp = Thread(name='halt-port', target=self.processStop, args=(Event.HALT, Orientation.PORT))
_ts = Thread(name='hapt-stbd', target=self.processStop, args=(Event.HALT, Orientation.STBD))
_tp.start()
_ts.start()
else:
self._log.debug('already stopped.')
self._log.info('halted.')
return True
# ..........................................................................
def brake(self):
'''
Slowly coasts both motors to a stop.
'''
self._log.info('braking...')
if self.is_stopped():
_tp = Thread(name='brake-port', target=self.processStop, args=(Event.BRAKE, Orientation.PORT))
_ts = Thread(name='brake-stbd', target=self.processStop, args=(Event.BRAKE, Orientation.STBD))
_tp.start()
_ts.start()
else:
self._log.warning('already stopped.')
self._log.info('braked.')
return True
# ..........................................................................
def stop(self):
'''
Stops both motors immediately, with no slewing.
'''
self._log.info('stopping...')
if not self.is_stopped():
self._port_motor.stop()
self._stbd_motor.stop()
self._log.info('stopped.')
else:
self._log.warning('already stopped.')
return True
# ..........................................................................
def is_stopped(self):
return self._port_motor.is_stopped() and self._stbd_motor.is_stopped()
# ..........................................................................
def processStop(self, event, orientation):
'''
Synchronised process control over various kinds of stopping.
'''
if orientation is Orientation.PORT:
if event is Event.HALT:
self._log.info('halting port motor...')
self._port_motor.halt()
elif event is Event.BRAKE:
self._log.info('braking port motor...')
self._port_motor.brake()
else: # is stop
self._log.info('stopping port motor...')
self._port_motor.stop()
else:
if event is Event.HALT:
self._log.info('halting starboard motor...')
self._stbd_motor.halt()
elif event is Event.BRAKE:
self._log.info('braking starboard motor...')
self._stbd_motor.brake()
else: # is stop
self._log.info('stopping starboard motor...')
self._stbd_motor.stop()
self.print_current_power_levels()
# ..........................................................................
def get_current_power_levels(self):
'''
Returns the last set power values.
'''
_port_power = self._port_motor.get_current_power_level()
_stbd_power = self._stbd_motor.get_current_power_level()
return [ _port_power, _stbd_power ]
# ..........................................................................
def print_current_power_levels(self):
'''
Prints the last set power values.
'''
self._msgIndex += 1
self._log.info('{}:\tcurrent power:\t{:6.1f}\t{:6.1f}'.format(self._msgIndex, self._last_set_power[0], self._last_set_power[1]))
# ..........................................................................
def enable(self):
'''
Enables the motors, ticker and velocity calculator. This issues
a warning if already enabled, but no harm is done in calling
it repeatedly.
'''
if self._enabled:
self._log.warning('already enabled.')
if not self._port_motor.enabled:
self._port_motor.enable()
if not self._stbd_motor.enabled:
self._stbd_motor.enable()
self._ticker.enable()
self._enabled = True
self._log.info('enabled.')
# ..........................................................................
def disable(self):
'''
Disable the motors, halting first if in motion.
'''
if self._enabled:
self._log.info('disabling...')
self._enabled = False
if self.is_in_motion(): # if we're moving then halt
self._log.warning('event: motors are in motion (halting).')
self.halt()
self._port_motor.disable()
self._stbd_motor.disable()
self._log.info('disabling pigpio...')
self._pi.stop()
self._log.info('disabled.')
else:
self._log.debug('already disabled.')
# ..........................................................................
def close(self):
'''
Halts, turn everything off and stop doing anything.
'''
if not self._closed:
if self._enabled:
self.disable()
self._log.info('closing...')
self._port_motor.close()
self._stbd_motor.close()
self._closed = True
self._log.info('closed.')
else:
self._log.debug('already closed.')
# ..........................................................................
@staticmethod
def cancel():
print('cancelling motors...')
Motor.cancel()
def test_motors():
global _port_motor, _stbd_motor, action_A, action_B
# read YAML configuration
_loader = ConfigLoader(Level.INFO)
filename = 'config.yaml'
_config = _loader.configure(filename)
_log.info('creating message factory...')
_message_factory = MessageFactory(Level.INFO)
_log.info('creating message bus...')
_message_bus = MessageBus(Level.WARN)
_log.info('creating clock...')
_clock = Clock(_config, _message_bus, _message_factory, Level.WARN)
_motor_configurer = MotorConfigurer(_config, _clock, Level.INFO)
_motors = _motor_configurer.get_motors()
_motors.enable()
_i2c_scanner = I2CScanner(Level.WARN)
if ENABLE_MOTH:
if _i2c_scanner.has_address([0x18]):
_rgbmatrix = RgbMatrix(Level.WARN)
# _rgbmatrix.set_display_type(DisplayType.SOLID)
_moth = Moth(_config, None, Level.WARN)
else:
_log.warning('cannot enable moth: no IO Expander found.')
_moth = None
_pin_A = 16
_button_16 = Button(_pin_A, callback_method_A, Level.INFO)
_log.info(
Style.BRIGHT +
'press button A (connected to pin {:d}) to toggle or initiate action.'.
format(_pin_A))
_pin_B = 24
_button_24 = Button(_pin_B, callback_method_B, Level.INFO)
_log.info(Style.BRIGHT +
'press button B connected to pin {:d}) to exit.'.format(_pin_B))
_log.info('starting motors...')
_port_motor = _motors.get_motor(Orientation.PORT)
_stbd_motor = _motors.get_motor(Orientation.STBD)
if ENABLE_PORT:
_port_motor.enable()
if ENABLE_STBD:
_stbd_motor.enable()
_rot_ctrl = RotaryControl(_config, 0, 50, 2, Level.WARN) # min, max, step
_rate = Rate(5)
_step_limit = 2312
_velocity_stbd = 0.0
_velocity_port = 0.0
_start_time = dt.now()
_moth_port = 1.0
_moth_stbd = 1.0
_moth_offset = 0.6
_moth_bias = [0.0, 0.0]
_moth_fudge = 0.7
try:
action_A = True # if not using buttons at all set to True
action_B = True
while INFINITE or action_B or (_port_motor.steps < _step_limit
and _stbd_motor.steps < _step_limit):
if action_A:
action_A = False # trigger once
while action_B:
if USE_ROTARY_CONTROL:
_target_velocity = _rot_ctrl.read()
else:
_target_velocity = 30.0
# _power = _target_velocity / 100.0
_power = Motor.velocity_to_power(_target_velocity)
if ENABLE_MOTH and _moth:
_moth_bias = _moth.get_bias()
# _log.info(Fore.WHITE + Style.BRIGHT + 'port: {:5.2f}; stbd: {:5.2f}'.format(_moth_port, _moth_stbd))
# _rgbmatrix.show_hue(_moth_hue, Orientation.BOTH)
_orientation = _moth.get_orientation()
if _orientation is Orientation.PORT:
_moth_port = _moth_bias[0] * _moth_fudge
_moth_stbd = 1.0
_rgbmatrix.show_color(Color.BLACK,
Orientation.STBD)
_rgbmatrix.show_color(Color.RED, Orientation.PORT)
elif _orientation is Orientation.STBD:
_moth_port = 1.0
_moth_stbd = _moth_bias[1] * _moth_fudge
_rgbmatrix.show_color(Color.BLACK,
Orientation.PORT)
_rgbmatrix.show_color(Color.GREEN,
Orientation.STBD)
else:
_moth_port = 1.0
_moth_stbd = 1.0
_rgbmatrix.show_color(Color.BLACK,
Orientation.PORT)
_rgbmatrix.show_color(Color.BLACK,
Orientation.STBD)
if ENABLE_STBD:
_stbd_motor.set_motor_power(_stbd_rotate * _power *
_moth_stbd)
_velocity_stbd = _stbd_motor.velocity
if ENABLE_PORT:
_port_motor.set_motor_power(_port_rotate * _power *
_moth_port)
_velocity_port = _port_motor.velocity
_log.info(Fore.YELLOW + 'power: {:5.2f}; bias: '.format(_power) \
+ Fore.RED + ' {:5.2f} '.format(_moth_bias[0]) + Fore.GREEN + '{:5.2f};'.format(_moth_bias[1]) \
+ Fore.BLACK + ' target velocity: {:5.2f};'.format(_target_velocity) \
+ Fore.CYAN + ' velocity: ' \
+ Fore.RED + ' {:5.2f} '.format(_velocity_port) + Fore.GREEN + ' {:5.2f}'.format(_velocity_stbd))
# _log.info(Fore.RED + 'power {:5.2f}/{:5.2f}; {:>4d} steps; \t'.format(_stbd_motor.get_current_power_level(), _power, _port_motor.steps) \
# + Fore.GREEN + 'power {:5.2f}/{:5.2f}; {:>4d} steps.'.format(_port_motor.get_current_power_level(), _power, _stbd_motor.steps))
_rate.wait()
action_B = True # reentry into B loop, waiting for A
_log.info('waiting for A button press...')
time.sleep(1.0)
# end wait loop ....................................................
if ENABLE_PORT:
_log.info('port motor: {:d} steps.'.format(_port_motor.steps))
if ENABLE_STBD:
_log.info('stbd motor: {:d} steps.'.format(_stbd_motor.steps))
except KeyboardInterrupt:
_log.info('Ctrl-C caught; exiting...')
except Exception as e:
_log.error('error: {}'.format(e))
finally:
close_motors(_log)
_elapsed_ms = round((dt.now() - _start_time).total_seconds() * 1000.0)
_log.info(Fore.YELLOW + 'complete: elapsed: {:d}ms'.format(_elapsed_ms))
def cancel():
print('cancelling motors...')
Motor.cancel()
def main():
print("start")
motor = None
try:
"""
LOAD SETUP VARIABLES
"""
cfg = load_config()
motor = Motor(cfg)
"""
# NEUTRAL ANALOG RANGE: 370-393
neutral = 370
print(motor.analog_to_pulse(neutral))
neutral = 393
print(motor.analog_to_pulse(neutral))
# FORWARD MAXIMUM ANALOG VALUE: 280
analog = 280
print(motor.analog_to_pulse(analog))
# BACKWARD MAXIMUM ANALOG VALUE: 440
analog = 440
print(motor.analog_to_pulse(analog))
"""
# FORWARD: 369(START) - 280(MAX SPEED)
motor.set_speed(0)
time.sleep(2)
speed = 0
"""
for i in range(100):
speed += 1
motor.set_speed(speed)
print(motor.get_speed())
time.sleep(0.1)
for i in range(100):
speed -= 1
motor.set_speed(speed)
print(motor.get_speed())
time.sleep(0.1)
"""
# Setting the "BACKWARD" value to the value will apply the brake. (It is probably the function of ESC.)
# BACKWORD: 394(START) - 440(MAX SPEED)
speed = -30 # 30% backword throttle
delay = 5.0
for i in range(2):
speed = 30 # 30% throttle
motor.set_speed(speed, delay)
time.sleep(2.5)
speed = 0
motor.set_speed(speed, delay)
time.sleep(2.5)
motor.set_speed(0)
time.sleep(2)
except:
import traceback
traceback.print_exc()
finally:
if motor is not None:
motor.set_speed(0)
time.sleep(3)
print("end")
def test_velocity_to_power_conversion():
# velocity to power conversion test:
for v in numpy.arange(0.0, 60.0, 2.0):
p = Motor.velocity_to_power(v)
_log.info('velocity: {:5.2f};'.format(v) + Fore.YELLOW +
' power: {:5.2f};'.format(p))
dist_prox_cone.append(dpc)
return da,dpc
def pegar_coordenadas_com_delay():
for i in range(0,5):
da, dpc = ground.logica_localizacao()
diff_ang.append(da)
dist_prox_cone.append(dpc)
time.sleep(0.01) # wait for the coordinates to update
return da,dpc
diff_ang = []; dist_prox_cone = []
ground = gps()
motor = Motor(23,24,18)
motor.move_forward()
servo = Curves(12)
servo.posicionar(0,100)
vel1 = motor.change_speed(15,80)
da,dpc = pegar_coordenadas_com_delay()
while (dpc[-1] > 3):
vel1 = motor.change_speed(45,30)
da,dpc = pegar_coordenadas()
angulo = servo.posicionar(da[-1]*.25,50)
#print("da = " + str(da))
#if (da[-1] < -25):
# angulo1 = servo.posicionar(da[-1],50)
#elif (((da[-1] > -40) & (da[-1] < -25))):
# angulo1 = servo.posicionar(da[-1],50)
class RosCar():
def __init__(self, cfg):
self.STEERING_NEUTRAL = cfg['servo_neutral_angle']
self.MIN_STEERING_ANGLE = cfg[
'servo_min_angle_limit'] # Steering left/right max angle.
self.MAX_STEERING_ANGLE = cfg[
'servo_max_angle_limit'] # Steering left/right max angle.
self.STEERING_RATE = cfg[
'steering_rate'] # Server steering angle is fomula angle. But car_client depends on car. Drift type steering, normal steering, etc. Therefore, use this rate for ajust good angle.
self.MOTOR_NEUTRAL_SPEED = cfg['motor_neutral_speed']
self.MOTOR_FORWARD_SPEED_RATE = float(
cfg['motor_max_speed_limit']
) / float(
100
) # Server max speed is 100. But car_client depends on config parameter.
self.MOTOR_BACK_SPEED_RATE = float(
cfg['motor_min_speed_limit']
) / float(
-100
) # Server min speed is -100. But car_client depends on config parameter.
self.steering = Servo(cfg)
self.motor = Motor(cfg)
self.steering.set_angle(self.STEERING_NEUTRAL, delay=0)
self.motor.set_speed(self.MOTOR_NEUTRAL_SPEED, delay=0)
return
def listener(self):
"""
READ ROS TOPICS AND RUN FUNCTION BY HZ.
angle_deg: -45 to 45
speed_deg: -100 to 100
brake_bool: True or False
"""
rospy.init_node('RoboCarROS', anonymous=True)
rospy.Subscriber('/steer/angle_deg', Int8, self.set_angle)
rospy.Subscriber('/motor/speed_deg', Int8, self.set_speed)
rospy.Subscriber('/motor/brake_bool', Bool, self.brake)
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
return
def set_angle(self, angle):
"""
angle.data: -45 to 45 degree.
"""
steering_angle = angle.data
steering_angle = int(float(steering_angle) * float(self.STEERING_RATE))
steering_angle = self.STEERING_NEUTRAL + steering_angle
"""
Adjust within operable angle
"""
if steering_angle > self.MAX_STEERING_ANGLE:
steering_angle = self.MAX_STEERING_ANGLE
if steering_angle < self.MIN_STEERING_ANGLE:
steering_angle = self.MIN_STEERING_ANGLE
self.steering.set_angle(steering_angle)
def set_speed(self, speed):
motor_speed = speed.data
if motor_speed > 0:
motor_speed = int(
float(motor_speed) * float(self.MOTOR_FORWARD_SPEED_RATE))
elif motor_speed < 0:
motor_speed = int(
float(motor_speed) * float(self.MOTOR_BACK_SPEED_RATE))
self.motor.set_speed(motor_speed)
def brake(self, value):
if value.data:
self.motor.set_speed(self.MOTOR_NEUTRAL_SPEED, delay=0)
class RosCar():
def __init__(self, cfg):
self.STEERING_NEUTRAL = cfg['servo_neutral_angle']
self.MIN_STEERING_ANGLE = cfg[
'servo_min_angle_limit'] # Steering left/right max angle.
self.MAX_STEERING_ANGLE = cfg[
'servo_max_angle_limit'] # Steering left/right max angle.
self.STEERING_RATE = cfg[
'steering_rate'] # Server steering angle is fomula angle. But car_client depends on car. Drift type steering, normal steering, etc. Therefore, use this rate for ajust good angle.
self.MOTOR_NEUTRAL_SPEED = cfg['motor_neutral_speed']
self.MOTOR_FORWARD_SPEED_RATE = float(
cfg['motor_max_speed_limit']
) / float(
100
) # Server max speed is 100. But car_client depends on config parameter.
self.MOTOR_BACK_SPEED_RATE = float(
cfg['motor_min_speed_limit']
) / float(
-100
) # Server min speed is -100. But car_client depends on config parameter.
self.steering = Servo(cfg)
self.motor = Motor(cfg)
self.steering.set_angle(self.STEERING_NEUTRAL, delay=0)
self.motor.set_speed(self.MOTOR_NEUTRAL_SPEED, delay=0)
self.speed = self.MOTOR_NEUTRAL_SPEED
return
def __del__(self):
self.steering.set_angle(self.STEERING_NEUTRAL, delay=0)
self.motor.set_speed(self.MOTOR_NEUTRAL_SPEED, delay=0)
return
def listener(self):
"""
ROS Topicの更新時に処理を行う設定
rosnode list
rostopic echo /cmd_vel
rostopic echo /ackermann_cmd
"""
rospy.init_node('ROSCarNano', anonymous=True)
"""
アッカーマン運動力学
"""
ackermann = AckermannPublisher()
ackermann.add()
rospy.Subscriber('/ackermann_cmd', AckermannDriveStamped,
self.ackermann_cmd)
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
return
def ackermann_cmd(self, values):
"""
ステアリング・速度のターゲット値の処理
ステアリング値のsteering_angleは角度(rad)なので、degreeに変換する
speed: m/s
target_speed: motor power %
# map(指示を受けた速度(m/s), 指示速度の後方最高速度m/s, 指示速度の最高速度m/s, モーター後方最高出力%, モーター前方最高出力%)
# ここでは前後モーター最高出力は100%として、デバイス設定時に車両設定の速度制限を適用する。
"""
print("/ackermann_cmd: {}".format(values))
#speed = abs(values.drive.speed) # no backwords
speed = values.drive.speed
target_speed = map(speed, -0.28, 0.28, -100.0, 100.0)
"""
ステアリング角
アッカーマン運動力学のステアリング角を利用する
"""
angle_rad = values.drive.steering_angle
self.set_speed(target_speed)
self.set_angle_rad(angle_rad)
def rad_to_angle(self, angle_rad):
"""
angle_radをサーボ可動域に変換する。
angle_radは右カーブがマイナス、左カーブがプラス。
サーボは右カーブが90-180度、左カーブが0-90度となるため、
angle_radの正負を逆にしてdegreeに変換してから90度加える。
ω(rad/s)
θ= angle_rad*180/pi (deg/s)
rad = θ*pi/180
"""
angle_rad = -1.0 * angle_rad
theta = float(angle_rad) * float(180) / math.pi
angle = 90.0 - (180.0 - theta) / 2.0
return angle
def set_angle_rad(self, angle_rad):
"""
omega: rad/s
"""
steering_angle = self.rad_to_angle(angle_rad)
#print("omega to angle: {} -> {}".format(omega, steering_angle))
steering_angle = int(float(steering_angle) * float(self.STEERING_RATE))
steering_angle = self.STEERING_NEUTRAL + steering_angle
"""
Adjust within operable angle
"""
if steering_angle > self.MAX_STEERING_ANGLE:
steering_angle = self.MAX_STEERING_ANGLE
if steering_angle < self.MIN_STEERING_ANGLE:
steering_angle = self.MIN_STEERING_ANGLE
print("steering: {}".format(steering_angle))
self.steering.set_angle(steering_angle)
def set_speed(self, speed):
"""
speed: Analog int value for PCA9685
"""
motor_speed = speed
if motor_speed > 0:
motor_speed = int(
float(motor_speed) * float(self.MOTOR_FORWARD_SPEED_RATE))
elif motor_speed < 0:
motor_speed = int(
float(motor_speed) * float(self.MOTOR_BACK_SPEED_RATE))
print("motor: {}".format(motor_speed))
self.motor.set_speed(motor_speed)
def brake(self, value):
if value.data:
self.motor.set_speed(self.MOTOR_NEUTRAL_SPEED, delay=0)
def quit():
Motor.cancel()
sys.stderr = DevNull()
print('exit.')
sys.exit(0)
class RosCar():
def __init__(self, cfg):
self.STEERING_NEUTRAL = cfg['servo_neutral_angle']
self.MIN_STEERING_ANGLE = cfg['servo_min_angle_limit'] # Steering left/right max angle.
self.MAX_STEERING_ANGLE = cfg['servo_max_angle_limit'] # Steering left/right max angle.
self.STEERING_RATE = cfg['steering_rate'] # Server steering angle is fomula angle. But car_client depends on car. Drift type steering, normal steering, etc. Therefore, use this rate for ajust good angle.
self.MOTOR_NEUTRAL_SPEED = cfg['motor_neutral_speed']
self.MOTOR_FORWARD_SPEED_RATE = float(cfg['motor_max_speed_limit'])/float(100) # Server max speed is 100. But car_client depends on config parameter.
self.MOTOR_BACK_SPEED_RATE = float(cfg['motor_min_speed_limit'])/float(-100) # Server min speed is -100. But car_client depends on config parameter.
self.steering = Servo(cfg)
self.motor = Motor(cfg)
self.steering.set_angle(self.STEERING_NEUTRAL, delay=0)
self.motor.set_speed(self.MOTOR_NEUTRAL_SPEED, delay=0)
self.speed = self.MOTOR_NEUTRAL_SPEED
self.back_in = False
self.back_start_time = None
self.target_speed = self.MOTOR_NEUTRAL_SPEED
return
def __del__(self):
self.steering.set_angle(self.STEERING_NEUTRAL, delay=0)
self.motor.set_speed(self.MOTOR_NEUTRAL_SPEED, delay=0)
return
def listener(self):
"""
READ ROS TOPICS AND RUN FUNCTION BY HZ.
rosnode list
rosnode info /twist_filter
rostopic echo /twist_cmd
"""
rospy.init_node('RoboCarROS', anonymous=True)
rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cmd)
# current_velocity
rospy.Subscriber('/current_velocity', TwistStamped, self.current_velocity)
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
return
def current_velocity(self, values):
"""
現在速度と目標速度を比較して、モーター出力を調整する。
self.speed: モーター出力
current_velocity: 現在速度
self.target_speed: 目標速度
"""
current_velocity = values.twist.linear.x
print("speed now: {} target: {}".format(current_velocity, self.target_speed))
if current_velocity < self.target_speed:
self.speed += 1
elif current_velocity > self.target_speed:
self.speed -= 1
if self.speed >= 100:
self.speed = 100
elif self.speed <= 0:
self.speed = 0
"""
現在速度が0.1(m/s)未満の時、急発進を抑えるためにモーター出力を制限する。
"""
if self.speed > 25 and current_velocity < 0.1:
self.speed = 25
self.set_speed(self.speed)
return
def twist_cmd(self, values):
"""
ステアリング・速度のターゲット値の処理
ステアリング値はここで適用する
速度は変数で保持し、current_velocityによる現在速度取得時に適用する
ステアリング値のomegaはターゲット角速度である。現在の速度を加味してomegaを算出しなければならない
self.target_speed: target speed (m/s)
"""
#print(values)
"""
Autoware 1.9.1にバック機能は無い。
そのため速度は負の値は正の値として扱う。
"""
speed = abs(values.twist.linear.x)
self.target_speed = speed
"""
Autowareの角速度は右カーブがマイナス、左カーブがプラス。
omega: rad/s and 10 Hz
1 processing = 1 Hz = 0.1 * omega
left/right = -1 * omega
"""
omega = values.twist.angular.z
"""
omegaはtarget_speedが時速2kmの時、ベストマッチ。
速度が上がった時、それに応じてomegaを減算させる。
"""
print("before omega: {} speed: {}".format(omega, speed))
if self.target_speed >= 0.56:
"""
目標速度が時速2km(0.56m/s)以上の時、比率に応じてomegaを小さくする
"""
ratio = map(self.target_speed, 2.0, 1000.0, 1.0, 1000.0)
omega = omega*ratio
print("after omega: {} speed: {}".format(omega, speed))
self.set_angle_rad(omega)
return
def omega_to_angle(self, omega):
"""
角速度ωをサーボ可動域に変換する。
omegaは右カーブがマイナス、左カーブがプラス。
サーボは右カーブが90-180度、左カーブが0-90度となるため、
omegaの正負を逆にしてdegreeに変換してから90度加える。
ω(rad/s)
θ= ω*180/pi (deg/s)
rad = θ*pi/180
"""
omega = -1.0 * omega
theta = float(omega)*float(180)/math.pi
angle = 90.0 - (180.0 - theta)/2.0
return angle
def set_angle_rad(self, omega):
"""
omega: rad/s
"""
steering_angle = self.omega_to_angle(omega)
#print("omega to angle: {} -> {}".format(omega, steering_angle))
steering_angle = int(float(steering_angle) * float(self.STEERING_RATE))
steering_angle = self.STEERING_NEUTRAL + steering_angle
"""
Adjust within operable angle
"""
if steering_angle > self.MAX_STEERING_ANGLE:
steering_angle = self.MAX_STEERING_ANGLE
if steering_angle < self.MIN_STEERING_ANGLE:
steering_angle = self.MIN_STEERING_ANGLE
print("steering: {}".format(steering_angle))
self.steering.set_angle(steering_angle)
def set_speed(self, speed):
"""
speed: Analog int value for PCA9685
"""
motor_speed = speed
if motor_speed > 0:
motor_speed = int(float(motor_speed) * float(self.MOTOR_FORWARD_SPEED_RATE))
elif motor_speed < 0:
motor_speed = int(float(motor_speed) * float(self.MOTOR_BACK_SPEED_RATE))
self.motor.set_speed(motor_speed)
def brake(self, value):
if value.data:
self.motor.set_speed(self.MOTOR_NEUTRAL_SPEED, delay=0)
