I am working on the software for an autonomous Raspberry-Pi based robot which should keep moving for 2 minutes unless an unintended key is fed to it as input. Upon receipt of the unintended key, the robot must stop.
I appropriated the code from this tutorial and it looks like this (I removed some lines for brevity):
#Obstacle avoidance
HowNear= 15
timeLimit= 2
dt = rospy.Duration(secs=120)
reversetime= 0.5
turntime= 0.75
stop= False
#Set pins as output and input
GPIO.setup(pinTrigger, GPIO.OUT) #Trigger
GPIO.setup(pinEcho, GPIO.IN) #Echo
# Set the GPIO Pin mode to be Output
GPIO.setup(pinMotorAForwards, GPIO.OUT)
GPIO.setup(pinMotorABackwards, GPIO.OUT)
GPIO.setup(pinMotorBForwards, GPIO.OUT)
GPIO.setup(pinMotorBBackwards, GPIO.OUT)
# Set the GPIO to software PWM at 'Frequency' Hertz
pwmMotorAForwards = GPIO.PWM(pinMotorAForwards, Frequency)
pwmMotorABackwards = GPIO.PWM(pinMotorABackwards, Frequency)
pwmMotorBForwards = GPIO.PWM(pinMotorBForwards, Frequency)
pwmMotorBBackwards = GPIO.PWM(pinMotorBBackwards, Frequency)
# Start the software PWM with a duty cycle of 0 (i.e. not moving)
pwmMotorAForwards.start(Stop)
pwmMotorABackwards.start(Stop)
pwmMotorBForwards.start(Stop)
pwmMotorBBackwards.start(Stop)
def myhook():
print "shutting down"
# Turn all motors off
def stopmotors():
pwmMotorAForwards.ChangeDutyCycle(Stop)
pwmMotorABackwards.ChangeDutyCycle(Stop)
pwmMotorBForwards.ChangeDutyCycle(Stop)
pwmMotorBBackwards.ChangeDutyCycle(Stop)
# Turn both motors forwards
def forwards():
pwmMotorAForwards.ChangeDutyCycle(DutyCycle)
pwmMotorABackwards.ChangeDutyCycle(Stop)
pwmMotorBForwards.ChangeDutyCycle(DutyCycle)
pwmMotorBBackwards.ChangeDutyCycle(Stop)
def isnearobstacle(localhownear):
distance= measure()
print("IsNearObstacle: " + str(distance))
if distance < localhownear:
return True
else:
return False
def measure():
GPIO.output(pinTrigger, True)
time.sleep(0.00001)
GPIO.output(pinTrigger, False)
starttime= time.time()
stoptime= starttime
while GPIO.input(pinEcho)== 0:
starttime= time.time()
stoptime= starttime
while GPIO.input(pinEcho)== 1:
stoptime= time.time()
if stoptime - starttime >= 0.04:
print("Too close/too far!")
stoptime= starttime
break
elapsedtime= stoptime - starttime
distance= (elapsedtime * 34300)/2
return distance
def avoidobstacle():
print("Backwards")
backwards()
time.sleep(reversetime)
stopmotors()
print("Right")
right()
time.sleep(turntime)
stopmotors()
def PathScanning():
GPIO.output(pinTrigger, False)
time.sleep(0.1)
globalStartTime= rospy.Time.now()
globalStopTime= globalStartTime
try:
while True:
while not rospy.is_shutdown():
if stop == True:
print(stop)
stopmotors()
elif (globalStopTime-globalStartTime) < dt:
globalStopTime= rospy.Time.now()
forwards()
time.sleep(0.1)
if isnearobstacle(HowNear):
stopmotors()
avoidobstacle()
except KeyboardInterrupt:
print("Key Press Detected. Stopping Motors")
stopmotors()
# Message handler
def CommandCallback(commandMessage):
command = commandMessage.data
if command == 'A':
PathScanning()
else:
print('Unknown command, stopping instead')
stop = True
print(stop)
stopmotors()
rospy.init_node('driver', anonymous=True)
rospy.Subscriber('command', String, CommandCallback)
rospy.spin()
print('Shutting down: stopping motors')
stopmotors()
GPIO.cleanup()
The problematic portion of the code is as follows:
def PathScanning():
GPIO.output(pinTrigger, False)
time.sleep(0.1)
globalStartTime= rospy.Time.now()
globalStopTime= globalStartTime
try:
while True:
while not rospy.is_shutdown():
if stop == True:
print(stop)
stopmotors()
elif (globalStopTime-globalStartTime) < dt:
globalStopTime= rospy.Time.now()
forwards()
time.sleep(0.1)
if isnearobstacle(HowNear):
stopmotors()
avoidobstacle()
except KeyboardInterrupt:
print("Key Press Detected. Stopping Motors")
stopmotors()
# Message handler
def CommandCallback(commandMessage):
command = commandMessage.data
if command == 'A':
PathScanning()
else:
print('Unknown command, stopping instead')
stop = True
print(stop)
stopmotors()
rospy.init_node('driver', anonymous=True)
rospy.Subscriber('command', String, CommandCallback)
My goal is to stop the robot from moving when the user passes any string/character besides 'A' as input. However, that does not happen when the PathScanning()
function is already running. When I type any string besides 'A', I see the print statements on the console but stopmotors()
is not called and the robot keeps moving (if 'A' was pressed earlier). I believe this is because PathScanning() has a loop in it:
def PathScanning():
GPIO.output(pinTrigger, False)
time.sleep(0.1)
globalStartTime= rospy.Time.now()
globalStopTime= globalStartTime
try:
while True:
while not rospy.is_shutdown():
if stop == True:
print(stop)
stopmotors()
elif (globalStopTime-globalStartTime) < dt:
globalStopTime= rospy.Time.now()
forwards()
time.sleep(0.1)
if isnearobstacle(HowNear):
stopmotors()
avoidobstacle()
except KeyboardInterrupt:
print("Key Press Detected. Stopping Motors")
stopmotors()
When I remove the loop from PathScanning()
, the robot stops when I provide any input besides 'A'. However, my other requirement of making the robot run the PathScanning()
function for exactly two minutes is not fulfilled.
How is it possible that when I give any input besides 'A', the print statements in the branch are appropriately recognized but the stopmotors()
function is not called?
stopmotors()
, does that get printed out? Also, inside the function itself, you could add some print statements. This might help narrow down the source of the problem. Later edit: Would recommend looking intoactions
for a task running for as long as 2 minutes. $\endgroup$actions
? $\endgroup$