from motors import Motors
from time import time, sleep
mc = Motors() # Create an instance of the Motors class used in SDP
motor_id = 3 # The port that your motor is plugged in to
speed = 100 # forward = positive, backwards = negative
run_time = 2 # amount of seconds to run motors
mc.move_motor(motor_id,
speed) # Move motor with the given ID at your set speed
start_time = time()
# The encoder board can be a little fragile - always use a try/except loop
try:
while time() < start_time + run_time:
mc.print_encoder_data()
sleep(
0.2
) # Always use a sleep of at least 0.1, or you will get an error
except:
print "IO Error"
mc.stop_motors()
class Paul(object):
def __init__(self):
# Name of the controller
self.name = "PAUL controller for Raspberry Pi"
# Current Speed of the robot
self.speed = 0
# Speed that the robot will travel up (note negative speed goes up)
self.up_speed = -100
#Speed that the robot will travel down (note negative speed goes down)
self.down_speed = 100
# Threshold reading on top distance sensors before triggering
self.top_threshold = 10
# Threshold reading on bottom distance sensors before triggering
self.bottom_threshold = 10
# Note: The following should only be set if distance sensors are not attached,
# they should be set to 0 if sensors are attached.
self.top_ds_reading = 20
self.bottom_ds_reading = 20
# Create an instance of the Motors class used in SDP
self.mc = Motors()
# Movement Functions
def start_motors(self, speed):
motor_ids = [5, 4, 3, 2, 1, 0]
self.speed = speed
print 'Starting motors...'
for motor_id in motor_ids:
self.mc.move_motor(motor_id, self.speed)
def stop_motors(self):
print 'Stopping motors...'
self.speed = 0
self.mc.stop_motors()
# Motors and Sensors Readings
def display_all_readings(self):
self.display_motor_readings()
self.display_top_ds_reading()
self.display_bottom_ds_reading()
print ""
def display_motor_readings(self):
self.mc.print_encoder_data()
def get_motor_readings(self):
#mc.read_encoder(id)
#where id is the port number on the encoder board that you wish to read
return self.speed
def check_top_ds_reading(self):
return False
def display_top_ds_reading(self):
print "Top DS Reading: --"
def get_top_ds_reading(self):
return self.top_ds_reading
def check_bottom_ds_reading(self):
return False
def display_bottom_ds_reading(self):
print "Bottom DS Reading: --"
def get_bottom_ds_reading(self):
return self.bottom_ds_reading
# Getters and Setters
def get_name(self):
return self.name
def set_speed(self, name):
self.name = name
def get_speed(self):
return self.speed
def set_speed(self, speed):
self.speed = speed
def get_up_speed(self):
return self.up_speed
def set_up_speed(self, up_speed):
self.up_speed = up_speed
def get_down_speed(self):
return self.down_speed
def set_down_speed(self, up_speed):
self.down_speed = up_speed
def get_top_threshold(self):
return self.top_threshold
def set_top_threshold(self, top_threshold):
self.top_threshold = top_threshold
def get_bottom_threshold(self):
return self.bottom_threshold
def set_bottom_threshold(self, bottom_threshold):
self.bottom_threshold = bottom_threshold
class Paul(object):
def __init__(self):
# Name of the controller
self.name = "PAUL controller for Raspberry Pi"
# Current Speed of the robot
self.speed = 0
# Speed that the robot will travel up (note negative speed goes up)
self.up_speed = -70
#Speed that the robot will travel down (note negative speed goes down)
self.down_speed = 70
# Threshold reading on top distance sensors before triggering
self.top_threshold = 1.9
# Threshold reading on bottom distance sensors before triggering
self.bottom_threshold = 2.6
# Note: The following should only be set if distance sensors are not attached,
# they should be set to 0 if sensors are attached.
self.top_ds_reading = 0
self.bottom_ds_reading = 0
# Create an instance of the Motors class used in SDP
self.mc = Motors()
#Sensors
#define array of digital inputs to store readings from the bump sensors
self.bump_sensors = [DigitalInput() for i in range (0,6)]
#addressing the channels for the bump sensors
#0-2 are top sensors
#3-5 are bottom channels
for i in range(0,6):
self.bump_sensors[i].setChannel(i)
self.bump_sensors[i].openWaitForAttachment(5000)
#define array of analogue inputs to store readings from the ir sensors
self.ir_sensors = [VoltageInput() for i in range (0,6)]
#addressing the channels for the ir sensors
#0-2 are top sensors
#3-5 are bottom channels
for i in range(0,6):
self.ir_sensors[i].setChannel(i)
#this line will basically call the event handler when the readings change
self.ir_sensors[i].openWaitForAttachment(5000)
#Light
# connect to pin 12(slot PWM)
PIN = 18
# For Grove - WS2813 RGB LED Strip Waterproof - 30 LED/m
# there is 30 RGB LEDs.
COUNT = 60
self.strip = GroveWS2813RgbStrip(PIN, COUNT)
# Movement Functions
def start_motors(self,speed_input):
#If speed_input is an int then the speeds of all the motors should be the same
if type(speed_input) is int:
speeds = [speed_input, speed_input, speed_input, speed_input, speed_input, speed_input]
self.speed = speed_input
#If speed_input is a list then the speeds should be set different, set speed to a non int value to disable changing speed on that motor
elif type(speed_input) is list:
speeds = speed_input
#If list is not valid then set speeds to the current speed
else:
speeds = [self.speed, self.speed, self.speed, self.speed, self.speed, self.speed]
#ID's of all the motors to run
motor_ids = [5,4,3,2,1,0]
print 'Starting motors...'
#print speeds
for i in range(len(speeds)):
#for motor_id in motor_ids:
if type(speeds[i]) is int:
self.mc.move_motor(motor_ids[i],speeds[i])
#self.mc.move_motor(motor_id,self.speed)
def stop_motors(self):
print 'Stopping motors...'
self.speed = 0
self.mc.stop_motors()
# Motors and Sensors Readings
def display_all_readings(self):
self.display_motor_readings()
self.display_top_ds_reading()
self.display_bottom_ds_reading()
print ""
def display_motor_readings(self):
self.mc.print_encoder_data()
def get_motor_readings(self):
#mc.read_encoder(id)
#where id is the port number on the encoder board that you wish to read
return self.speed
#Returns true if the top distance sensors are within range of the top
#Returns false otherwise
def check_top_ds_reading(self):
#If the voltage of the ir sensors is above a threshold then return true
if(self.ir_sensors[0].getVoltage() >= self.top_threshold or self.ir_sensors[1].getVoltage() >= self.top_threshold or self.ir_sensors[2].getVoltage() >= self.top_threshold):
print "Top ir threshold exceeded"
return True
#If any of the top bumper sensors have been activated then return true
if(self.bump_sensors[0].getState() == 1 or self.bump_sensors[1].getState() == 1 or self.bump_sensors[2].getState() == 1):
print "Top bump sensor triggered"
return True
#Else return false
return False
def display_top_ds_reading(self):
print "Top DS Reading: " + str(self.top_ds_reading)
def get_top_ds_reading(self):
return self.top_ds_reading
#Returns true if the top distance sensors are within range of the top
#Returns false otherwise
def check_bottom_ds_reading(self):
if(self.ir_sensors[3].getVoltage() >= self.top_threshold or self.ir_sensors[4].getVoltage() >= self.top_threshold or self.ir_sensors[5].getVoltage() >= self.top_threshold):
#If the voltage of the ir sensors is above a threshold then return true
print "Bottom ir threshold exceeded"
return True
#If any of the bottom bumper sensors have been activated then return true
if(self.bump_sensors[3].getState() == 1 or self.bump_sensors[4].getState() == 1 or self.bump_sensors[5].getState() == 1):
print "Bottom bump sensor triggered"
return True
#Else return false
return False
def display_bottom_ds_reading(self):
print "Bottom DS Reading: " + str(self.bottom_ds_reading)
def get_bottom_ds_reading(self):
return self.bottom_ds_reading
# Light functions
def light_on(self):
for i in range(self.strip.numPixels()):
self.strip.setPixelColor(i, Color(62,6,148))
self.strip.show()
def light_off(self):
for i in range(self.strip.numPixels()):
self.strip.setPixelColor(i, Color(0,0,0))
self.strip.show()
# Terminate script
def paul_tidy(self):
#Stop the motors
self.stop_motors()
self.light_off()
#Keyboard inerrupt may be needed to reset phidget
try:
input("Press Enter to Stop\n")
except (Exception, KeyboardInterrupt):
pass
#Close all the channels
for i in range (0, 6):
self.ir_sensors[i].close()
self.bump_sensors[i].close()
# Getters and Setters
def get_name(self):
return self.name
def set_speed(self,name):
self.name = name
def get_speed(self):
return self.speed
def set_speed(self,speed):
self.speed = speed
def get_up_speed(self):
return self.up_speed
def set_up_speed(self,up_speed):
self.up_speed = up_speed
def get_down_speed(self):
return self.down_speed
def set_down_speed(self,up_speed):
self.down_speed = up_speed
def get_top_threshold(self):
return self.top_threshold
def set_top_threshold(self,top_threshold):
self.top_threshold = top_threshold
def get_bottom_threshold(self):
return self.bottom_threshold
def set_bottom_threshold(self,bottom_threshold):
self.bottom_threshold = bottom_threshold
