def turnOnTheSpot(self, steps, direction):
"""
This function makes the tortoise turn with the centre of rotation in one of the wheels.
:param steps: number of rotations of the motors
:param direction: one of these four combinations: [forwards/backwards]_[left/right]
:type steps: int
:type direction: enums.Direction
"""
if(steps < 0):
messages.printMessage('bad_steps')
self.blinkLEDs_error()
return -1
if( direction != enums.Direction.backwards_right and direction != enums.Direction.backwards_left and
direction != enums.Direction.forwards_right and direction != enums.Direction.forwards_left ) :
messages.printMessage('bad_direction_turn')
self.blinkLEDs_error()
return -1
# Only wheel A moves
if direction == enums.Direction.backwards_right or direction == enums.Direction.forwards_right:
return self.moveMotors(steps, 0, self.minDelayMotors, 0, direction)
# Only wheel B moves
elif direction == enums.Direction.backwards_left or direction == enums.Direction.forwards_left:
return self.moveMotors(0, steps, 0, self.minDelayMotors, direction)
def getLEDValue(self, position):
if (position < 1 or position > 4):
messages.printMessage('bad_LED')
self.blinkLEDs_error()
return -1
return self.actuators.getActuatorValue(enums.ActuatorType.led, position)
def setLEDValue(self, position, value):
if(position < 1 or position > 4):
messages.printMessage('bad_LED')
self.blinkLEDs_error()
return -1
if(value != 0 and value != 1):
messages.printMessage('bad_LED_value')
self.blinkLEDs_error()
return -1
self.actuators.setActuatorValue(enums.ActuatorType.led, position, value)
return 0
def shuffleOnTheSpot(self, steps, direction):
if(steps < 0):
messages.printMessage('bad_steps')
self.blinkLEDs_error()
return -1
if( direction != enums.Direction.clockwise and direction != enums.Direction.counterClockwise ) :
messages.printMessage('bad_shuffle')
self.blinkLEDs_error()
return -1
return self.moveMotors(steps, steps, self.minDelayMotors, self.minDelayMotors, direction)
def getLEDValue(self, position):
"""
Returns 1 if the LED is ON, 0 if it's OFF.
:param position: position in the PCB of the sensor queried
:type position: int
:rtype: int
"""
if (position < 1 or position > 4):
messages.printMessage('bad_LED')
self.blinkLEDs_error()
return -1
return self.actuators.getActuatorValue(enums.ActuatorType.led, position)
def turnOnTheSpot(self, steps, direction):
if(steps < 0):
messages.printMessage('bad_steps')
self.blinkLEDs_error()
return -1
if( direction != enums.Direction.backwards_right and direction != enums.Direction.backwards_left and
direction != enums.Direction.forwards_right and direction != enums.Direction.forwards_left ) :
messages.printMessage('bad_direction_turn')
self.blinkLEDs_error()
return -1
if direction == enums.Direction.backwards_right or direction == enums.Direction.forwards_right:
return self.moveMotors(steps, 0, self.minDelayMotors, 0, direction)
elif direction == enums.Direction.backwards_left or direction == enums.Direction.forwards_left:
return self.moveMotors(0, steps, 0, self.minDelayMotors, direction)
def shuffleOnTheSpot(self, steps, direction):
"""
This function makes the tortoise turn with the centre of rotation between both wheels.
:param steps: number of rotations of the motors
:param direction: either clockwise or counter-clockwise
:type steps: int
:type direction: enums.Direction
"""
if(steps < 0):
messages.printMessage('bad_steps')
self.blinkLEDs_error()
return -1
if( direction != enums.Direction.clockwise and direction != enums.Direction.counterClockwise ) :
messages.printMessage('bad_shuffle')
self.blinkLEDs_error()
return -1
return self.moveMotors(steps, steps, self.minDelayMotors, self.minDelayMotors, direction)
def turn(self, stepsWheelA, stepsWheelB, direction):
"""
This function makes the tortoise turn by specifying different steps for the wheels.
The function computes the delay that the wheel with less rotations should have in order to finish at the same time than the other wheel.
:param stepsWheelA: number of rotations of the motor attached to wheel A
:param stepsWheelB: number of rotations of the motor attached to wheel B
:param direction: one of these four combinations: [forwards/backwards]_[left/right]
:type stepsWheelA: int
:type stepsWheelB: int
:type direction: enums.Direction
"""
if( direction != enums.Direction.backwards_right and direction != enums.Direction.backwards_left and
direction != enums.Direction.forwards_right and direction != enums.Direction.forwards_left ) :
messages.printMessage('bad_direction_turn')
self.blinkLEDs_error()
return -1
if (direction == enums.Direction.backwards_right or direction == enums.Direction.forwards_right) and (stepsWheelB >= stepsWheelA):
messages.printMessage('bad_turn')
self.blinkLEDs_error()
return -1
if (direction == enums.Direction.backwards_left or direction == enums.Direction.forwards_left) and (stepsWheelA >= stepsWheelB):
messages.printMessage('bad_turn')
self.blinkLEDs_error()
return -1
if(stepsWheelA < 0 or stepsWheelB < 0):
messages.printMessage('bad_steps')
self.blinkLEDs_error()
return -1
if direction == enums.Direction.backwards_right or direction == enums.Direction.forwards_right:
# The delay of the wheel with less movements is worked out so that both wheels finish more or less at the same time
delay = (stepsWheelA * self.minDelayMotors) / stepsWheelB
return self.moveMotors(stepsWheelA, stepsWheelB, self.minDelayMotors, delay, direction)
elif direction == enums.Direction.backwards_left or direction == enums.Direction.forwards_left:
# The delay of the wheel with less movements is worked out so that both wheels finish more or less at the same time
delay = (stepsWheelB * self.minDelayMotors) / stepsWheelA
return self.moveMotors(stepsWheelA, stepsWheelB, delay, self.minDelayMotors, direction)
def setLEDValue(self, position, value):
"""
Turns on/off an LED.
:param position: position in the PCB of the sensor queried
:param value: 1 (ON) or 0 (OFF)
:type position: int
:type value: int
"""
if(position < 1 or position > 4):
messages.printMessage('bad_LED')
self.blinkLEDs_error()
return -1
if(value != 0 and value != 1):
messages.printMessage('bad_LED_value')
self.blinkLEDs_error()
return -1
self.actuators.setActuatorValue(enums.ActuatorType.led, position, value)
return 0
def turn(self, stepsWheelA, stepsWheelB, direction):
if( direction != enums.Direction.backwards_right and direction != enums.Direction.backwards_left and
direction != enums.Direction.forwards_right and direction != enums.Direction.forwards_left ) :
messages.printMessage('bad_direction_turn')
self.blinkLEDs_error()
return -1
if (direction == enums.Direction.backwards_right or direction == enums.Direction.forwards_right) and (stepsWheelB >= stepsWheelA):
messages.printMessage('bad_turn')
self.blinkLEDs_error()
return -1
if (direction == enums.Direction.backwards_left or direction == enums.Direction.forwards_left) and (stepsWheelA >= stepsWheelB):
messages.printMessage('bad_turn')
self.blinkLEDs_error()
return -1
if(stepsWheelA < 0 or stepsWheelB < 0):
messages.printMessage('bad_steps')
self.blinkLEDs_error()
return -1
if direction == enums.Direction.backwards_right or direction == enums.Direction.forwards_right:
delay = (stepsWheelA * self.minDelayMotors) / stepsWheelB
return self.moveMotors(stepsWheelA, stepsWheelB, self.minDelayMotors, delay, direction)
elif direction == enums.Direction.backwards_left or direction == enums.Direction.forwards_left:
delay = (stepsWheelB * self.minDelayMotors) / stepsWheelA
return self.moveMotors(stepsWheelA, stepsWheelB, delay, self.minDelayMotors, direction)
def calibrateLight(self):
global lowerBoundLight, upperBoundLight, isLightCalibrated
messages.printMessage('calibration_ambient')
raw_input()
#lowerBoundLight = max(self.sensors.readSensor(enums.SensorType.light, 1), self.sensors.readSensor(enums.SensorType.light, 2))
lowerBoundLight = self.sensors.readSensor(enums.SensorType.light, 1)
#print "Light in normal conditions is: ", lowerBoundLight
messages.printMessage('calibration_light_source')
raw_input()
#upperBoundLight = min((self.sensors.readSensor(enums.SensorType.light, 1), self.sensors.readSensor(enums.SensorType.light, 2)))
upperBoundLight = self.sensors.readSensor(enums.SensorType.light, 1)
# print "Light when there is a light source is:", upperBoundLight
isLightCalibrated = True
messages.printMessage('calibration_complete')
def calibrateLight(self):
"""
Calibrates the light sensor, defining the upper and lower bounds of the light, i.e. the values returned by the light sensor of the ambience and when a light source is placed in front of it.
Currently, only the light sensor in the position 1 of the PCB is used for calibration.
"""
global lowerBoundLight, upperBoundLight, isLightCalibrated
messages.printMessage('calibration_ambient')
raw_input()
lowerBoundLight = self.sensors.readSensor(enums.SensorType.light, 1)
messages.printMessage('calibration_light_source')
raw_input()
upperBoundLight = self.sensors.readSensor(enums.SensorType.light, 1)
isLightCalibrated = True
messages.printMessage('calibration_complete')
def blinkLEDs(self, positions, numberOfBlinks, delay, blocking = False):
if numberOfBlinks < 0:
messages.printMessage('blinks_negative')
self.blinkLEDs_error()
return -1
if numberOfBlinks == 0:
messages.printMessage('blinks_zero')
self.blinkLEDs_error()
return -1
if delay < 0:
messages.printMessage('blinking_fast')
self.blinkLEDs_error()
return -1
try:
for y in range(0, len(positions)):
if positions[y] < 0 or positions[y] > 4:
messages.printMessage('bad_LED')
self.blinkLEDs_error()
return -1
except TypeError: # It's not an array but an integer
if positions < 0 or positions > 4:
messages.printMessage('bad_LED')
self.blinkLEDs_error()
return -1
previousStateLEDs = [ self.getLEDValue(x) for x in range(1, 5) ]
cont = True
# Infinite loop to "stop" the execution of the program and keep blinkind the LEDs
while cont:
for x in range(0, numberOfBlinks):
try:
for y in range(0, len(positions)):
self.actuators.setActuatorValue(enums.ActuatorType.led, positions[y], 1)
time.sleep(delay)
for y in range(0, len(positions)):
self.actuators.setActuatorValue(enums.ActuatorType.led, positions[y], 0)
time.sleep(delay)
except TypeError: # It's not an array but an integer
self.actuators.setActuatorValue(enums.ActuatorType.led, positions, 1)
time.sleep(delay)
self.actuators.setActuatorValue(enums.ActuatorType.led, positions, 0)
time.sleep(delay)
cont = blocking
# If it doesn't block, the previous state of the LEDs is restored
for x in range(1, 5):
self.setLEDValue(x, previousStateLEDs[x - 1])
return 0
def moveMotors(self, stepsWheelA, stepsWheelB, delayWheelA, delayWheelB, direction):
"""
Move the motors of the wheels.
Running the motors is done in different threads so that both wheels can move at the same time. The thread that executes this functions waits until the threads are finished, i.e. the motion is done. However, it doesn't block, but checks every half a second if the e-stop button has been pressed. If so, it stops the motors and exits.
:param stepsWheelA: number of rotations of the motor attached to wheel A
:param stepsWheelB: number of rotations of the motor attached to wheel B
:param delayWheelA: controls the speed of rotation of the motor attached to wheel A (minimum is 2 milliseconds)
:param delayWheelB: controls the speed of rotation of the motor attached to wheel B (minimum is 2 milliseconds)
:param direction: the direction in which the tortoise should move
:type stepsWheelA: int
:type stepsWheelB: int
:type delayWheelA: int
:type delayWheelB: int
:type direction: enums.Direction
"""
if( direction != enums.Direction.backwards_right and direction != enums.Direction.backwards_left and direction != enums.Direction.forwards_right and direction != enums.Direction.forwards_left and direction != enums.Direction.forwards and direction != enums.Direction.backwards and direction != enums.Direction.clockwise and direction != enums.Direction.counterClockwise ) :
messages.printMessage('bad_direction')
self.blinkLEDs_error()
return -1
if(stepsWheelA < 0 or stepsWheelB < 0):
messages.printMessage('bad_steps')
self.blinkLEDs_error()
return -1
if((stepsWheelA > 0 and delayWheelA < self.minDelayMotors) or (stepsWheelB > 0 and delayWheelB < self.minDelayMotors)):
messages.printMessage('bad_delay')
self.blinkLEDs_error()
return -1
# If a stop command has been sent, the turtle will stop its movement
if self.getSensorData(enums.SensorType.emergencyStop, 4) == 0:
if self.getStateTortoise() == enums.State.running:
self.setStateTortoise(enums.State.paused)
messages.printMessage('paused')
else:
if self.getStateTortoise() == enums.State.paused:
self.setStateTortoise(enums.State.running)
messages.printMessage('resumed')
# The threads are created. They aren't started yet though.
motorAprocess_backwards = Process(target=self.A.backwards, args=(delayWheelA / 1000.00, stepsWheelA))
motorBprocess_backwards = Process(target=self.B.backwards, args=(delayWheelB / 1000.00, stepsWheelB))
motorAprocess_forwards = Process(target=self.A.forwards, args=(delayWheelA / 1000.00, stepsWheelA))
motorBprocess_forwards = Process(target=self.B.forwards, args=(delayWheelB / 1000.00, stepsWheelB))
# The specific wheels are started in order to accomplish the desired movement in the direction commanded
if direction == enums.Direction.backwards_left or direction == enums.Direction.backwards or direction == enums.Direction.backwards_right:
if stepsWheelA > 0:
motorAprocess_backwards.start()
if stepsWheelB > 0:
motorBprocess_backwards.start()
elif direction == enums.Direction.forwards_right or direction == enums.Direction.forwards or direction == enums.Direction.forwards_left:
if stepsWheelA > 0:
motorAprocess_forwards.start()
if stepsWheelB > 0:
motorBprocess_forwards.start()
elif direction == enums.Direction.clockwise:
if stepsWheelA > 0:
motorAprocess_backwards.start()
if stepsWheelB > 0:
motorBprocess_forwards.start()
elif direction == enums.Direction.counterClockwise:
if stepsWheelA > 0:
motorAprocess_forwards.start()
if stepsWheelB > 0:
motorBprocess_backwards.start()
# The main loop pools the emergencyStop while the motors are running
while motorAprocess_backwards.is_alive() or motorBprocess_backwards.is_alive() or motorAprocess_forwards.is_alive() or motorBprocess_forwards.is_alive():
# If a stop command has been sent, the turtle will stop its movement and exit this function
if self.getSensorData(enums.SensorType.emergencyStop, 4) == 0:
if self.getStateTortoise() == enums.State.running:
self.setStateTortoise(enums.State.paused)
messages.printMessage('paused')
if motorAprocess_backwards.is_alive():
motorAprocess_backwards.terminate()
motorAprocess_backwards.join()
if motorBprocess_backwards.is_alive():
motorBprocess_backwards.terminate()
motorBprocess_backwards.join()
if motorAprocess_forwards.is_alive():
motorAprocess_forwards.terminate()
motorAprocess_forwards.join()
if motorBprocess_forwards.is_alive():
motorBprocess_forwards.terminate()
motorBprocess_forwards.join()
elif self.getStateTortoise() == enums.State.paused:
self.setStateTortoise(enums.State.running)
messages.printMessage('resumed')
time.sleep(0.5)
# When the movement finishes, the motors are turned off
self.A.stopMotors()
self.B.stopMotors()
return 0
def moveMotors(self, stepsWheelA, stepsWheelB, delayWheelA, delayWheelB, direction):
if( direction != enums.Direction.backwards_right and direction != enums.Direction.backwards_left and direction != enums.Direction.forwards_right and direction != enums.Direction.forwards_left and direction != enums.Direction.forwards and direction != enums.Direction.backwards and direction != enums.Direction.clockwise and direction != enums.Direction.counterClockwise ) :
messages.printMessage('bad_direction')
self.blinkLEDs_error()
return -1
if(stepsWheelA < 0 or stepsWheelB < 0):
messages.printMessage('bad_steps')
self.blinkLEDs_error()
return -1
if((stepsWheelA > 0 and delayWheelA < self.minDelayMotors) or (stepsWheelB > 0 and delayWheelB < self.minDelayMotors)):
messages.printMessage('bad_delay')
self.blinkLEDs_error()
return -1
# If a stop command has been sent, the turtle will stop its movement
if self.getSensorData(enums.SensorType.emergencyStop, 1) == 0:
if self.getStateTortoise() == enums.State.running:
self.setStateTortoise(enums.State.paused)
messages.printMessage('paused')
else:
if self.getStateTortoise() == enums.State.paused:
self.setStateTortoise(enums.State.running)
messages.printMessage('resumed')
motorAprocess_backwards = Process(target=self.A.backwards, args=(delayWheelA / 1000.00, stepsWheelA))
motorBprocess_backwards = Process(target=self.B.backwards, args=(delayWheelB / 1000.00, stepsWheelB))
motorAprocess_forwards = Process(target=self.A.forwards, args=(delayWheelA / 1000.00, stepsWheelA))
motorBprocess_forwards = Process(target=self.B.forwards, args=(delayWheelB / 1000.00, stepsWheelB))
if direction == enums.Direction.backwards_left or direction == enums.Direction.backwards or direction == enums.Direction.backwards_right:
if stepsWheelA > 0:
motorAprocess_backwards.start()
if stepsWheelB > 0:
motorBprocess_backwards.start()
elif direction == enums.Direction.forwards_right or direction == enums.Direction.forwards or direction == enums.Direction.forwards_left:
if stepsWheelA > 0:
motorAprocess_forwards.start()
if stepsWheelB > 0:
motorBprocess_forwards.start()
elif direction == enums.Direction.clockwise:
if stepsWheelA > 0:
motorAprocess_backwards.start()
if stepsWheelB > 0:
motorBprocess_forwards.start()
elif direction == enums.Direction.counterClockwise:
if stepsWheelA > 0:
motorAprocess_forwards.start()
if stepsWheelB > 0:
motorBprocess_backwards.start()
# The main loop pools the emergencyStop
while motorAprocess_backwards.is_alive() or motorBprocess_backwards.is_alive() or motorAprocess_forwards.is_alive() or motorBprocess_forwards.is_alive():
# If a stop command has been sent, the turtle will stop its movement
if self.getSensorData(enums.SensorType.emergencyStop, 1) == 0:
if self.getStateTortoise() == enums.State.running:
self.setStateTortoise(enums.State.paused)
messages.printMessage('paused')
if motorAprocess_backwards.is_alive():
motorAprocess_backwards.terminate()
motorAprocess_backwards.join()
if motorBprocess_backwards.is_alive():
motorBprocess_backwards.terminate()
motorBprocess_backwards.join()
if motorAprocess_forwards.is_alive():
motorAprocess_forwards.terminate()
motorAprocess_forwards.join()
if motorBprocess_forwards.is_alive():
motorBprocess_forwards.terminate()
motorBprocess_forwards.join()
elif self.getStateTortoise() == enums.State.paused:
self.setStateTortoise(enums.State.running)
messages.printMessage('resumed')
time.sleep(0.5)
self.A.stopMotors()
self.B.stopMotors()
return 0
def __init__(self):
"""
This method creates a tortoise. It initializes the sensors, the variables that control the random motion and creates a file with the PID of the process which has created the tortoise so that the watchdog (a background process) can stops the motors and LEDs in case the user process finishes because of an error. The tortoise is created uncalibrated.
The reasons of termination of a user process could be because of normal termination
or because of an error (exceptions, ctrl-c, ...). When an error happens, the motors
and may be still on. In this case, the motors and LEDs should be turned off
for the battery not to drain.
The solution implemented is to have a background process (a watchdog) running
continously. This process checks if the user process doesn't exist anymore (termination).
If it doesn't, it stops the motors, switches off the LEDs and cleans up all the pins.
In order to identy that the user script has finished, a file with the name [PID].pid is
created in the folder ~/.tortoise_pids/, where [PID] is the PID of the user process.
Regarding calibration, the purpose was to avoid calibration everytime the tortoise object is created.
However, this hasn't been implemented yet. The idea could be to save the light
values in a file and read that file when creating the tortoise object.
Light conditions could have changed, so this should be done carefully.
At the moment, the tortoise object is created without calibration. If the users
want to use the light sensors, they need will need to execute the calibrateLight
function before using those sensors.
"""
# Variables that control the calibration of the light sensors
global isLightCalibrated
global lowerBoundLight
global upperBoundLight
isLightCalibrated = False
lowerBoundLight = 0
upperBoundLight = 0
# --- Variables that control the calibration of the light sensors ---
# No warnings from the GPIO library
GPIO.setwarnings(False)
# Variables that control the random motion
self.lastRandomCommand = None
self.timesSameRandomCommandExecuted = 0
self.numberRepeatsRandomCommand = -1
self.lastRandomStepsWheelA = None
self.lastRandomStepsWheelB = None
self.lastRandomDirection = None
# --- Variables that control the random motion ---
# Setting the motors, sensors and actuators
# Pin numbers of the motors
motorPins = [13, 6, 5, 7, 20, 10, 9, 11]
self.A = Motor(motorPins[0], motorPins[1], motorPins[2], motorPins[3])
self.B = Motor(motorPins[4], motorPins[5], motorPins[6], motorPins[7])
self.sensors = Sensors()
self.actuators = Actuators()
# Position 1 of the light sensors area in the PCB assigned to pin 17
self.sensors.setSensor(enums.SensorType.light, 1, 17)
# Position 2 of the light sensors area in the PCB assigned to pin 4
self.sensors.setSensor(enums.SensorType.light, 2, 4)
# Position 1 of the touch sensors area in the PCB assigned to pin 3
self.sensors.setSensor(enums.SensorType.emergencyStop, 1, 3)
# Position 2 of the touch sensors area in the PCB assigned to pin 27
self.sensors.setSensor(enums.SensorType.touch, 2, 27)
# Position 3 of the touch sensors area in the PCB assigned to pin 2
self.sensors.setSensor(enums.SensorType.touch, 3, 2)
# Position 4 of the touch sensors area in the PCB assigned to pin 18
self.sensors.setSensor(enums.SensorType.touch, 4, 18)
# Position 1 of the proximity sensors area in the PCB assigned to pin 19
self.sensors.setSensor(enums.SensorType.proximity, 1, 19)
# Position 2 of the proximity sensors area in the PCB assigned to pin 21
self.sensors.setSensor(enums.SensorType.proximity, 2, 21)
# Position 3 of the proximity sensors area in the PCB assigned to pin 22
self.sensors.setSensor(enums.SensorType.proximity, 3, 22)
# Position 4 of the proximity sensors area in the PCB assigned to pin 26
self.sensors.setSensor(enums.SensorType.proximity, 4, 26)
# Positions of the LEDs area in the PCB assigned to pins 8, 16, 25, 12
ledPins = [8, 16, 25, 12]
self.actuators.initActuator(enums.ActuatorType.led, 1, ledPins[0])
self.actuators.initActuator(enums.ActuatorType.led, 2, ledPins[1])
self.actuators.initActuator(enums.ActuatorType.led, 3, ledPins[2])
self.actuators.initActuator(enums.ActuatorType.led, 4, ledPins[3])
# --- Setting the motors, sensors and actuators ---
# Times pressed the touch sensor for the latching behavour
self.lastTouch = [-1,-1,-1]
# Minimum milliseconds to send to the motors as delay
self.minDelayMotors = 2
# Creation of a file with the PID of the process
# PID of process
pid = os.getpid()
# ~/.tortoise_pids/
directory = os.path.expanduser("~") + "/.tortoise_pids/"
# Filename: [PID].pid
f = open(directory + str(pid) + ".pid", "w")
# First line: motor pins
f.write(str(motorPins[0]) + " " + str(motorPins[1]) + " " + str(motorPins[2]) + " " + str(motorPins[3]) + " " + str(motorPins[4]) + " " + str(motorPins[5]) + " " + str(motorPins[6]) + " " + str(motorPins[7]) + "\n")
# Second line: LED pins
f.write(str(ledPins[0]) + " " + str(ledPins[1]) + " " + str(ledPins[2]) + " " + str(ledPins[3]) + "\n")
f.close()
# --- Creation of a file with the PID of the process ---
# Waiting for the user to press the e-stop button
self.state = enums.State.paused
messages.printMessage('greetings')
while self.getSensorData(enums.SensorType.emergencyStop, 4) == 0:
time.sleep(0.1)
messages.printMessage('running')
self.state = enums.State.running
def blinkLEDs(self, positions, numberOfBlinks, delay, blocking = False):
"""
Blinks the LEDs wanted the number of times specified.
:param positions: position or positions in the PCB of the LEDs wanted
:param numberOfBlinks: number of blinks
:param delay: milliseconds to wait between blinking
:param blocking: whether the function should block forever or not.
:type positions: int, [int]
:type numberOfBlinks: int
:type delay: int
:type blocking: boolean
.. warning:: If blocking == True, the thread will block forever because of infinite loop. It's only used when there are errors.
"""
if numberOfBlinks < 0:
messages.printMessage('blinks_negative')
self.blinkLEDs_error()
return -1
if numberOfBlinks == 0:
messages.printMessage('blinks_zero')
self.blinkLEDs_error()
return -1
if delay < 0:
messages.printMessage('blinking_fast')
self.blinkLEDs_error()
return -1
# If no TypeError exception raised, 'positions' is an array
try:
# All positions are checked
for y in range(0, len(positions)):
if positions[y] < 0 or positions[y] > 4:
messages.printMessage('bad_LED')
self.blinkLEDs_error()
return -1
except TypeError: # It's not an array but an integer
if positions < 0 or positions > 4:
messages.printMessage('bad_LED')
self.blinkLEDs_error()
return -1
# The current state of the LEDs is saved to restore it later
previousStateLEDs = [ self.getLEDValue(x) for x in range(1, 5) ]
cont = True
# If blocking == True, it's an infinite loop to "stop" the execution of the program and keep blinkind the LEDs
while cont:
for x in range(0, numberOfBlinks):
# If no TypeError exception raised, 'positions' is an array
try:
for y in range(0, len(positions)):
self.actuators.setActuatorValue(enums.ActuatorType.led, positions[y], 1)
time.sleep(delay)
for y in range(0, len(positions)):
self.actuators.setActuatorValue(enums.ActuatorType.led, positions[y], 0)
time.sleep(delay)
except TypeError: # It's not an array but an integer
self.actuators.setActuatorValue(enums.ActuatorType.led, positions, 1)
time.sleep(delay)
self.actuators.setActuatorValue(enums.ActuatorType.led, positions, 0)
time.sleep(delay)
# Depending on the parameter, it blocks or not
cont = blocking
# If it doesn't block, the previous state of the LEDs is restored
for x in range(1, 5):
self.setLEDValue(x, previousStateLEDs[x - 1])
return 0
def __init__(self):
global isLightCalibrated
global lowerBoundLight
global upperBoundLight
GPIO.setwarnings(False)
# self.lock = threading.RLock()
self.lastRandomCommand = None
self.timesSameRandomCommandExecuted = 0
self.numberRepeatsRandomCommand = -1
self.lastRandomStepsWheelA = None
self.lastRandomStepsWheelB = None
self.lastRandomDirection = None
isLightCalibrated = False
lowerBoundLight = 0
upperBoundLight = 0
# Previous: [4, 17, 23, 24, 27, 22, 18, 5]
motorPins = [13, 6, 5, 7, 20, 10, 9, 11]
ledPins = [8, 16, 25, 12]
# CREATING FILE WITH PID
# PID of process
pid = os.getpid()
# ~/.tortoise_pids/
directory = os.path.expanduser("~") + "/.tortoise_pids/"
# Filename: [PID].pid
f = open(directory + str(pid) + ".pid", "w")
# First line: motor pins
f.write(str(motorPins[0]) + " " + str(motorPins[1]) + " " + str(motorPins[2]) + " " + str(motorPins[3]) + " " + str(motorPins[4]) + " " + str(motorPins[5]) + " " + str(motorPins[6]) + " " + str(motorPins[7]) + "\n")
# Second line: LED pins
f.write(str(ledPins[0]) + " " + str(ledPins[1]) + " " + str(ledPins[2]) + " " + str(ledPins[3]) + "\n")
f.close()
# ----------------------
# TODO: change to self.Motor.Left
self.A = Motor(motorPins[0], motorPins[1], motorPins[2], motorPins[3])
self.B = Motor(motorPins[4], motorPins[5], motorPins[6], motorPins[7])
self.sensors = Sensors()
self.actuators = Actuators()
self.minDelayMotors = 2
self.state = enums.State.paused
self.sensors.setSensor(enums.SensorType.light, 1, 17) # Previous: 16
self.sensors.setSensor(enums.SensorType.light, 2, 4) # Previous: 2
self.sensors.setSensor(enums.SensorType.emergencyStop, 1, 3) # Previous: 6
self.sensors.setSensor(enums.SensorType.touch, 1, 27) # Previous: 8
self.sensors.setSensor(enums.SensorType.touch, 2, 2) # Previous: 13
self.sensors.setSensor(enums.SensorType.touch, 3, 18) # Previous: 7
self.sensors.setSensor(enums.SensorType.proximity, 1, 19) # Previous: 10
self.sensors.setSensor(enums.SensorType.proximity, 2, 21) # Previous: 11
self.sensors.setSensor(enums.SensorType.proximity, 3, 22) # Previous: x
self.sensors.setSensor(enums.SensorType.proximity, 4, 26) # Previous: x
self.actuators.initActuator(enums.ActuatorType.led, 1, ledPins[0]) # Previous: 19
self.actuators.initActuator(enums.ActuatorType.led, 2, ledPins[1]) # Previous: 26
self.actuators.initActuator(enums.ActuatorType.led, 3, ledPins[2]) # Previous: x
self.actuators.initActuator(enums.ActuatorType.led, 4, ledPins[3]) # Previous: x
self.lastTouch = [-1,-1,-1]
#print "light sensor value:"
#print self.sensors.readSensor(enums.SensorType.light, 1)
#if not isLightCalibrated:
#self.calibrateLight()
# try:
# thread.start_new_thread(self.pauseAndResume, ())
# except:
# print "Error: unable to start thread"
messages.printMessage('greetings')
while self.getSensorData(enums.SensorType.emergencyStop, 1) == 0:
time.sleep(0.1)
messages.printMessage('running')
self.state = enums.State.running
def getSensorData(self, sensor_type, position):
if (sensor_type == enums.SensorType.touch):
if (position < 1 or position > 3):
messages.printMessage('bad_touch_sensor')
self.blinkLEDs_error()
return -1
elif (sensor_type == enums.SensorType.light):
if (position != 1 and position!=2):
messages.printMessage('bad_light_sensor')
self.blinkLEDs_error()
return -1
elif (sensor_type == enums.SensorType.proximity):
if (position < 1 or position > 4):
messages.printMessage('bad_proximity_sensor')
self.blinkLEDs_error()
return -1
elif (sensor_type == enums.SensorType.emergencyStop):
if (position != 1):
messages.printMessage('bad_emergency_sensor')
self.blinkLEDs_error()
return -1
else:
messages.printMessage('bad_sensor')
self.blinkLEDs_error()
return -1
value = self.sensors.readSensor(sensor_type, position)
if sensor_type == enums.SensorType.light:
return value
if (upperBoundLight - lowerBoundLight) == 0:
messages.printMessage('no_calibration')
self.blinkLEDs_error()
return -1
# Scale
lightVal = int(9 - round(abs(value-upperBoundLight)/(abs(upperBoundLight - lowerBoundLight)/9)))
if lightVal < 0:
messages.printMessage('no_calibration')
self.blinkLEDs_error()
return -1
return lightVal
elif sensor_type == enums.SensorType.touch:
return self.getSwitchTriggered(position,value)
elif sensor_type == enums.SensorType.emergencyStop:
return value % 2
else:
return value
def getSensorData(self, sensor_type, position):
"""
Returns a different value depending on the type of sensor queried:
- For the light sensor: an int in the range [0, 9]
- For the touch sensor: 1 if the sensor has been pressed since the last time it was queried, 0 if not
- For the e-stop: 1 if it's ON, 0 if it's OFF
- For the proximity sensor: 1 if there's an obstacle (it's ON), 0 if not (it's OFF)
:param sensor_type: type of the sensor queried
:param position: position in the PCB of the sensor queried
:type sensor_type: enums.SensorType
:type position: int
:rtype: int
"""
if (sensor_type == enums.SensorType.touch):
if (position < 1 or position > 3):
messages.printMessage('bad_touch_sensor')
self.blinkLEDs_error()
return -1
elif (sensor_type == enums.SensorType.emergencyStop):
if (position != 4):
messages.printMessage('bad_emergency_sensor')
self.blinkLEDs_error()
return -1
elif (sensor_type == enums.SensorType.light):
if (position != 1 and position!=2):
messages.printMessage('bad_light_sensor')
self.blinkLEDs_error()
return -1
elif (sensor_type == enums.SensorType.proximity):
if (position < 1 or position > 4):
messages.printMessage('bad_proximity_sensor')
self.blinkLEDs_error()
return -1
else:
messages.printMessage('bad_sensor')
self.blinkLEDs_error()
return -1
# The value of the sensor is read at lowlevel
value = self.sensors.readSensor(sensor_type, position)
# For the light sensor, 'value' is the count of the light
if sensor_type == enums.SensorType.light:
return value
if (upperBoundLight - lowerBoundLight) == 0:
messages.printMessage('no_calibration')
self.blinkLEDs_error()
return -1
# A scale to the range [0, 9] is done
lightVal = int(9 - round(abs(value-upperBoundLight)/(abs(upperBoundLight - lowerBoundLight)/9)))
if lightVal < 0:
messages.printMessage('no_calibration')
self.blinkLEDs_error()
return -1
return lightVal
# For the touch sensor, 'value' is the number of times the sensor has been pressed
elif sensor_type == enums.SensorType.touch:
# Returns if the sensor has been pressed since the last time it was queried
return self.getSwitchTriggered(position,value)
# For the e-stop, 'value' is the number of times the sensor has been pressed
elif sensor_type == enums.SensorType.emergencyStop:
# Returns if it's either 1 (ON) or 0 (OFF)
return value % 2
# For the proximity sensor, 'value' is either 1 (ON) or 0 (OFF)
elif sensor_type == enums.SensorType.proximity:
# Returns 1 (ON) or 0 (OFF)
return value
