def __init__(self):
'''
Read config file to init camera's parameter
'''
config = configparser.ConfigParser()
config.read("config.ini")
self.statu=""
# Horiazonal direction control parameters
HIntfNum = config.getint("camera", "HIntfNum")
HInitPosition = config.getint("camera", "HInitPosition")
HMinPosition = config.getint("camera", "HMinPosition")
HMaxPosition = config.getint("camera", "HMaxPosition")
HSpeed = config.getint("camera", "HSpeed")
# Vertical direction control parameters
VIntfNum = config.getint("camera", "VIntfNum")
VInitPosition = config.getint("camera", "VInitPosition")
VMinPosition = config.getint("camera", "VMinPosition")
VMaxPosition = config.getint("camera", "VMaxPosition")
VSpeed = config.getint("camera", "VSpeed")
self.HCameraControl = Steering(
HIntfNum, HInitPosition, HMinPosition, HMaxPosition, HSpeed)
self.VCameraControl = Steering(
VIntfNum, VInitPosition, VMinPosition, VMaxPosition, VSpeed)
def __init__(self, arg):
self.args = arg
isDebug = False
if (len(self.args) > 1):
print("\n".join(self.args[1:]))
if (self.args[1] == "debug"):
self.__debug = True
atexit.register(self.cleanup)
self.__connect_to_cruncher()
try:
self.__kit = ServoKit(channels=16)
self.__space_invaders = SpaceInvaders(self.__kit)
except:
print("Servokit not initialised, Servo Calibration won't work")
try:
self.__steering = Steering(self.__kit,
steeringStatusFile=os.path.abspath(
"./config/steering_status.json"))
self.__suspension = Suspension(self.__kit)
except:
type, value, traceback = sys.exc_info()
print("Steering status failed to load")
print('Error Details %s: %s %s' % (type, value, traceback))
def __init__(self):
self.ub.Init()
self.tb.Init()
self.t1 = datetime.now()
self.tickSpeed = 0.05
self.us = Ultrasonics(self.ub)
self.motors = Motors(self.tb, self.ub, self.tickSpeed)
self.steering = Steering(self.tb, self.ub, self.tickSpeed)
self.teleLogger = Telemetry("telemetry", "csv").get()
self.ptc = PanTiltController(self.ub, 270, 135)
self.joystick_controller = JoystickController(self.tb, self.ub)
battMin, battMax = self.tb.GetBatteryMonitoringLimits()
battCurrent = self.tb.GetBatteryReading()
print('Battery monitoring settings:')
print(' Minimum (red) %02.2f V' % (battMin))
print(' Half-way (yellow) %02.2f V' % ((battMin + battMax) / 2))
print(' Maximum (green) %02.2f V' % (battMax))
print()
print(' Current voltage %02.2f V' % (battCurrent))
print(' Mode %s' % (self.mode))
print()
def __init__(self, servoKit):
self.__kit = servoKit
self.__keyboardInput = KeyboardInput("Steering Calibration")
config_file = servoStatusFile = os.path.abspath(
os.path.join(os.path.dirname(__file__),
"..")) + "/config/servo_status.json"
self.__steering = Steering(self.__kit, config_file)
self.__suspension = Suspension(self.__kit, config_file)
def __init__(self):
self.logger = logging.getLogger(self.__class__.__name__)
GPIO.setmode(GPIO.BCM)
self.steer = Steering()
self.brakes = False
self.has_turned = False
# motor driver
self.input1 = 20
self.input2 = 16
self.enable1 = 21 # always disable after finishing and\or in catch
# set GPIO direction (IN / OUT)
# we send instructions to motor driver IC
GPIO.setup(self.input1, GPIO.OUT)
GPIO.setup(self.input2, GPIO.OUT)
# this will determine if signal should be sent to output pins
GPIO.setup(self.enable1, GPIO.OUT)
def back(self):
if self.has_turned:
with Steering() as steer:
steer.turn_center()
#Log('move start')
GPIO.output(self.enable1, GPIO.HIGH)
# if input1 and input2 are swapped a regular motor will go in reverse.
GPIO.output(self.input1, GPIO.HIGH)
# but lego will not as it has different pin for reverse. so we should disable this signal and enable that , creepy
GPIO.output(self.input2, GPIO.LOW)
def front(self):
#Log('move start')
# if input1 and input2 are swapped we go in reverse.
if self.has_turned:
with Steering() as steer:
steer.turn_center()
GPIO.output(self.input1, GPIO.LOW)
GPIO.output(self.input2, GPIO.HIGH)
GPIO.output(self.enable1, GPIO.HIGH)
def data1(self, msg: can.Message):
if not Receiver.check_integrity(msg.data):
return self.on_can_error()
data = msg.data
self.car_data.steering_angle = Steering.to_value(data[0])
self.car_data.wheel_data.fr.rpm = data[1]
self.car_data.wheel_data.fl.rpm = data[2]
self.car_data.wheel_data.rr.rpm = data[3]
self.car_data.wheel_data.rl.rpm = data[4]
self.car_data.velocity = Driving.to_value(data[5])
self.on_update(self.car_data)
def __init__(self):
config = configparser.ConfigParser()
config.read("./config.ini")
HIntfNum = config.getint("camera", "HIntfNum")
HInitPosition = config.getint("camera", "HInitPosition")
HMinPosition = config.getint("camera", "HMinPosition")
HMaxPosition = config.getint("camera", "HMaxPosition")
HSpeed = config.getint("camera", "HSpeed")
# Vertical direction control parameters
VIntfNum = config.getint("camera", "VIntfNum")
VInitPosition = config.getint("camera", "VInitPosition")
VMinPosition = config.getint("camera", "VMinPosition")
VMaxPosition = config.getint("camera", "VMaxPosition")
VSpeed = config.getint("camera", "VSpeed")
self.HCameraControl = Steering(HIntfNum, HInitPosition, HMinPosition,
HMaxPosition, HSpeed)
self.VCameraControl = Steering(VIntfNum, VInitPosition, VMinPosition,
VMaxPosition, VSpeed)
def __init__(self):
self.pwm = PWM(16)
self.pwm.start()
self.light_level = 0
# self.pwm.set_pwm_freq(60)
self.steering_wheel_left = ServoMotor(self.pwm, 0)
self.steering_wheel_right = ServoMotor(self.pwm, 1)
self.left_motor = DCMotor(self.pwm, 2, 3)
self.right_motor = DCMotor(self.pwm, 4, 5)
self.camera = ServoMotor(self.pwm, 12)
self.steering = Steering(mount_height=46.1,
mount_width=40.0,
wheel_arm=26.1,
servo_horn=20.0,
bridge=40.0,
width=123,
length=193.650)
self._mode_button = Button(24)
self._mode_button.when_pressed = self._on_mode_button
self._mode_led_1 = LED(22)
self._mode_led_2 = LED(10)
self.mode = 1
self._update_mode()
def __init__(self):
config = configparser.ConfigParser()
config.read("config.ini")
trigger = config.getint("ultrasonic", "Trigger")
echo = config.getint("ultrasonic", "Echo")
servoNum = config.getint("steering", "servoNum")
InitPosition = config.getint("steering", "InitPosition")
minPosition = config.getint("steering", "MinPosition")
maxPosition = config.getint("steering", "maxPosition")
speed = config.getint("steering", "speed")
self.IN_1 = config.getint("car", "IN1")
self.IN_2 = config.getint("car", "IN2")
self.IN_3 = config.getint("car", "IN3")
self.IN_4 = config.getint("car", "IN4")
self.Sonic = Ultrasonic(trigger, echo)
self.Servo = Steering(servoNum, InitPosition, minPosition, maxPosition,
speed)
self.motor = Motor(self.IN_1, self.IN_2, self.IN_3, self.IN_4)
def move(self, signal):
if signal == 258:
self.logger.debug('going forward')
if self.brakes == True:
self.logger.debug('disabling brakes')
GPIO.output(self.enable1, GPIO.HIGH)
self.brakes = False
self.front()
if signal == 260:
self.logger.debug('turning left')
with Steering() as steer:
steer.turn_left()
self.has_turned = True
if signal == 261:
self.logger.debug('turning right')
with Steering() as steer:
steer.turn_right()
self.has_turned = True
if signal == 259:
self.logger.debug('going back')
self.back()
if signal == 32:
self.logger.debug('applying brakes')
self.brakes = True
GPIO.output(self.enable1, GPIO.LOW)
if signal == 27:
self.logger.debug('clearing')
self.clear()
return signal
def __init__(self, arg):
self.arg = arg
atexit.register(self.cleanup)
try:
self.__kit = ServoKit(channels=16)
except:
print("Servokit not initialised, Servo Calibration won't work")
try:
self.__steering = Steering(self.__kit, servoStatusFile=os.path.abspath("./config/servo_status.json"))
except:
type, value, traceback = sys.exc_info()
print("Steering status failed to load")
print('Error Details %s: %s %s' % (traceback, type, value))
def __init__(self, options):
options.setdefault('debug_mode', False)
self.name = options['vehicle_name']
self.debug_mode = options['debug_mode']
self.set_turning_apex(float(options['vehicle_turning_apex']))
the_accessory_klass = (DebugAccessory if self.debug_mode else Accessory)
self.accessory = the_accessory_klass(options['accessories'])
self.steering = Steering.get(options['steering_strategy'])
the_wheel_klass = (DebugWheel if self.debug_mode else Wheel)
self.left_wheel = the_wheel_klass({'side': 'left'})
self.right_wheel = the_wheel_klass({'side': 'right'})
initial_vehicle_state = {
self.TORQUE_LEVEL: 0,
self.REVERSE: False,
self.DIRECTION_LEVEL: 0,
self.DIRECTION: self.steering.STRAIGHT
}
self.vehicle_state = initial_vehicle_state
class ServoCalibration:
__kit = None
__looper = True
__keyboardInput = None
__steering = None
__suspension = None
def __init__(self, servoKit):
self.__kit = servoKit
self.__keyboardInput = KeyboardInput("Steering Calibration")
config_file = servoStatusFile = os.path.abspath(
os.path.join(os.path.dirname(__file__),
"..")) + "/config/servo_status.json"
self.__steering = Steering(self.__kit, config_file)
self.__suspension = Suspension(self.__kit, config_file)
def menu(self):
self.__keyboardInput.clear()
print("J2 Controller Steering Calibration Menu:")
print()
print("c: Setup Wheel ports (On Adafruit PWM Board)")
print("n: Setup Suspension ports (On Adafruit PWM Board)")
print("w: Front left Wheel")
print("e: Front right Wheel")
print("s: Rear left Wheel")
print("d: Rear right Wheel")
print("o: Front Suspension")
print("k: Rear Suspension")
print("r: Save current status")
print("t: Reset all to *_start angle in sttering_status.json")
print("a: Set Actuation Angle")
print("--------------------")
print("q: Back")
print("")
self.waitForInput()
def waitForInput(self):
while self.__looper:
keyp = self.__keyboardInput.readkey()
if (keyp == 'q'):
print("Saving...")
self.__steering.save_servo_status()
print("Quit")
self.__looper = False
elif (keyp == 'c'):
print("Enter Port on which front_left steering motor is: ")
self.__steering.set_steering_port(Steering.FRONT_LEFT_POS,
input())
print("Enter Port on which front_right steering motor is: ")
self.__steering.set_steering_port(Steering.FRONT_RIGHT_POS,
input())
print("Enter Port on which rear_left steering motor is: ")
self.__steering.set_steering_port(Steering.REAR_LEFT_POS,
input())
print("Enter Port on which rear_right steering motor is: ")
self.__steering.set_steering_port(Steering.REAR_RIGHT_POS,
input())
self.__steering.save_servo_status()
elif (keyp == 'w'):
self.calibrate_steering(1)
elif (keyp == 'e'):
self.calibrate_steering(2)
elif (keyp == 's'):
self.calibrate_steering(3)
elif (keyp == 'd'):
self.calibrate_steering(4)
elif (keyp == 'r'):
self.__steering.save_servo_status()
print("Saved to file", end='\r', flush=True)
elif (keyp == 't'):
self.__steering.move_servo_to(
Steering.FRONT_LEFT_POS,
self.__steering.servo_status.front_left_start)
self.__steering.move_servo_to(
Steering.FRONT_RIGHT_POS,
self.__steering.servo_status.front_right_start)
self.__steering.move_servo_to(
Steering.REAR_LEFT_POS,
self.__steering.servo_status.rear_left_start)
self.__steering.move_servo_to(
Steering.REAR_RIGHT_POS,
self.__steering.servo_status.rear_right_start)
print("\r\n Servos updated")
print("\r\n Servo status")
self.__steering.print_servo_stats()
elif (keyp == 'a'):
print("Set actuation degrees [180-270]: ")
self.__steering.set_actuation_degrees(int(input()))
time.sleep(0.01)
def calibrate_steering(self, index):
waitForWheel = True
keyboardInput = KeyboardInput("Calibrate Steering:")
print(
"Press Up/Down to test or w/z to adjust wheel delta, 0 to reset, q when done"
)
while waitForWheel:
key = keyboardInput.readkey()
if (key == 'w'):
self.__steering.increment_position(index, 1)
self.__steering.update_servos()
elif (ord(key) == 16):
self.__steering.move_servo_by(index, 1)
elif (key == 'z'):
self.__steering.decrement_position(index, 1)
self.__steering.update_servos()
elif (ord(key) == 17):
self.__steering.move_servo_by(index, -1)
elif (key == 'q'):
self.__steering.save_servo_status()
waitForWheel = False
time.sleep(0.01)
def getSteeringPIDDerivativeGain(self):
return 0.0008
def getSteeringPIDIntegralTermLimit(self):
return -1.0
if __name__ == "__main__":
log_message_queue_listener.start()
logging.info('Loading')
ev3.Sound.beep().wait()
ev3.Sound.beep().wait()
settings = Settings()
bumpers = Bumpers(settings.getFrontBumperAddress()) if (settings.hasFrontBumper()) else None
steering = Steering(
settings.getSteeringMotorAddress(),
settings.getSteeringMotorSpeedFactor(),
settings.getSteeringSpeed(),
settings.getSteeringMaxRange(),
settings.getSteeringMotorPositionFactor(),
settings.getDoCenterSteeringWhenInitializing()
)
folkracer = Folkracer(steering, Engine(settings), Distances(settings), bumpers, Buttons(), settings, None, None, LightsAndSounds())
folkracer.start()
folkracer.join()
logging.info('Shutting down.')
log_message_queue_listener.stop()
ev3.Sound.beep().wait()
ev3.Sound.beep().wait()
def setUp(self):
self.steering = Steering(TEST_STEERING_MOTOR_ADDRESS,
TEST_STEERING_MOTOR_SPEED_FACTOR,
TEST_STEERING_SPEED, TEST_STEERING_MAX_RANGE,
TEST_STEERING_MOTOR_POSITION_FACTOR, True)
self.steering.steering_motor = MagicMock()
class J2controller():
"""The J2 Controller Main processing loop"""
__looper = True
__kit = None
__bt_client = None
__terminalMenu = TerminalMenu()
__piconzero_drive = None # PiconzeroDrive()
__gpiozero_drive = GpiozeroDrive()
__space_invaders = None
__steering = None
__suspension = None
__debug = False
def __init__(self, arg):
self.args = arg
isDebug = False
if (len(self.args) > 1):
print("\n".join(self.args[1:]))
if (self.args[1] == "debug"):
self.__debug = True
atexit.register(self.cleanup)
self.__connect_to_cruncher()
try:
self.__kit = ServoKit(channels=16)
self.__space_invaders = SpaceInvaders(self.__kit)
except:
print("Servokit not initialised, Servo Calibration won't work")
try:
self.__steering = Steering(self.__kit,
steeringStatusFile=os.path.abspath(
"./config/steering_status.json"))
self.__suspension = Suspension(self.__kit)
except:
type, value, traceback = sys.exc_info()
print("Steering status failed to load")
print('Error Details %s: %s %s' % (type, value, traceback))
def init(self):
while self.__looper:
if (self.__bt_client == None):
self.__connect_to_cruncher()
elif (self.__bt_client.connected == False):
try:
print("Trying to connect")
self.__bt_client.connect()
except KeyboardInterrupt:
self.__looper = False
self.cleanup()
except:
type, value, traceback = sys.exc_info()
print('Error Details %s: %s %s' % (type, value, traceback))
time.sleep(1 / 60)
def __connect_to_cruncher(self):
try:
self.__bt_client = BluetoothClient("B8:27:EB:55:22:18",
self.data_received,
auto_connect=True)
if (self.__debug):
print(
"Bluetooth client initialised, ready for Cruncher comms:")
except:
type, value, traceback = sys.exc_info()
if (self.__debug):
print(
"Bluetooth client not initialised, Cruncher comms won't work"
)
print('Error Details %s: %s %s' % (type, value, traceback))
time.sleep(1)
def data_received(self, data_string):
#print("BT Recieved:" + data_string)
try:
lines = data_string.splitlines()
for line in lines:
request = BtRequest(json_def=line)
if (request.cmd == "calibrate"):
if (request.action == "getStatus"):
self.bt_request.send(
json.dumps(self.__steering.steering()))
elif (request.action == "setStatus"):
# self.bt_request.s
pass
if (request.cmd == "suspension"):
if (request.action == "raise"):
if (request.data.which == "front"):
self.__suspension.raise_front_by(
request.data.position)
elif (request.data.which == "rear"):
self.__suspension.raise_rear_by(
request.data.position)
elif (request.data.which == "both"):
self.__suspension.raise_both_by(
request.data.position)
if (request.action == "lower"):
if (request.data.which == "front"):
self.__suspension.lower_front_by(
request.data.position)
elif (request.data.which == "rear"):
self.__suspension.lower_rear_by(
request.data.position)
elif (request.data.which == "both"):
self.__suspension.lower_both_by(
request.data.position)
if (request.cmd == "steering"):
if (request.action == "move"):
self.__gpiozero_drive.move(
int(request.data.directionLeft),
int(request.data.directionRight),
float(request.data.speedLeft),
float(request.data.speedRight))
if (request.cmd == "wheels"):
if (request.action == "strafe"):
# if(self.__debug):
# print("Strafing")
self.__steering.move_servo_to(
Steering.FRONT_LEFT_POS,
int(request.data.frontLeftAngle))
self.__steering.move_servo_to(
Steering.FRONT_RIGHT_POS,
int(request.data.frontRightAngle))
self.__steering.move_servo_to(
Steering.REAR_LEFT_POS,
int(request.data.rearLeftAngle))
self.__steering.move_servo_to(
Steering.REAR_RIGHT_POS,
int(request.data.rearRightAngle))
if (request.cmd == "cannon"):
print("BT Recieved:" + data_string)
if (request.action == "aim"):
self.__space_invaders.aim(int(request.data.position))
# Turn laser on
elif (request.action == "launch"):
# Fire appropriate cannon
self.__space_invaders.launch(int(
request.data.position))
except:
type, value, traceback = sys.exc_info()
if (self.__debug):
print('Error Deserialising %s: %s %s' %
(data_string, type, value))
def cleanup(self):
if (self.__piconzero_drive != None):
self.__piconzero_drive.cleanup()
from machine import Pin
import blesync_server
import blesync_uart.server
from motors import Motor
import pinout
from steering import Steering
motor_r = Motor(pinout.MOTOR_1A, pinout.MOTOR_2A, pinout.MOTOR_12EN)
motor_l = Motor(pinout.MOTOR_3A, pinout.MOTOR_4A, pinout.MOTOR_34EN)
steering = Steering(motor_l, motor_r)
built_in_led = Pin(2, Pin.OUT)
@blesync_uart.server.UARTService.on_message
def on_message(service, conn_handle, message):
if message == b'!B516':
steering.center(1000)
elif message == b'!B507':
steering.center(0)
elif message == b'!B615':
steering.center(-1000)
elif message == b'!B606':
steering.center(0)
elif message == b'!B813':
steering.right(1000)
elif message == b'!B804':
steering.right(0)
elif message == b'!B714':
steering.left(1000)
class ServoCalibration:
__kit = None
__looper = True
__keyboardInput = None
__steering = None
def __init__(self, servoKit):
self.__kit = servoKit
self.__keyboardInput = KeyboardInput("Steering Calibration")
self.__steering = Steering(
self.__kit,
steeringStatusFile=os.path.abspath(
os.path.join(os.path.dirname(__file__), "..")) +
"/config/steering_status.json")
def increment_position(self, index, value):
if (key == 'w'):
self.__steering.increment_position(index, 1)
self.__steering.update_servos()
elif (ord(key) == 16):
self.__steering.move_servo_by(index, 1)
elif (key == 'z'):
self.__steering.decrement_position(index, 1)
self.__steering.update_servos()
elif (ord(key) == 17):
self.__steering.move_servo_by(index, -1)
elif (key == 'q'):
self.__steering.save_steering_status()
waitForWheel = False
time.sleep(0.01)
def steering_angle(self, steering: Union[int, float]) -> None:
self.drive_msg.steering = Steering.to_can(int(steering))
#!/usr/bin/python3
import logging
import time
from steering import Steering
logging.basicConfig(format='%(asctime)s %(message)s',
filename='steering_systemtest.log',
level=logging.DEBUG)
if __name__ == "__main__":
logging.info('Steering system test: starting')
steering = Steering(steering_motor_address='outC',
steering_motor_speed_factor=10,
steering_speed=5,
steering_max_range=35,
steering_motor_position_factor=-1)
logging.info('=== Steering system test: Initialize ===')
steering.initialize()
logging.info('Steering system test: steering position = ' +
str(steering.get_current_steering_position()))
time.sleep(1)
logging.info('=== Steering system test: turn to 100% left ===')
steering.set_steering_position(-100)
logging.info('Steering system test: steering position = ' +
str(steering.get_current_steering_position()))
logging.info('Steering system test: steering motor state = ' +
str(steering.steering_motor.state))
time.sleep(1)
from steering import Steering from motor import Motor from time import sleep from capture import Capture #import config pygame.init() pygame.joystick.init() DrivingMotor = Motor(388) SteeringWheel = Steering(298) SteeringWheel.Run() axis = 0 stop = 0 autoMode = 1 joystick_pressed = 0 i = 0 steer_enhancer = 2 cap = Capture() while stop == 0: pygame.event.get() if pygame.joystick.get_count() == 1: # 1
class Car():
def __init__(self):
self.logger = logging.getLogger(self.__class__.__name__)
GPIO.setmode(GPIO.BCM)
self.steer = Steering()
self.brakes = False
self.has_turned = False
# motor driver
self.input1 = 20
self.input2 = 16
self.enable1 = 21 # always disable after finishing and\or in catch
# set GPIO direction (IN / OUT)
# we send instructions to motor driver IC
GPIO.setup(self.input1, GPIO.OUT)
GPIO.setup(self.input2, GPIO.OUT)
# this will determine if signal should be sent to output pins
GPIO.setup(self.enable1, GPIO.OUT)
def apply_brakes(self):
self.brakes = True
GPIO.output(self.enable1, GPIO.LOW)
return 'Applying Brakes'
def turn(self, angle):
self.steer.turn(angle)
return 'Turned ' + str(angle)
def reverse(self):
if self.brakes:
self.logger.debug('disabling brakes')
GPIO.output(self.enable1, GPIO.HIGH)
self.brakes = False
#Log('move start')
GPIO.output(self.enable1, GPIO.HIGH)
# if input1 and input2 are swapped a regular motor will go in reverse.
GPIO.output(self.input1, GPIO.HIGH)
# but lego will not as it has different pin for reverse. so we should disable this signal and enable that , creepy
GPIO.output(self.input2, GPIO.LOW)
return 'Driving Reverse'
def forward(self):
#Log('move start')
# if input1 and input2 are swapped we go in reverse.
if self.brakes:
self.logger.debug('disabling brakes')
GPIO.output(self.enable1, GPIO.HIGH)
self.brakes = False
GPIO.output(self.enable1, GPIO.HIGH)
GPIO.output(self.input1, GPIO.LOW)
GPIO.output(self.input2, GPIO.HIGH)
return 'Driving Forward'
def clear(self):
self.steer.stop()
GPIO.output(self.input1, GPIO.LOW)
GPIO.output(self.enable1, GPIO.LOW)
GPIO.cleanup()
return 'Engine Off'
class Sense():
def __init__(self):
config = configparser.ConfigParser()
config.read("config.ini")
trigger = config.getint("ultrasonic", "Trigger")
echo = config.getint("ultrasonic", "Echo")
servoNum = config.getint("steering", "servoNum")
InitPosition = config.getint("steering", "InitPosition")
minPosition = config.getint("steering", "MinPosition")
maxPosition = config.getint("steering", "maxPosition")
speed = config.getint("steering", "speed")
self.IN_1 = config.getint("car", "IN1")
self.IN_2 = config.getint("car", "IN2")
self.IN_3 = config.getint("car", "IN3")
self.IN_4 = config.getint("car", "IN4")
self.Sonic = Ultrasonic(trigger, echo)
self.Servo = Steering(servoNum, InitPosition, minPosition, maxPosition,
speed)
self.motor = Motor(self.IN_1, self.IN_2, self.IN_3, self.IN_4)
#get obstacles type
def detection(self):
distance = self.ask_distance()
#NO Obstacles or 35cm safe distance type 0
if distance >= 30:
return "Fgo"
#Obstacles ahead
#self.Infrared.check_distance() == "Warning"
else:
l_d = self.ask_distance_l()
r_d = self.ask_distance_r()
#Obstacles ahead&&right =safe type a
if ((l_d >= 30) and (r_d >= 30)):
if (l_d > r_d):
return "Lgo"
else:
return "Rgo"
#Obstacles ahead&&right R>L type b
elif ((l_d <= 30) and (r_d > 30)):
return "Rgo"
#Obstacles ahead&&right L>R type c
elif ((l_d > 30) and (r_d <= 30)):
return "Lgo"
#Obstacles ahead&&right L&R<safe type d
elif ((l_d < 30) and (r_d < 30)):
return "Bgo"
def ask_distance(self):
self.Servo.reset()
return self.Sonic.get_distance()
def ask_distance_l(self):
self.motor.stop()
self.Servo.turnleft()
time.sleep(0.1)
distance = self.Sonic.get_distance()
self.Servo.reset()
return distance
def ask_distance_r(self):
self.motor.stop()
self.Servo.turnright()
time.sleep(0.1)
distance = self.Sonic.get_distance()
self.Servo.reset()
return distance
class SteeringUnitTest(unittest.TestCase):
def setUp(self):
self.steering = Steering(TEST_STEERING_MOTOR_ADDRESS,
TEST_STEERING_MOTOR_SPEED_FACTOR,
TEST_STEERING_SPEED, TEST_STEERING_MAX_RANGE,
TEST_STEERING_MOTOR_POSITION_FACTOR, True)
self.steering.steering_motor = MagicMock()
def test_shouldCalculateCorrectCurrentSteeringPositionWhenTurnedLeft(self):
# given
__test_steering_motor_position = -21
self.steering.steering_motor.position = __test_steering_motor_position
# when
__actual_steering_position = self.steering.get_current_steering_position(
)
# then
__expected_steering_position = self.__get_expected_steering_position(
__test_steering_motor_position)
self.assertEqual(__expected_steering_position,
__actual_steering_position)
def test_shouldCalculateCorrectCurrentSteeringPositionWhenTurnedMaxLeft(
self):
# given
self.steering.steering_motor.position = -TEST_STEERING_MAX_RANGE
# when
__actual_steering_position = self.steering.get_current_steering_position(
)
# then
__expected_steering_position = TEST_STEERING_MOTOR_POSITION_FACTOR * -100
self.assertEqual(__expected_steering_position,
__actual_steering_position)
def test_shouldCalculateCorrectCurrentSteeringPositionWhenTurnedRight(
self):
# given
__test_steering_motor_position = 18
self.steering.steering_motor.position = __test_steering_motor_position
# when
__actual_steering_position = self.steering.get_current_steering_position(
)
# then
__expected_steering_position = self.__get_expected_steering_position(
__test_steering_motor_position)
self.assertEqual(__expected_steering_position,
__actual_steering_position)
def test_shouldCalculateCorrectCurrentSteeringPositionWhenTurnedMaxRight(
self):
# given
self.steering.steering_motor.position = TEST_STEERING_MAX_RANGE
# when
__actual_steering_position = self.steering.get_current_steering_position(
)
# then
__expected_steering_position = TEST_STEERING_MOTOR_POSITION_FACTOR * 100
self.assertEqual(__expected_steering_position,
__actual_steering_position)
def test_shouldCalculateCorrectCurrentSteeringPositionWhenAtCenter(self):
# given
self.steering.steering_motor.position = 0
# when
__actual_steering_position = self.steering.get_current_steering_position(
)
# then
self.assertEqual(0, __actual_steering_position)
def test_shouldResetSteeringPositionToCenterWhenInitialized(self):
# given
self.steering.steering_motor.position = 32
# when
self.steering.initialize()
# then
self.steering.steering_motor.reset.assert_called_once()
def test_shouldTurnMaxRangeRightForCenteringWhenInitialized(self):
# given
self.steering.steering_motor.position = 32
# when
self.steering.initialize()
# then
__expected_steering_motor_turn = TEST_STEERING_MOTOR_POSITION_FACTOR * TEST_STEERING_MAX_RANGE
self.steering.steering_motor.run_to_rel_pos.assert_called_with(
position_sp=__expected_steering_motor_turn,
speed_sp=EXPECTED_STEERING_MOTOR_SPEED,
stop_action=EXPECTED_STEERING_MOTOR_STOP_ACTION)
def test_should_turn_steering_motor_left_by_correct_amount_when_steering_motor_position_was_zero(
self):
# given
__test_steering_motor_initial_position = 0
__test_steering_position = -50
self.steering.steering_motor.position = __test_steering_motor_initial_position
# when
self.steering.set_steering_position(__test_steering_position)
# then
__expected_steering_motor_turn = self.__get_expected_steering_motor_turn(
__test_steering_position)
self.steering.steering_motor.run_to_abs_pos.assert_called_with(
position_sp=__expected_steering_motor_turn,
speed_sp=EXPECTED_STEERING_MOTOR_SPEED,
stop_action=EXPECTED_STEERING_MOTOR_STOP_ACTION)
def test_should_turn_steering_motor_right_by_correct_amount_when_steering_motor_position_was_zero(
self):
# given
__test_steering_motor_initial_position = 0
__test_steering_position = 66
self.steering.steering_motor.position = __test_steering_motor_initial_position
# when
self.steering.set_steering_position(__test_steering_position)
# then
__expected_steering_motor_turn = self.__get_expected_steering_motor_turn(
__test_steering_position)
self.steering.steering_motor.run_to_abs_pos.assert_called_with(
position_sp=__expected_steering_motor_turn,
speed_sp=EXPECTED_STEERING_MOTOR_SPEED,
stop_action=EXPECTED_STEERING_MOTOR_STOP_ACTION)
def test_should_turn_steering_motor_left_by_correct_amount_when_steering_motor_position_was_in_left(
self):
# given
__test_steering_motor_initial_position = -10
__test_steering_position = -50
self.steering.steering_motor.position = __test_steering_motor_initial_position
# when
self.steering.set_steering_position(__test_steering_position)
# then
__expected_steering_motor_turn = self.__get_expected_steering_motor_turn(
__test_steering_position)
self.steering.steering_motor.run_to_abs_pos.assert_called_with(
position_sp=__expected_steering_motor_turn,
speed_sp=EXPECTED_STEERING_MOTOR_SPEED,
stop_action=EXPECTED_STEERING_MOTOR_STOP_ACTION)
def test_should_turn_steering_motor_left_by_correct_amount_when_steering_motor_position_was_in_right(
self):
# given
__test_steering_motor_initial_position = 15
__test_steering_position = -50
self.steering.steering_motor.position = __test_steering_motor_initial_position
# when
self.steering.set_steering_position(__test_steering_position)
# then
__expected_steering_motor_turn = self.__get_expected_steering_motor_turn(
__test_steering_position)
self.steering.steering_motor.run_to_abs_pos.assert_called_with(
position_sp=__expected_steering_motor_turn,
speed_sp=EXPECTED_STEERING_MOTOR_SPEED,
stop_action=EXPECTED_STEERING_MOTOR_STOP_ACTION)
def test_should_turn_steering_motor_right_by_correct_amount_when_steering_motor_position_was_in_right(
self):
# given
__test_steering_motor_initial_position = 15
__test_steering_position = 60
self.steering.steering_motor.position = __test_steering_motor_initial_position
# when
self.steering.set_steering_position(__test_steering_position)
# then
__expected_steering_motor_turn = self.__get_expected_steering_motor_turn(
__test_steering_position)
self.steering.steering_motor.run_to_abs_pos.assert_called_with(
position_sp=__expected_steering_motor_turn,
speed_sp=EXPECTED_STEERING_MOTOR_SPEED,
stop_action=EXPECTED_STEERING_MOTOR_STOP_ACTION)
def test_should_turn_steering_motor_right_by_correct_amount_when_steering_motor_position_was_in_left(
self):
# given
__test_steering_motor_initial_position = -20
__test_steering_position = 60
self.steering.steering_motor.position = __test_steering_motor_initial_position
# when
self.steering.set_steering_position(__test_steering_position)
# then
__expected_steering_motor_turn = self.__get_expected_steering_motor_turn(
__test_steering_position)
self.steering.steering_motor.run_to_abs_pos.assert_called_with(
position_sp=__expected_steering_motor_turn,
speed_sp=EXPECTED_STEERING_MOTOR_SPEED,
stop_action=EXPECTED_STEERING_MOTOR_STOP_ACTION)
@staticmethod
def __get_expected_steering_motor_turn(desired_steering_position):
return TEST_STEERING_MOTOR_POSITION_FACTOR * (int(
desired_steering_position * TEST_STEERING_MAX_RANGE / 100))
@staticmethod
def __get_expected_steering_position(steering_motor_position):
return TEST_STEERING_MOTOR_POSITION_FACTOR * int(
steering_motor_position * 100 / TEST_STEERING_MAX_RANGE)
class Camera:
def __init__(self):
config = configparser.ConfigParser()
config.read("./config.ini")
HIntfNum = config.getint("camera", "HIntfNum")
HInitPosition = config.getint("camera", "HInitPosition")
HMinPosition = config.getint("camera", "HMinPosition")
HMaxPosition = config.getint("camera", "HMaxPosition")
HSpeed = config.getint("camera", "HSpeed")
# Vertical direction control parameters
VIntfNum = config.getint("camera", "VIntfNum")
VInitPosition = config.getint("camera", "VInitPosition")
VMinPosition = config.getint("camera", "VMinPosition")
VMaxPosition = config.getint("camera", "VMaxPosition")
VSpeed = config.getint("camera", "VSpeed")
self.HCameraControl = Steering(HIntfNum, HInitPosition, HMinPosition,
HMaxPosition, HSpeed)
self.VCameraControl = Steering(VIntfNum, VInitPosition, VMinPosition,
VMaxPosition, VSpeed)
def cameraRotate(self, direction):
if direction == "A":
self.HCameraControl.forwardRotation()
elif direction == "D":
self.HCameraControl.reverseRotation()
elif direction == "W":
self.VCameraControl.forwardRotation()
elif direction == "S":
self.VCameraControl.reverseRotation()
elif direction == "R":
self.HCameraControl.reset()
self.VCameraControl.reset()
else:
print(
"Your input for camera direction is wrong, please input: D, A, W, S or RESET!"
)
from motor import Motor
from time import sleep
from predict import Predict
#import config
pygame.init()
pygame.joystick.init()
prediction = Predict("VGG16_RealImg_Adagrad_150x2.EP74-0.62-0.04.h5", 150, 150)
DrivingMotor = Motor(388)
SteeringWheel = Steering(298)
SteeringWheel.Run()
axis = 0
stop = 0
autoMode = 0
joystick_pressed = 0
steer_enhancer = 2
while stop == 0:
pygame.event.get()
if pygame.joystick.get_count() == 1: # 1
Joystick = pygame.joystick.Joystick(0)
def drive(self, v: float, s: float) -> None:
print(f'Setting velocity {Driving.to_can(int(v))}')
self.drive_msg.velocity = Driving.to_can(int(v))
print(f'Setting steering {Steering.to_can(int(s))}')
self.drive_msg.steering = Steering.to_can(int(s))
class Main():
ub = UltraBorg3.UltraBorg()
tb = ThunderBorg3.ThunderBorg()
MODE_STRAIGHT_LINE_SPEED = 0
MODE_OVER_THE_RAINBOW = 1
MODE_MAZE_SOLVE = 2
MODE_DERANGED_GOLF = 3
MODE_DUCK_SHOOT = 4
MODE_OBSTACLE_COURSE = 5
MODE_PI_NOON = 6
MODE_TEST = 99
mode = MODE_OBSTACLE_COURSE
def __init__(self):
self.ub.Init()
self.tb.Init()
self.t1 = datetime.now()
self.tickSpeed = 0.05
self.us = Ultrasonics(self.ub)
self.motors = Motors(self.tb, self.ub, self.tickSpeed)
self.steering = Steering(self.tb, self.ub, self.tickSpeed)
self.teleLogger = Telemetry("telemetry", "csv").get()
self.ptc = PanTiltController(self.ub, 270, 135)
self.joystick_controller = JoystickController(self.tb, self.ub)
battMin, battMax = self.tb.GetBatteryMonitoringLimits()
battCurrent = self.tb.GetBatteryReading()
print('Battery monitoring settings:')
print(' Minimum (red) %02.2f V' % (battMin))
print(' Half-way (yellow) %02.2f V' % ((battMin + battMax) / 2))
print(' Maximum (green) %02.2f V' % (battMax))
print()
print(' Current voltage %02.2f V' % (battCurrent))
print(' Mode %s' % (self.mode))
print()
def log(self):
if (self.motors.speed != 0):
self.teleLogger.info(
'%(left)f, %(front)f, %(right)f, %(back)f, %(distanceMoved)f, %(forwardSpeed)f, %(direction)s, %(degree)f, %(ratio)f',
{
"left": self.us.left,
"front": self.us.front,
"right": self.us.right,
"back": self.us.back,
"distanceMoved": self.motors.distanceMoved,
"forwardSpeed": self.motors.forwardSpeed,
"direction": self.steering.going,
"degree": self.steering.steeringPosition,
"ratio": self.steering.sideRatio
})
def run(self):
try:
if (self.mode == self.MODE_STRAIGHT_LINE_SPEED):
print("Straight line speed")
self.modeStraightLineSpeed()
elif (self.mode == self.MODE_OVER_THE_RAINBOW):
print("Rainbow")
self.modeOverTheRainbow()
elif (self.mode == self.MODE_DERANGED_GOLF
or self.mode == self.MODE_PI_NOON
or self.mode == self.MODE_OBSTACLE_COURSE):
print("Joystick control: " + str(self.mode))
self.joystick_controller.run()
else:
# Wait for Buttons
print('Pan -62 = ' + str(self.ptc.pan(-62)))
time.sleep(1)
print('Pan 0 = ' + str(self.ptc.pan(0)))
time.sleep(1)
#
# print('Pan 62 = ' + str(self.ptc.pan(62)))
# time.sleep(1)
#
# print('Pan absolute 1.0 = ' + str(self.ptc.pan_absolute(1.0)))
# time.sleep(1)
#
# print('Pan absolute -1.0 = ' + str(self.ptc.pan_absolute(-1.0)))
# time.sleep(1)
#
# print('Pan absolute 0.0 = ' + str(self.ptc.pan_absolute(0.0)))
# time.sleep(1)
#
# print('Tilt -30 = ' + str(self.ptc.tilt(-30)))
# time.sleep(1)
# print('Tilt 0 = ' + str(self.ptc.tilt(0)))
# time.sleep(1)
#
# print('Tilt 30 = ' + str(self.ptc.tilt(30)))
# time.sleep(1)
#
# print('Tilt absolute 0.6 = ' + str(self.ptc.tilt_absolute(0.6)))
# time.sleep(1)
#
# print('Tilt absolute -0.6 = ' +
# str(self.ptc.tilt_absolute(-0.6)))
# time.sleep(1)
#
# print('Tilt absolute 0.0 = ' + str(self.ptc.tilt_absolute(0.0)))
# time.sleep(1)
except KeyboardInterrupt:
# User has pressed CTRL+C
self.ub.SetServoPosition2(0)
t2 = datetime.now()
delta = t2 - self.t1
print("Run complete in : " + str(delta.total_seconds()))
print('Done')
if (self.motors):
self.motors.shutdown()
except Exception as e:
tb = traceback.format_exc()
e = sys.exc_info()[0]
print(tb)
if (self.motors):
self.motors.shutdown()
def modeStraightLineSpeed(self):
self.straight_line_speed = StraightLineVision(self.steering,
self.motors)
self.teleLogger.info(
'left, front, right, back, distanceMoved, forwardSpeed, direction, steering position, ratio'
)
self.steering.reset()
slVa = VisionAttributes()
slVa.startTiltAngle = 0.6
slVa.startPanAngle = 0
slVa.targetMinSize = 1000
slVa.targetMaxSize = 18000
slVa.minPanAngle = -0.5
slVa.maxPanAngle = 0.5
slVa.colour = Vision.COLOUR_WHITE
slVa.targetColorPattern = Vision.COLOUR_WHITE
slVa.topSpeed = 0.6
slVa.topSpinSpeed = 1.0
self.ptc.tilt(0.5)
slsPtc = PanTiltController(self.ub, 270, 135)
slsPtc.initPanServo(5000, 1000)
self.straight_line_speed.initialise(slVa, slsPtc)
self.motors.stop()
prev_block_position = None
t1 = datetime.now()
self.straight_line_speed.track(self.straight_line_speed.COLOUR_WHITE)
t2 = datetime.now()
delta = t2 - self.t1
print("Run complete in: " + str(delta.total_seconds()))
def modeOverTheRainbow(self):
self.vision = Vision(self.steering, self.motors)
slVa = VisionAttributes()
slVa.startTiltAngle = 0.12
slVa.startPanAngle = -1.00
slVa.targetMinSize = 20
slVa.targetMaxSize = 2200
slVa.minPanAngle = -1.0
slVa.maxPanAngle = 1.0
slVa.targetColorPattern = Vision.COLOUR_WHITE
slVa.topSpeed = 1.0
slVa.topSpinSpeed = 1.0
self.motors.move(-1, -1, 0.3)
time.sleep(0.8)
self.motors.stop()
rainbowPtc = PanTiltController(self.ub, 270, 135)
rainbowPtc.initPanServo(5000, 1000)
self.vision.initialise(slVa, rainbowPtc)
time.sleep(0.5)
self.vision.scan()
print("Scan Complete")
index = 0
prev_position = 0
for ball_position in self.vision.ball_positions:
ball_position = self.vision.ball_positions[0]
print("Size: " + str(ball_position.size) + ', x :' +
str(ball_position.x) + ', y :' + str(ball_position.y) +
', pan-position :' + str(ball_position.pan_position) +
', angle : ' + str(ball_position.pan_position * 135) +
', Colour:' + str(ball_position.colour))
if (index > 0):
prev_position = self.vision.ball_positions[index -
1].pan_position
index = index + 1
self.vision.goto_ball_position(ball_position, prev_position)