def handleCommand(command, keyPosition, price=0):
# only uses pressing down of keys
if keyPosition != "down":
return
print("handle command", command, keyPosition)
if command == 'L':
gopigo.left_rot()
time.sleep(0.15)
gopigo.stop()
if command == 'R':
gopigo.right_rot()
time.sleep(0.15)
gopigo.stop()
if command == 'F':
gopigo.forward()
time.sleep(0.4)
gopigo.stop()
if command == 'B':
gopigo.backward()
time.sleep(0.3)
gopigo.stop()
robot_util.handleSoundCommand(command, keyPosition, price)
def turn_in_place(degrees_to_turn): # Turning amount should not be more than 360 degrees degrees_to_turn = degrees_to_turn % FULL_REVOLUTION_DEGREES if degrees_to_turn == 0: return # Make turning efficient so that robot does not turn more than half turn if abs(degrees_to_turn) > HALF_REVOLUTION_DEGREES: if degrees_to_turn > 0: degrees_to_turn = -1 * (FULL_REVOLUTION_DEGREES - degrees_to_turn) else: degrees_to_turn = FULL_REVOLUTION_DEGREES + degrees_to_turn #Compute the number of encoder steps needed encoder_steps_needed = int(ENCODER_STEPS_FOR_ABOUT_TURN * abs(degrees_to_turn) / FULL_REVOLUTION_DEGREES) #If encoder steps needed are zero, due to truncation, do nothing if encoder_steps_needed == 0: return #Turn the number of encoder steps computed gopigo.enable_encoders() gopigo.enc_tgt(1, 1, abs(encoder_steps_needed)) turnAngleSign = 1 if degrees_to_turn > 0: gopigo.right_rot() else: gopigo.left_rot() turnAngleSign = -1 wait_till_encoder_target_reached() RobotPosePrediction.currentRobotPose = RobotPosePrediction.getPredictedRobotPose(currentRobotPose = RobotPosePrediction.currentRobotPose, movementType = RobotPosePrediction.MOVEMENT_TYPE_ROTATE_IN_PLACE, movementAmount = turnAngleSign * encoder_steps_needed * FULL_REVOLUTION_DEGREES / ENCODER_STEPS_PER_REVOLUTION) RobotPosePrediction.currentRobotLocationUncertainty = RobotPosePrediction.getPredictedRobotUncertainty(currentRobotLocationUncertainty = RobotPosePrediction.currentRobotLocationUncertainty, movementType = RobotPosePrediction.MOVEMENT_TYPE_ROTATE_IN_PLACE, movementAmount = turnAngleSign * encoder_steps_needed * FULL_REVOLUTION_DEGREES / ENCODER_STEPS_PER_REVOLUTION, currentRobotPose = RobotPosePrediction.currentRobotPose)
def move(self, dist, how="F"):
"""this takes care of moving the robot.
how can be
F: which is forward, dist is in cm
L: which is left (right motor on left motor off), dist is in degrees
R: which is right (left motor on right motor off), dist is in degrees
B: which is backward. dist is in cm
TL: rotate in place, to the left (tank steering) dist is in pulses
TR: rotate in place, to the right (tank steering) dist is in pulses
"""
if (dist < 30 and (how == "F" or how == "B")):
#If dist <30 the robot will
#try to move 0 distance which means go for ever. Bad news!
return -1
elif (dist < 8 and (how == "L" or how == "R")):
#also the robot rotates for ever if the angle is less than 8
return -1
else:
#take an initial record
#self.recordEncoders()
#save the current position because we're about to reset it
#self.keyframe = self.timerecord[-1][:-1]
#basically we are recording our position while moving.
#prevtime = int(time.time())
#go forward! this runs in the background pretty much
if (how == "F"):
go.fwd(dist)
elif (how == "L"):
go.turn_left(dist)
elif (how == "R"):
go.turn_right(dist)
elif (how == "B"):
go.bwd(dist)
elif (how == "TR"):
go.enc_tgt(0, 1, dist)
go.right_rot()
elif (how == "TL"):
go.enc_tgt(0, 1, dist)
go.left_rot()
#record while we haven't reached our destination
#while(go.read_enc_status()):
#this resets both encoders so we start counting from zero!
#this next part should only trigger once per recordFreq
#if(time.time()-prevtime>self.recordFreq):
#prevtime = time.time() #make sure to reset the timer...
#tell the recordEncoders function which direction
#we are going
#dir = "forward"
#if(how=="B"):
# dir = "backward"
#self.recordEncoders(dir=dir)
return 1
def turn_right():
if msg_en:
print("Turn right")
if gpg_en:
gopigo.enc_tgt(0, 1, 15)
gopigo.fwd()
time.sleep(.9)
gopigo.stop()
time.sleep(1)
gopigo.set_speed(80)
gopigo.enc_tgt(1, 1, 35)
gopigo.right_rot()
time.sleep(.7)
gopigo.stop()
time.sleep(1)
curr = absolute_line_pos()
ic.right_turns += 1
def run(self, command):
if command == 'L':
gopigo.left_rot()
time.sleep(0.15)
gopigo.stop()
elif command == 'R':
gopigo.right_rot()
time.sleep(0.15)
gopigo.stop()
elif command == 'F':
gopigo.forward()
time.sleep(0.35)
gopigo.stop()
elif command == 'B':
gopigo.backward()
time.sleep(0.35)
gopigo.stop()
def moveGoPiGo2(command):
if command == 'L':
gopigo.left_rot()
time.sleep(0.15)
gopigo.stop()
if command == 'R':
gopigo.right_rot()
time.sleep(0.15)
gopigo.stop()
if command == 'F':
gopigo.forward()
time.sleep(0.35)
gopigo.stop()
if command == 'B':
gopigo.backward()
time.sleep(0.35)
gopigo.stop()
def turn_in_place(degrees_to_turn):
# Turning amount should not be more than 360 degrees
degrees_to_turn = degrees_to_turn % FULL_REVOLUTION_DEGREES
if degrees_to_turn == 0:
return
# Make turning efficient so that robot does not turn more than half turn
if abs(degrees_to_turn) > HALF_REVOLUTION_DEGREES:
if degrees_to_turn > 0:
degrees_to_turn = -1 * (FULL_REVOLUTION_DEGREES - degrees_to_turn)
else:
degrees_to_turn = FULL_REVOLUTION_DEGREES + degrees_to_turn
#Compute the number of encoder steps needed
encoder_steps_needed = int(ENCODER_STEPS_FOR_ABOUT_TURN *
abs(degrees_to_turn) / FULL_REVOLUTION_DEGREES)
#If encoder steps needed are zero, due to truncation, do nothing
if encoder_steps_needed == 0:
return
#Turn the number of encoder steps computed
gopigo.enable_encoders()
gopigo.enc_tgt(1, 1, abs(encoder_steps_needed))
turnAngleSign = 1
if degrees_to_turn > 0:
gopigo.right_rot()
else:
gopigo.left_rot()
turnAngleSign = -1
wait_till_encoder_target_reached()
RobotPosePrediction.currentRobotPose = RobotPosePrediction.getPredictedRobotPose(
currentRobotPose=RobotPosePrediction.currentRobotPose,
movementType=RobotPosePrediction.MOVEMENT_TYPE_ROTATE_IN_PLACE,
movementAmount=turnAngleSign * encoder_steps_needed *
FULL_REVOLUTION_DEGREES / ENCODER_STEPS_PER_REVOLUTION)
RobotPosePrediction.currentRobotLocationUncertainty = RobotPosePrediction.getPredictedRobotUncertainty(
currentRobotLocationUncertainty=RobotPosePrediction.
currentRobotLocationUncertainty,
movementType=RobotPosePrediction.MOVEMENT_TYPE_ROTATE_IN_PLACE,
movementAmount=turnAngleSign * encoder_steps_needed *
FULL_REVOLUTION_DEGREES / ENCODER_STEPS_PER_REVOLUTION,
currentRobotPose=RobotPosePrediction.currentRobotPose)
def turn_right(self):
gopigo.set_right_speed(35)
gopigo.set_left_speed(35)
gopigo.forward()
time.sleep(.8)
gopigo.right_rot()
time.sleep(.5)
while True:
self.read_position()
self.read_position()
self.read_position()
pos = self.read_position()
print(pos)
debug(pos)
if pos == "center":
gopigo.stop()
break
def move(args):
command = args['command']
if command == 'L':
gopigo.left_rot()
time.sleep(0.15)
gopigo.stop()
if command == 'R':
gopigo.right_rot()
time.sleep(0.15)
gopigo.stop()
if command == 'F':
gopigo.forward()
time.sleep(0.35)
gopigo.stop()
if command == 'B':
gopigo.backward()
time.sleep(0.35)
gopigo.stop()
def move(args):
command = args['button']['command']
if command == 'l':
gopigo.left_rot()
time.sleep(0.15)
gopigo.stop()
if command == 'r':
gopigo.right_rot()
time.sleep(0.15)
gopigo.stop()
if command == 'f':
gopigo.forward()
time.sleep(0.35)
gopigo.stop()
if command == 'b':
gopigo.backward()
time.sleep(0.35)
gopigo.stop()
def _process_left_thumbstick(self, data):
"""
Read the state of the left thumbstick and issue the corresponding command
to GoPiGo. If no input were given, return False. Return
True otherwise.
"""
# The valid values ranges from -1.0 to 1.0. Thus, setting threshold
# at 0.8.
threshold = 0.8
# Read the Left thumb stick
X = data['LeftThumbstickX']
Y = data['LeftThumbstickY']
# Take the absolute value
abs_X = abs(X)
abs_Y = abs(Y)
if abs_X > abs_Y and abs_X > threshold:
if X > 0:
# rotate right
gopigo.right_rot()
else:
# rotate left
gopigo.left_rot()
elif abs_Y > abs_X and abs_Y > threshold:
if Y > 0:
# move forward
gopigo.motor_fwd()
else:
# move backward
gopigo.motor_bwd()
else:
# No Input
return False
return True
def _process_left_thumbstick(self, data):
"""
Read the state of the left thumbstick and issue the corresponding command
to GoPiGo. If no input were given, return False. Return
True otherwise.
"""
# The valid values ranges from -1.0 to 1.0. Thus, setting threshold
# at 0.8.
threshold = 0.8
# Read the Left thumb stick
X = data['LeftThumbstickX']
Y = data['LeftThumbstickY']
# Take the absolute value
abs_X = abs(X)
abs_Y = abs(Y)
if abs_X > abs_Y and abs_X > threshold:
if X > 0:
# rotate right
gopigo.right_rot()
else:
# rotate left
gopigo.left_rot()
elif abs_Y > abs_X and abs_Y > threshold:
if Y > 0:
# move forward
gopigo.motor_fwd()
else:
# move backward
gopigo.motor_bwd()
else:
# No Input
return False
return True
def _process_dpad(self, data):
"""
Read the state of the dpad buttons and issue the corresponding command
to GoPiGo. If no dpad buttons were pressed, return False. Return
True otherwise.
"""
if data['Buttons'] & self._button_dpad_up:
# move forward
gopigo.motor_fwd()
elif data['Buttons'] & self._button_dpad_down:
# move backward
gopigo.motor_bwd()
elif data['Buttons'] & self._button_dpad_left:
# rotate left
gopigo.left_rot()
elif data['Buttons'] & self._button_dpad_right:
# rotate right
gopigo.right_rot()
else:
# no input
return False
return True
def _process_dpad(self, data):
"""
Read the state of the dpad buttons and issue the corresponding command
to GoPiGo. If no dpad buttons were pressed, return False. Return
True otherwise.
"""
if data['Buttons'] & self._button_dpad_up:
# move forward
gopigo.motor_fwd()
elif data['Buttons'] & self._button_dpad_down:
# move backward
gopigo.motor_bwd()
elif data['Buttons'] & self._button_dpad_left:
# rotate left
gopigo.left_rot()
elif data['Buttons'] & self._button_dpad_right:
# rotate right
gopigo.right_rot()
else:
# no input
return False
return True
print "---------------------------->Turning state"
#Starting speed
time.sleep(2)
print(
'Start Heading={0:0.2F},Targt Heading={1:0.2F},Current Heading={2:0.2F},Diff={3:0.2F}'
.format(start_heading, target_heading, heading,
target_heading - heading))
gopigo.set_speed(60)
start_heading = heading
target_heading = start_heading + 90.0
if target_heading < start_heading:
target_heading += 360.0
turning_start = 1
gopigo.right_rot()
else:
if target_heading > 360:
if heading < 270:
heading += 360
if target_heading - heading < 5:
print "------------------------------------------->target reached"
gopigo.stop()
turning_start = 0
turning_stat = 0
moving_forward_state = 1
start_flag = 0
print(
'Start Heading={0:0.2F},Targt Heading={1:0.2F},Current Heading={2:0.2F},Diff={3:0.2F}'
.format(start_heading, target_heading, heading,
target_heading - heading))
def right_rot(kargs):
r = {'return_value': gopigo.right_rot()}
return r
def do_command(command=None):
logging.debug(command)
if command in ["forward", "fwd"]:
gopigo.fwd()
elif command == "left":
gopigo.left()
elif command == "left_rot":
gopigo.left_rot()
elif command == "right":
gopigo.right()
elif command == "right_rot":
gopigo.right_rot()
elif command == "stop":
gopigo.stop()
elif command == "leftled_on":
gopigo.led_on(0)
elif command == "leftled_off":
gopigo.led_off(0)
elif command == "rightled_on":
gopigo.led_on(1)
elif command == "rightled_off":
gopigo.led_off(1)
elif command in ["back", "bwd"]:
gopigo.bwd()
elif command == "speed":
logging.debug("speed")
speed = flask.request.args.get("speed")
logging.debug("speed:" + str(speed))
if speed:
logging.debug("in if speed")
gopigo.set_speed(int(speed))
left_speed = flask.request.args.get("left_speed")
logging.debug("left_speed:" + str(left_speed))
if left_speed:
logging.debug("in if left_speed")
gopigo.set_left_speed(int(left_speed))
right_speed = flask.request.args.get("right_speed")
logging.debug("right_speed:" + str(right_speed))
if right_speed:
logging.debug("in if right_speed")
gopigo.set_right_speed(int(right_speed))
speed_result = gopigo.read_motor_speed()
logging.debug(speed_result)
return flask.json.jsonify({"speed": speed_result, "right": speed_result[0], "left": speed_result[1]})
elif command == "get_data":
speed_result = gopigo.read_motor_speed()
enc_right = gopigo.enc_read(0)
enc_left = gopigo.enc_read(1)
volt = gopigo.volt()
timeout = gopigo.read_timeout_status()
return flask.json.jsonify(
{
"speed": speed_result,
"speed_right": speed_result[0],
"speed_left": speed_result[1],
"enc_right": enc_right,
"enc_left": enc_left,
"volt": volt,
"timeout": timeout,
"fw_ver": gopigo.fw_ver(),
}
)
elif command in ["enc_tgt", "step"]:
tgt = flask.request.args.get("tgt")
direction = flask.request.args.get("dir")
if tgt:
gopigo.gopigo.enc_tgt(1, 1, int(tgt))
if dir:
if dir == "bwd":
gopigo.bwd()
else:
gopigo.fwd()
else:
gopigo.fwd()
return ""
def rightRot(aTicks):
if checkTicks(aTicks):
gpg.enc_tgt(1, 1, aTicks)
gpg.right_rot()
waitForTarget()
conn.send("Moving back")
elif data=='left':
gopigo.left()
conn.send("Turning left")
elif data=='left_rot':
gopigo.left_rot()
conn.send("Turning left_rot")
elif data=='right':
gopigo.right()
conn.send("Turning right")
elif data=='right_rot':
gopigo.right_rot()
conn.send("Turning right")
elif data=='i':
gopigo.increase_speed()
conn.send("Increase speed")
elif data=='d':
gopigo.decrease_speed()
conn.send("Decrease speed")
elif data=='ledOn':
gopigo.led_on()
conn.send("Led ON")
elif data=='ledOff':
def tank_right(degrees):
go.enc_tgt(0, 1, degrees)
go.right_rot()
