def rightThumbYCallBack(value):
print "Right Thumb Y, Value = {}".format(value)
if value < 0:
initio.forward(speed)
elif value > 0:
initio.reverse(speed)
else:
initio.stop()
def rightThumbXCallBack(value):
print "Right Thumb X, Value = {}".format(value)
if value < 0:
initio.spinLeft(speed)
elif value > 0:
initio.spinRight(speed)
else:
initio.stop()
def reroute(changepin):
changePin = int(changepin) #cast changepin to an int
if changePin == 1:
initio.spinLeft(speed)
elif changePin == 2:
initio.forward(speed)
elif changePin == 3:
initio.spinRight(speed)
elif changePin == 4:
initio.reverse(speed)
else:
initio.stop()
response = make_response(redirect(url_for('index')))
return (response)
def execute():
print('Startup')
initio.init(Motors=True)
cmd_forwards = 'f'
cmd_backwards = 'b'
cmd_left = 'l'
cmd_right = 'r'
cmd_stop = 's'
cmd_query = '?'
r = redis.Redis(host='192.168.0.1', port=6379,
db=0, decode_responses=True)
p = r.pubsub(ignore_subscribe_messages=True)
p.subscribe('fernando')
r.publish('services', 'fernando.on')
systemd.daemon.notify('READY=1')
print('Startup complete')
try:
initio.stop()
r.publish('fernando.status', cmd_stop)
last_cmd = cmd_stop
for message in p.listen():
cmd = message['data']
if cmd == cmd_forwards:
initio.forward(100)
elif cmd == cmd_backwards:
initio.reverse(100)
elif cmd == cmd_left:
initio.spinLeft(100)
elif cmd == cmd_right:
initio.spinRight(100)
elif cmd == cmd_stop:
initio.stop()
elif cmd == cmd_query:
# Don't need to do anything in this case
pass
if cmd != cmd_query:
last_cmd = cmd
r.publish('fernando.status', last_cmd)
except:
p.close()
initio.stop()
initio.cleanup()
r.publish('services', 'fernando.off')
print('Goodbye')
def getMessage() :
while True :
try:
global message
message = clisock.recv(100)
print message
except socket.error:
continue
# If condition box is ticked, variables are assigned so robot does an action
# if it encounters an obstacle
if (message.find('y') == 0) :
global shouldCheck
shouldCheck = True
print 'shouldCheck is ' + str(shouldCheck)
global conditionDistance
conditionDistance = message[1:4].lstrip("0")
print 'conditionDistance = ' + conditionDistance
global conditionCommand
conditionCommand = message[4:-4]
print 'conditionCommand = ' + conditionCommand
global conditionValue
conditionValue = message[-4:].lstrip("0")
print 'conditionValue = ' + str(float(conditionValue))
# Condition box is not ticked, so obstacles are not checked for
elif (message.find('n') == 0) :
global shouldCheck
shouldCheck = False
print 'shouldCheck is ' + str(shouldCheck)
# If Java program is closed, this program closes the socket and waits for
# another connection
elif (message == 'quit') :
lock.acquire()
clisock.close()
print 'Closing socket and creating new one...'
createSocket()
lock.release()
# Shuts down the raspberry pi
elif (message == 'shutdown') :
clisock.close()
shutdown()
# Starts sending the ultrasonic sensor distance every 2 seconds. If the
# distance is already being sent, it stops sending it
elif (message == 'ultra') :
if shouldUltra == True:
global shouldUltra
print 'changing shouldUltra to False'
shouldUltra = False
else :
global shouldUltra
print 'changing shouldUltra to True'
shouldUltra = True
# Makes the robot follow a line
elif (message == 'followline') :
linesensor.followLine()
# Stops the robot and clears commands from the queue
elif (message == 'stop') :
lock.acquire()
initio.stop()
commands[:] = ['none']
values [:] = ['none']
lock.release()
# Sets the servo position back to the middle
elif (message == 'servoinit') :
servo.panInit()
servo.tiltInit()
# Moves the servos
elif (message == 'panleft') :
servo.panLeft()
elif (message == 'panright') :
servo.panRight()
elif (message == 'tiltup') :
servo.tiltUp()
elif (message == 'tiltdown') :
servo.tiltDown()
# If anything else is sent, it must be a command so it is appended to a list
# to be executed
else :
lock.acquire()
commands.append(getCommand(message))
print commands
values.append(getValue(message))
print values
lock.release()
time.sleep(0.1)
def doCondition() : if conditionCommand == 'reverse' : initio.reverse(speed) time.sleep(float(conditionValue)) initio.stop() elif conditionCommand == 'stop' : initio.stop() elif conditionCommand == 'left' : initio.spinLeft(100) time.sleep(float(conditionValue)) initio.stop() elif conditionCommand == 'right' : initio.spinRight(100) time.sleep(float(conditionValue)) initio.stop() elif conditionCommand == 'reverseleft' : initio.turnReverse(5,100) time.sleep(float(conditionValue)) initio.stop() elif conditionCommand == 'reverseright' : initio.turnReverse(100,5) time.sleep(float(conditionValue)) initio.stop() global shouldCheck shouldCheck = False initio.stop()
def doAction() : while True: while (len(commands) != 0 and len(values) != 0) : if commands[0] == 'forward' : initio.forward(speed) time.sleep(float(values[0])) initio.stop() elif commands[0] == 'reverse' : initio.reverse(speed) time.sleep(float(values[0])) initio.stop() elif commands[0] == 'left' : initio.spinLeft(100) time.sleep(float(values[0])) initio.stop() elif commands[0] == 'right' : initio.spinRight(100) time.sleep(float(values[0])) initio.stop() elif commands[0] == 'forwardleft' : initio.turnForward(5,100) time.sleep(float(values[0])) initio.stop() elif commands[0] == 'forwardright' : initio.turnForward(100,5) time.sleep(float(values[0])) initio.stop() elif commands[0] == 'reverseright' : initio.turnReverse(100,5) time.sleep(float(values[0])) initio.stop() elif commands[0] == 'reverseleft' : initio.turnReverse(5,100) time.sleep(float(values[0])) initio.stop() lock.acquire() if (len(commands) > 1) : for i in range(1, (len(commands))) : commands[i-1] = commands[i] values [i-1] = values[i] values.pop() commands.pop() lock.release()
# To check wiring is correct ensure the order of movement as above is correct
# Run using: sudo python motorTest.py
import initio, time
speed = 100
initio.init()
# main loop
try:
while True:
initio.forward(speed)
print 'Forward'
time.sleep(3)
initio.reverse(speed)
print 'Reverse'
time.sleep(3)
initio.spinRight(speed)
print 'Spin Right'
time.sleep(3)
initio.spinLeft(speed)
print 'Spin Left'
time.sleep(3)
initio.stop()
print 'Stop'
time.sleep(3)
except KeyboardInterrupt:
initio.cleanup()
if keyp == 'w' or ord(keyp) == 16:
initio.forward(speed)
print 'Forward', speed
elif keyp == 'z' or ord(keyp) == 17:
initio.reverse(speed)
print 'Reverse', speed
elif keyp == 's' or ord(keyp) == 18:
initio.spinRight(speed)
print 'Spin Right', speed
elif keyp == 'a' or ord(keyp) == 19:
initio.spinLeft(speed)
print 'Spin Left', speed
elif keyp == '.' or keyp == '>':
speed = min(100, speed+10)
print 'Speed+', speed
elif keyp == ',' or keyp == '<':
speed = max (0, speed-10)
print 'Speed-', speed
elif keyp == ' ':
initio.stop()
print 'Stop'
elif ord(keyp) == 3:
break
except KeyboardInterrupt:
print
finally:
initio.cleanup()
def followLine() : mostRecent = 'none' counter = 0 if initio.irLeftLine() == True and initio.irRightLine() == True : while shouldFollow == True : while initio.irLeftLine() == True and initio.irRightLine() == True : counter = 0 mostRecent = 'forward' initio.forward(speed) while initio.irLeftLine() == True and initio.irRightLine() == False : counter = 0 mostRecent = 'left' #initio.turnForward(speedLow,speedHigh) initio.spinLeft(100) while initio.irLeftLine() == False and initio.irRightLine() == True : counter = 0 mostRecent = 'right' #initio.turnForward(speedHigh,speedLow) initio.spinRight(100) if initio.irLeftLine() == False and initio.irRightLine() == False : initio.stop() break # This part makes robot continue doing its last action for a second or so, to # try and find the line again if it has been lost. Optional. # while initio.irLeftLine() == False and initio.irRightLine() == False : # counter = counter + 1 # if counter > 6 : # initio.stop() # return # elif mostRecent == 'forward' : # initio.forward(speed) # time.sleep(0.1) # elif mostRecent == 'left' : # initio.turnForward(speedLow,speedHigh) # time.sleep(0.2) # elif mostRecent == 'right' : # initio.turnForward(speedHigh,speedLow) # time.sleep(0.2) # initio.stop() # elif initio.irLeftLine() == False and initio.irRightLine() == False : while shouldFollow == True: while initio.irLeftLine() == False and initio.irRightLine() == False : counter = 0 mostRecent = 'forward' initio.forward(speed) while initio.irLeftLine() == False and initio.irRightLine() == True : counter = 0 mostRecent = 'left' #initio.turnForward(speedLow,speedHigh) initio.spinLeft(100) while initio.irLeftLine() == True and initio.irRightLine() == False : counter = 0 mostRecent = 'right' initio.spinRight(100) #initio.turnForward(speedHigh,speedLow) if initio.irLeftLine() == True and initio.irRightLine() == True : initio.stop() break
