def __init__(self, motorhat_addr=0x6f):
# Setup the motorhat with the passed in address
self._mh = Raspi_MotorHAT(addr=motorhat_addr)
self.left_motor = self._mh.getMotor(1)
self.right_motor = self._mh.getMotor(2)
# Setup the Leds
self.leds = leds_led_shim.Leds()
# Set up servo motors for pan and tilt.
self.servos = Servos(addr=motorhat_addr, deflect_90_in_ms=1)
# Setup The Distance Sensors
self.left_distance_sensor = DistanceSensor(echo=17,
trigger=27,
queue_len=2)
self.right_distance_sensor = DistanceSensor(echo=5,
trigger=6,
queue_len=2)
# Setup the Encoders
EncoderCounter.set_constants(self.wheel_diameter_mm,
self.ticks_per_revolution)
self.left_encoder = EncoderCounter(4)
self.right_encoder = EncoderCounter(26)
# ensure the motors get stopped when the code exits
atexit.register(self.stop_all)
def __init__(self, motorhat_addr=0x6f, drive_enabled=True):
# Setup the motorhat with the passed in address
self._mh = Raspi_MotorHAT(addr=motorhat_addr)
# get local variable for each motor
self.left_motor = self._mh.getMotor(1)
self.right_motor = self._mh.getMotor(2)
self.drive_enabled = drive_enabled
# ensure the motors get stopped when the code exits
atexit.register(self.stop_all)
# Setup the line sensors
self.left_line_sensor = LineSensor(23, queue_len=3, pull_up=True)
self.right_line_sensor = LineSensor(16, queue_len=3, pull_up=True)
# Setup The Distance Sensors
self.left_distance_sensor = DistanceSensor(17, 27)
self.right_distance_sensor = DistanceSensor(5, 6)
# Setup the Encoders
EncoderCounter.set_constants(self.wheel_diameter_mm,
self.ticks_per_revolution)
self.left_encoder = EncoderCounter(4)
self.right_encoder = EncoderCounter(26)
# Setup the Leds
self.leds = leds_8_apa102c.Leds()
# Set up servo motors for pan and tilt.
self.servos = Servos(addr=motorhat_addr)
def __init__(self):
self.GPIO_FRONTTRIGGER = 20
self.GPIO_BACKTRIGGER = 5
self.GPIO_FRONTECHO = 6
self.GPIO_BACKECHO = 12
FRONTSERVO = 6
BACKSERVO = 7
#set GPIO direction (IN / OUT)
GPIO.setmode(GPIO.BCM)
GPIO.setup(self.GPIO_FRONTECHO, GPIO.IN)
GPIO.setup(self.GPIO_BACKECHO, GPIO.IN)
GPIO.setup(self.GPIO_FRONTTRIGGER, GPIO.OUT)
GPIO.output(self.GPIO_FRONTTRIGGER, False)
GPIO.setup(self.GPIO_BACKTRIGGER, GPIO.OUT)
GPIO.output(self.GPIO_BACKTRIGGER, False)
# initialise direction servos
self.servos = Servos(FRONTSERVO, BACKSERVO)
servoPositions = self.servos.FirstScanPosition()
self.frontServoDirection = servoPositions[0]
self.backServoDirection = servoPositions[1]
self.scannerActive = False
self.endthread = False
self.HistoryFront = [[0.0, 0.0, 0.0, 0.0, 0.0]]
self.HistoryBack = [[0.0, 0.0, 0.0, 0.0, 0.0]]
self.FrontDeltas = [[0.0, 0.0, 0.0, 0.0, 0.0]]
self.BackDeltas = [[0.0, 0.0, 0.0, 0.0, 0.0]]
self.FrontDeltaDelta = [0.0, 0.0, 0.0, 0.0, 0.0]
self.BackDeltaDelta = [0.0, 0.0, 0.0, 0.0, 0.0]
# initialise current distance readings
self.frontDistance = {
ServoDirection.Left: -1.0,
ServoDirection.OffLeft: -1.0,
ServoDirection.Ahead: -1.0,
ServoDirection.OffRight: -1.0,
ServoDirection.Right: -1.0
}
self.backDistance = {
ServoDirection.Left: -1.0,
ServoDirection.OffLeft: -1.0,
ServoDirection.Ahead: -1.0,
ServoDirection.OffRight: -1.0,
ServoDirection.Right: -1.0
}
time.sleep(1)
class RobotRunner:
def __init__(self):
print("Initialising")
self.servos=Servos()
self.Kinematic=Kinematic()
def calibrate(self):
print("Running")
while True:
servo=int(input("Enter Servo to calibrate (0-12)"))
# print("Current Offset is {}".format(self.servos.getServoOffset(servo)))
offset=int(input("Enter new Offset:"))
self.servos.angle(servo,offset)
def __init__(self, motorhat_addr=0x6f):
# set up motorhat with address parameter
self._mh = Raspi_MotorHAT(addr=motorhat_addr)
# get local variable for each motor
self.left_motor_rear = self._mh.getMotor(1)
self.left_motor_front = self._mh.getMotor(2)
self.right_motor_rear = self._mh.getMotor(4)
self.right_motor_front = self._mh.getMotor(3)
# make sure motors stop when code exits
atexit.register(self.stop_motors)
# Set up servo motors for pan and tilt
self.servos = Servos(addr=motorhat_addr)
def __init__(self, motorhat_addr=0x6f):
self._mh = Raspi_MotorHAT(addr=motorhat_addr)
self.left_motor = self._mh.getMotor(1)
self.right_motor = self._mh.getMotor(2)
atexit.register(self.stop_motors)
# Setup the line sensors
self.right_line_sensor = LineSensor(23, queue_len=3, pull_up=True)
self.left_line_sensor = LineSensor(16, queue_len=3, pull_up=True)
self.right_line_sensor_stuck = ""
self.left_line_sensor_stuck = ""
self.servos = Servos(addr=motorhat_addr)
def __init__(self, motorhat_addr=0x6f):
# Setup the motorhat with the passed in address
self._mh = Raspi_MotorHAT(addr=motorhat_addr)
# get local variable for each motor
self.left_motor = self._mh.getMotor(1)
self.right_motor = self._mh.getMotor(2)
# ensure the motors get stopped when the code exits
atexit.register(self.stop_all)
# Setup the line sensors
self.left_line_sensor = LineSensor(23, queue_len=3, pull_up=True)
self.right_line_sensor = LineSensor(16, queue_len=3, pull_up=True)
# Setup the Leds
self.leds = leds_8_apa102c.Leds()
# Set up servo motors for pan and tilt.
self.servos = Servos(addr=motorhat_addr)
def __init__(self):
self.display = None
print("Booting SpotMicroAI")
# self.display=RobotDisplay()
self.gyro = Gyro()
self.servos = Servos()
self.Kinematic = Kinematic()
self.network = Network()
# For testing purposed
self.mode = ModeStandby(self.servos)
def __init__(self, config_file): self.config = Config() self.config.read_config(config_file) self.servos = Servos(16, self.config.mimimum_pulse, self.config.maximum_pulse, self.config.kill_angle) legs = [] for i in range(4): leg = Leg(i, ) leg.servos(*(self.servos.servo[x] for x in leg_pins[i])) leg.offsets(*offsets[i]) leg.limbs(*lengths) legs.append(leg) self.leg0, self.leg1, self.leg2, self.leg3 = legs
def __init__(self, config_file):
self.config = Config()
self.config.read_config(config_file)
self.servos = Servos(16, self.config.mimimum_pulse, self.config.maximum_pulse, self.config.kill_angle, self.config.angle_offsets)
legs = []
for i in range(4):
leg_config = self.config["leg"+str(i+1)]
leg = Leg(leg_config["quadrants"], leg_config["positions"])
leg.servos(*(self.servos[x] for x in leg_config["servo_pins"]))
leg.limbs(*leg_config["limb_lengths"])
legs.append(leg)
self.leg0, self.leg1, self.leg2, self.leg3 = legs
def __init__(self, motorhat_addr=0x6f):
# Setup the motorhat with the passed in address
self._mh = Raspi_MotorHAT(addr=motorhat_addr)
# get local variable for each motor
self.left_motor = self._mh.getMotor(1)
self.right_motor = self._mh.getMotor(2)
# Setup The Distance Sensors
self.left_distance_sensor = DistanceSensor(echo=17,
trigger=27,
queue_len=2)
self.right_distance_sensor = DistanceSensor(echo=5,
trigger=6,
queue_len=2)
# Setup the Leds
self.leds = leds_led_shim.Leds()
# Set up servo motors for pan and tilt.
self.servos = Servos(addr=motorhat_addr)
# ensure the motors get stopped when the code exits
atexit.register(self.stop_all)
class Robot(object):
wheel_diameter_mm = 69.0
ticks_per_revolution = 40.0
wheel_distance_mm = 140.0
def __init__(self, motorhat_addr=0x6f, drive_enabled=True):
# Setup the motorhat with the passed in address
self._mh = Raspi_MotorHAT(addr=motorhat_addr)
# get local variable for each motor
self.left_motor = self._mh.getMotor(1)
self.right_motor = self._mh.getMotor(2)
self.drive_enabled = drive_enabled
# ensure the motors get stopped when the code exits
atexit.register(self.stop_all)
# Setup the line sensors
self.left_line_sensor = LineSensor(23, queue_len=3, pull_up=True)
self.right_line_sensor = LineSensor(16, queue_len=3, pull_up=True)
# Setup The Distance Sensors
self.left_distance_sensor = DistanceSensor(17, 27)
self.right_distance_sensor = DistanceSensor(5, 6)
# Setup the Encoders
EncoderCounter.set_constants(self.wheel_diameter_mm,
self.ticks_per_revolution)
self.left_encoder = EncoderCounter(4)
self.right_encoder = EncoderCounter(26)
# Setup the Leds
self.leds = leds_8_apa102c.Leds()
# Set up servo motors for pan and tilt.
self.servos = Servos(addr=motorhat_addr)
def stop_all(self):
self.stop_motors()
# Clear any sensor handlers
self.left_line_sensor.when_line = None
self.left_line_sensor.when_no_line = None
self.right_line_sensor.when_line = None
self.right_line_sensor.when_no_line = None
self.left_encoder.stop()
self.right_encoder.stop()
# Clear the display
self.leds.clear()
self.leds.show()
# Reset the servos
self.servos.stop_all()
def convert_speed(self, speed):
mode = Raspi_MotorHAT.RELEASE
if speed > 0:
mode = Raspi_MotorHAT.FORWARD
elif speed < 0:
mode = Raspi_MotorHAT.BACKWARD
output_speed = (abs(speed) * 255) / 100
return mode, output_speed
def set_left(self, speed):
if not self.drive_enabled:
return
mode, output_speed = self.convert_speed(speed)
self.left_motor.setSpeed(output_speed)
self.left_motor.run(mode)
def set_right(self, speed):
if not self.drive_enabled:
return
mode, output_speed = self.convert_speed(speed)
self.right_motor.setSpeed(output_speed)
self.right_motor.run(mode)
def stop_motors(self):
self.left_motor.run(Raspi_MotorHAT.RELEASE)
self.right_motor.run(Raspi_MotorHAT.RELEASE)
def set_pan(self, angle):
self.servos.set_servo_angle(1, angle)
def set_tilt(self, angle):
self.servos.set_servo_angle(0, angle)
dirr = "Downloads/DA/"
models_dirr = dirr + "models/"
sounds_dirr = dirr + "sounds/"
video_dirr = dirr + "video/"
MobileNetSSDDir = models_dirr + "MobileNetSSD/"
weight_file = models_dirr + "model.hdf5"
video_output_file = video_dirr + "test.h264"
model = MobileNetSSDDir + "MobileNetSSD_deploy.caffemodel"
prototxt = MobileNetSSDDir + "MobileNetSSD_deploy.prototxt.txt"
directions = ['center', 'left', 'right']
labels_name = {'center': 0, 'left': 1, 'right': 2}
# Servos and button
Servos.initServos()
Button.initButton()
# Model
DetectWay.initModel()
DetectWay.loadWeight(weight_file)
DetectObject.loadModel(prototxt, model)
# initialize the camera and grab a reference to the raw camera capture
camera = PiCamera()
camera.resolution = (720, 480)
camera.framerate = 30
idxs = [0, 0, 0]
print("Thu bam nut")
while True:
class Robot:
wheel_diameter_mm = 70.0
ticks_per_revolution = 40.0
wheel_distance_mm = 132.0
def __init__(self, motorhat_addr=0x6f):
# Setup the motorhat with the passed in address
self._mh = Raspi_MotorHAT(addr=motorhat_addr)
self.left_motor = self._mh.getMotor(1)
self.right_motor = self._mh.getMotor(2)
# Setup the Leds
self.leds = leds_led_shim.Leds()
# Set up servo motors for pan and tilt.
self.servos = Servos(addr=motorhat_addr, deflect_90_in_ms=1)
# Setup The Distance Sensors
self.left_distance_sensor = DistanceSensor(echo=17,
trigger=27,
queue_len=2)
self.right_distance_sensor = DistanceSensor(echo=5,
trigger=6,
queue_len=2)
# Setup the Encoders
EncoderCounter.set_constants(self.wheel_diameter_mm,
self.ticks_per_revolution)
self.left_encoder = EncoderCounter(4)
self.right_encoder = EncoderCounter(26)
# ensure the motors get stopped when the code exits
atexit.register(self.stop_all)
def convert_speed(self, speed):
# Choose the running mode
mode = Raspi_MotorHAT.RELEASE
if speed > 0:
mode = Raspi_MotorHAT.FORWARD
elif speed < 0:
mode = Raspi_MotorHAT.BACKWARD
# Scale the speed
output_speed = (abs(speed) * 255) // 100
return mode, int(output_speed)
def set_left(self, speed):
mode, output_speed = self.convert_speed(speed)
self.left_motor.setSpeed(output_speed)
self.left_motor.run(mode)
def set_right(self, speed):
mode, output_speed = self.convert_speed(speed)
self.right_motor.setSpeed(output_speed)
self.right_motor.run(mode)
def stop_motors(self):
self.left_motor.run(Raspi_MotorHAT.RELEASE)
self.right_motor.run(Raspi_MotorHAT.RELEASE)
def stop_all(self):
self.stop_motors()
# Clear the display
self.leds.clear()
self.leds.show()
# Clear any sensor handlers
self.left_encoder.stop()
self.right_encoder.stop()
# Reset the servos
self.servos.stop_all()
def set_pan(self, angle):
self.servos.set_servo_angle(1, angle)
def set_tilt(self, angle):
self.servos.set_servo_angle(0, angle)
class Robot(object):
def __init__(self, motorhat_addr=0x6f):
# Setup the motorhat with the passed in address
self._mh = Raspi_MotorHAT(addr=motorhat_addr)
# get local variable for each motor
self.left_motor = self._mh.getMotor(1)
self.right_motor = self._mh.getMotor(2)
# ensure the motors get stopped when the code exits
atexit.register(self.stop_all)
# Setup the line sensors
self.left_line_sensor = LineSensor(23, queue_len=3, pull_up=True)
self.right_line_sensor = LineSensor(16, queue_len=3, pull_up=True)
# Setup the Leds
self.leds = leds_8_apa102c.Leds()
# Set up servo motors for pan and tilt.
self.servos = Servos(addr=motorhat_addr)
def stop_all(self):
self.stop_motors()
# Clear any sensor handlers
self.left_line_sensor.when_line = None
self.left_line_sensor.when_no_line = None
self.right_line_sensor.when_line = None
self.right_line_sensor.when_no_line = None
# Clear the display
self.leds.clear()
self.leds.show()
# Reset the servos
self.servos.stop_all()
def convert_speed(self, speed):
mode = Raspi_MotorHAT.RELEASE
if speed > 0:
mode = Raspi_MotorHAT.FORWARD
elif speed < 0:
mode = Raspi_MotorHAT.BACKWARD
output_speed = (abs(speed) * 255) / 100
return mode, int(output_speed)
def set_left(self, speed):
mode, output_speed = self.convert_speed(speed)
self.left_motor.setSpeed(output_speed)
self.left_motor.run(mode)
def set_right(self, speed):
mode, output_speed = self.convert_speed(speed)
self.right_motor.setSpeed(output_speed)
self.right_motor.run(mode)
def stop_motors(self):
self.left_motor.run(Raspi_MotorHAT.RELEASE)
self.right_motor.run(Raspi_MotorHAT.RELEASE)
def set_pan(self, angle):
self.servos.set_servo_angle(1, angle)
def set_tilt(self, angle):
self.servos.set_servo_angle(0, angle)
class Robot(object):
def __init__(self, motorhat_addr=0x6f):
# set up motorhat with address parameter
self._mh = Raspi_MotorHAT(addr=motorhat_addr)
# get local variable for each motor
self.left_motor_rear = self._mh.getMotor(1)
self.left_motor_front = self._mh.getMotor(2)
self.right_motor_rear = self._mh.getMotor(4)
self.right_motor_front = self._mh.getMotor(3)
# make sure motors stop when code exits
atexit.register(self.stop_motors)
# Set up servo motors for pan and tilt
self.servos = Servos(addr=motorhat_addr)
# convert speed from 0-100 to robot speeds
def convert_speed(self, speed):
# choose the running mode
mode = Raspi_MotorHAT.RELEASE
if speed > 0:
mode = Raspi_MotorHAT.FORWARD
elif speed < 0:
mode = Raspi_MotorHAT.BACKWARD
#Scale the speed
output_speed = (abs(speed) * 255) / 100
return mode, output_speed
# release motors
def stop_motors(self):
self.left_motor_rear.run(Raspi_MotorHAT.RELEASE)
self.right_motor_rear.run(Raspi_MotorHAT.RELEASE)
self.left_motor_front.run(Raspi_MotorHAT.RELEASE)
self.right_motor_front.run(Raspi_MotorHAT.RELEASE)
# sets speeds of left wheels
def set_left(self, speed):
mode, output_speed = self.convert_speed(speed)
self.left_motor_rear.setSpeed(output_speed)
self.left_motor_front.setSpeed(output_speed)
self.left_motor_rear.run(mode)
self.left_motor_front.run(mode)
# sets speeds of right wheels
def set_right(self, speed):
mode, output_speed = self.convert_speed(speed)
self.right_motor_rear.setSpeed(output_speed)
self.right_motor_front.setSpeed(output_speed)
self.right_motor_rear.run(mode)
self.right_motor_front.run(mode)
def move(self, speed):
self.set_left(speed)
self.set_right(speed)
def set_pan(self, angle):
self.servos.set_servo_angle(1, angle)
def set_tilt(self, angle):
self.servos.set_servo_angle(0, angle)
def stop_all(self):
self.stop_motors()
# Reset the servos
self.servos.stop_all()
class DistanceSensors:
def __init__(self):
self.GPIO_FRONTTRIGGER = 20
self.GPIO_BACKTRIGGER = 5
self.GPIO_FRONTECHO = 6
self.GPIO_BACKECHO = 12
FRONTSERVO = 6
BACKSERVO = 7
#set GPIO direction (IN / OUT)
GPIO.setmode(GPIO.BCM)
GPIO.setup(self.GPIO_FRONTECHO, GPIO.IN)
GPIO.setup(self.GPIO_BACKECHO, GPIO.IN)
GPIO.setup(self.GPIO_FRONTTRIGGER, GPIO.OUT)
GPIO.output(self.GPIO_FRONTTRIGGER, False)
GPIO.setup(self.GPIO_BACKTRIGGER, GPIO.OUT)
GPIO.output(self.GPIO_BACKTRIGGER, False)
# initialise direction servos
self.servos = Servos(FRONTSERVO, BACKSERVO)
servoPositions = self.servos.FirstScanPosition()
self.frontServoDirection = servoPositions[0]
self.backServoDirection = servoPositions[1]
self.scannerActive = False
self.endthread = False
self.HistoryFront = [[0.0, 0.0, 0.0, 0.0, 0.0]]
self.HistoryBack = [[0.0, 0.0, 0.0, 0.0, 0.0]]
self.FrontDeltas = [[0.0, 0.0, 0.0, 0.0, 0.0]]
self.BackDeltas = [[0.0, 0.0, 0.0, 0.0, 0.0]]
self.FrontDeltaDelta = [0.0, 0.0, 0.0, 0.0, 0.0]
self.BackDeltaDelta = [0.0, 0.0, 0.0, 0.0, 0.0]
# initialise current distance readings
self.frontDistance = {
ServoDirection.Left: -1.0,
ServoDirection.OffLeft: -1.0,
ServoDirection.Ahead: -1.0,
ServoDirection.OffRight: -1.0,
ServoDirection.Right: -1.0
}
self.backDistance = {
ServoDirection.Left: -1.0,
ServoDirection.OffLeft: -1.0,
ServoDirection.Ahead: -1.0,
ServoDirection.OffRight: -1.0,
ServoDirection.Right: -1.0
}
time.sleep(1)
def UpdateStatistics(self):
if len(self.HistoryFront) == 1:
self.HistoryFront = [[
self.frontDistance[ServoDirection.Left],
self.frontDistance[ServoDirection.OffLeft],
self.frontDistance[ServoDirection.Ahead],
self.frontDistance[ServoDirection.OffRight],
self.frontDistance[ServoDirection.Right]
]]
self.HistoryBack = [[
self.backDistance[ServoDirection.Left],
self.backDistance[ServoDirection.OffLeft],
self.backDistance[ServoDirection.Ahead],
self.backDistance[ServoDirection.OffRight],
self.backDistance[ServoDirection.Right]
]]
self.HistoryFront += [[
self.frontDistance[ServoDirection.Left],
self.frontDistance[ServoDirection.OffLeft],
self.frontDistance[ServoDirection.Ahead],
self.frontDistance[ServoDirection.OffRight],
self.frontDistance[ServoDirection.Right]
]]
self.HistoryBack += [[
self.backDistance[ServoDirection.Left],
self.backDistance[ServoDirection.OffLeft],
self.backDistance[ServoDirection.Ahead],
self.backDistance[ServoDirection.OffRight],
self.backDistance[ServoDirection.Right]
]]
self.FrontDeltas += [[
round(self.HistoryFront[-1][0] - self.HistoryFront[-2][0], 1),
round(self.HistoryFront[-1][1] - self.HistoryFront[-2][1], 1),
round(self.HistoryFront[-1][2] - self.HistoryFront[-2][2], 1),
round(self.HistoryFront[-1][3] - self.HistoryFront[-2][3], 1),
round(self.HistoryFront[-1][4] - self.HistoryFront[-2][4], 1)
]]
self.BackDeltas += [[
round(self.HistoryBack[-1][0] - self.HistoryBack[-2][0], 1),
round(self.HistoryBack[-1][1] - self.HistoryBack[-2][1], 1),
round(self.HistoryBack[-1][2] - self.HistoryBack[-2][2], 1),
round(self.HistoryBack[-1][3] - self.HistoryBack[-2][3], 1),
round(self.HistoryBack[-1][4] - self.HistoryBack[-2][4], 1)
]]
# only keep the most recent 10 entries
if (len(self.HistoryFront) > 10):
del self.HistoryFront[0]
del self.HistoryBack[0]
del self.FrontDeltas[0]
del self.BackDeltas[0]
self.FrontDeltaDelta = [0.0, 0.0, 0.0, 0.0, 0.0]
self.BackDeltaDelta = [0.0, 0.0, 0.0, 0.0, 0.0]
for j in range(0, min(5, len(self.FrontDeltas))):
for i in range(0, 4):
self.FrontDeltaDelta[i] += self.FrontDeltas[j][i]
self.BackDeltaDelta[i] += self.BackDeltas[j][i]
for i in range(0, 4):
self.FrontDeltaDelta[i] = round(self.FrontDeltaDelta[i], 1)
self.BackDeltaDelta[i] = round(self.BackDeltaDelta[i], 1)
# threaded function
def GetDistance(self, delay):
while not (self.endthread):
frontError = backError = False
# Activate echo trigger (this is shared between front and rear sensors)
GPIO.output(self.GPIO_FRONTTRIGGER, True)
time.sleep(0.00001)
GPIO.output(self.GPIO_FRONTTRIGGER, False)
frontStartTime = frontStopTime = time.time()
while GPIO.input(self.GPIO_FRONTECHO) == 0:
frontStartTime = time.time()
if frontStartTime - frontStopTime > 0.02:
frontError = True
break
while GPIO.input(self.GPIO_FRONTECHO) == 1 and not (frontError):
frontStopTime = time.time()
if frontStopTime - frontStartTime > 0.02:
frontError = True
break
time.sleep(0.08)
# Activate echo trigger (this is shared between front and rear sensors)
GPIO.output(self.GPIO_BACKTRIGGER, True)
time.sleep(0.00001)
GPIO.output(self.GPIO_BACKTRIGGER, False)
backStartTime = backStopTime = time.time()
while GPIO.input(self.GPIO_BACKECHO) == 0:
backStartTime = time.time()
if backStartTime - backStopTime > 0.02:
backError = True
break
while GPIO.input(self.GPIO_BACKECHO) == 1 and not (backError):
backStopTime = time.time()
if backStopTime - backStartTime > 0.02:
backError = True
break
# time difference between start and return
frontdistance = (frontStopTime - frontStartTime) * 17150
backdistance = (backStopTime - backStartTime) * 17150
if frontdistance > 0 and not (frontError):
self.frontDistance[self.frontServoDirection] = frontdistance
if backdistance > 0 and not (backError):
self.backDistance[self.backServoDirection] = backdistance
if (self.frontServoDirection == ServoDirection.Left):
self.UpdateStatistics()
# move servos to next direction to scan
servoDirections = self.servos.NextScanPosition()
self.frontServoDirection = servoDirections[0]
self.backServoDirection = servoDirections[1]
time.sleep(delay)
def StartScanner(self, delay, getFirstScan=False):
self.endthread = False
self.ultrathread = threading.Thread(target=self.GetDistance,
args=(delay, ))
self.ultrathread.start()
self.scannerActive = True
if getFirstScan:
done = False
attempts = 3
while not (done) and attempts > 0:
time.sleep(delay * 5)
done = True
for key in self.frontDistance:
if self.frontDistance[key] == -1.0:
done = False
for key in self.backDistance:
if self.backDistance[key] == -1.0:
done = False
attempts -= 1
return done
else:
return True
def StopScanner(self):
self.endthread = True
self.ultrathread.join()
self.scannerActive = False
synced = False
### utils
def _reset_servos():
s.setXAxis(SERVO_CENTER)
s.setYAxis(SERVO_CENTER)
### code
trigger = Trigger()
# servo driver conf
s = Servos(SERVO_I2C_ADDRESS, SERVO_XAXIS_CHANNEL, SERVO_YAXIS_CHANNEL,
SERVO_PWM_FREQ)
_reset_servos()
def signal_handler(signal, frame):
_reset_servos()
trigger.reset()
sys.exit(0)
signal.signal(signal.SIGINT, signal_handler)
# pi camera setup
# camera = picamera.PiCamera()
## myo websocket handlers
class Robot:
def __init__(self, motorhat_addr=0x6f):
# Setup the motorhat with the passed in address
self._mh = Raspi_MotorHAT(addr=motorhat_addr)
# get local variable for each motor
self.left_motor = self._mh.getMotor(1)
self.right_motor = self._mh.getMotor(2)
# Setup The Distance Sensors
self.left_distance_sensor = DistanceSensor(echo=17,
trigger=27,
queue_len=2)
self.right_distance_sensor = DistanceSensor(echo=5,
trigger=6,
queue_len=2)
# Setup the Leds
self.leds = leds_led_shim.Leds()
# Set up servo motors for pan and tilt.
self.servos = Servos(addr=motorhat_addr)
# ensure the motors get stopped when the code exits
atexit.register(self.stop_all)
def convert_speed(self, speed):
# Choose the running mode
mode = Raspi_MotorHAT.RELEASE
if speed > 0:
mode = Raspi_MotorHAT.FORWARD
elif speed < 0:
mode = Raspi_MotorHAT.BACKWARD
# Scale the speed
output_speed = (abs(speed) * 255) // 100
return mode, int(output_speed)
def set_left(self, speed):
mode, output_speed = self.convert_speed(speed)
self.left_motor.setSpeed(output_speed)
self.left_motor.run(mode)
def set_right(self, speed):
mode, output_speed = self.convert_speed(speed)
self.right_motor.setSpeed(output_speed)
self.right_motor.run(mode)
def stop_motors(self):
self.left_motor.run(Raspi_MotorHAT.RELEASE)
self.right_motor.run(Raspi_MotorHAT.RELEASE)
def stop_all(self):
self.stop_motors()
# Clear the display
self.leds.clear()
self.leds.show()
# Reset the servos
self.servos.stop_all()
def set_pan(self, angle):
self.servos.set_servo_angle(1, angle)
def set_tilt(self, angle):
self.servos.set_servo_angle(0, angle)
from modes import AbstractMode
from servos import Servos
class ModeStandby(AbstractMode):
def __init__(self,servos):
AbstractMode.__init__(self,servos)
def init(self):
self.servos.angle(1,50)
# [self.servos.angle(v,50) for v in range(0,1)]
def update(self):
pass
if __name__ == "__main__":
try:
servos=Servos()
ms = ModeStandby(servos)
ms.init()
except Exception as e:
print(e)
pass
class Robot(object):
def __init__(self, motorhat_addr=0x6f):
self._mh = Raspi_MotorHAT(addr=motorhat_addr)
self.left_motor = self._mh.getMotor(1)
self.right_motor = self._mh.getMotor(2)
atexit.register(self.stop_motors)
# Setup the line sensors
self.right_line_sensor = LineSensor(23, queue_len=3, pull_up=True)
self.left_line_sensor = LineSensor(16, queue_len=3, pull_up=True)
self.right_line_sensor_stuck = ""
self.left_line_sensor_stuck = ""
self.servos = Servos(addr=motorhat_addr)
def stop_motors(self):
self.left_motor.run(Raspi_MotorHAT.RELEASE)
self.right_motor.run(Raspi_MotorHAT.RELEASE)
def stop_all(self):
self.stop_motors()
self.servos.stop_all()
# Clear sensor handlers
self.left_line_sensor.when_line = None
self.left_line_sensor.when_no_line = None
self.right_line_sensor.when_line = None
self.right_line_sensor.when_line = None
def convert_speed(self, speed):
#Choose the running mode
mode = Raspi_MotorHAT.RELEASE
if speed > 0:
mode = Raspi_MotorHAT.FORWARD
elif speed < 0:
mode = Raspi_MotorHAT.BACKWARD
output_speed = (abs(speed) * 255) / 100
return mode, int(output_speed)
def set_left(self, speed):
mode, output_speed = self.convert_speed(speed)
self.left_motor.setSpeed(output_speed)
self.left_motor.run(mode)
def set_right(self, speed):
mode, output_speed = self.convert_speed(speed)
self.right_motor.setSpeed(output_speed)
self.right_motor.run(mode)
def set_left_line_sensor_stuck(self, stuck):
self.left_line_sensor_stuck = stuck
def set_right_line_sensor_stuck(self, stuck):
self.right_line_sensor_stuck = stuck
def set_pan(self, angle):
self.servos.set_servo_angle(14, angle)
def set_tilt(self, angle):
self.servos.set_servo_angle(15, angle)
def __init__(self):
print("Initialising")
self.servos=Servos()
self.Kinematic=Kinematic()
from servos import Servos
servos = Servos(num=16, mimimum_pulse=450, maximum_pulse=2450, kill_angle=90)
servo = servos[int(input("What servo: "))]
x = input("What angle: ")
while x != "end":
try:
servo.angle = float(x)
except ValueError:
print("Must input a number")
x = input("New angle: ")
from imu import IMU
from pid import PID
from radio import Radio
from servos import Servos
# r = Radio('/dev/ttyUSB0')
# while True:
# if r.bytes_available() > 16:
# data = r.read()
# print(data.decode())
# r.close()
if __name__ == '__main__':
imu = IMU()
servos = Servos()
pid_roll = PID(600., 0., 0., control_range=[-250, 250])
pid_pitch = PID(-600., 0., 0., control_range=[-250, 250])
pid_yaw = PID(10., 0., 0)
pids = [pid_roll, pid_pitch, pid_yaw]
rpy = np.zeros(3)
sp_rpy = np.zeros(3)
cmd = np.array([1000, 1000, 1500, 1500])
throttle = 1100
tt = t = time.time()
running = True
while running:
import time
import signal
import math
from servos import Servos
SERVO_I2C_ADDRESS = 0x40 # I2C address of the PCA9685-based servo controller
SERVO_XAXIS_CHANNEL = 0 # Channel for the x axis rotation which controls laser up/down
SERVO_YAXIS_CHANNEL = 1 # Channel for the y axis rotation which controls laser left/right
SERVO_PWM_FREQ = 50 # PWM frequency for the servos in HZ (should be 50)
SERVO_MIN = 250 # Minimum rotation value for the servo, should be -90 degrees of rotation.
SERVO_MAX = 430 # Maximum rotation value for the servo, should be 90 degrees of rotation.
SERVO_CENTER = 340 # Center value for the servo, should be 0 degrees of rotation.
s = Servos(SERVO_I2C_ADDRESS, SERVO_XAXIS_CHANNEL, SERVO_YAXIS_CHANNEL, SERVO_PWM_FREQ)
def signal_handler(signal, frame):
s.setXAxis(SERVO_CENTER)
s.setYAxis(SERVO_CENTER)
sys.exit(0)
signal.signal(signal.SIGINT, signal_handler)
s.setXAxis(SERVO_CENTER)
s.setYAxis(SERVO_CENTER)
SPAN_MIN = SERVO_CENTER - SERVO_MIN
SPAN_MAX = SERVO_MAX - SERVO_CENTER
for i in range(0, 10000000):
deg = i / 360.0 * math.pi