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 intoactionsfor a task running for as long as 2 minutes. $\endgroup$actions? $\endgroup$