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_left():
if msg_en:
print("Turn left")
if gpg_en:
gopigo.enc_tgt(0, 1, 6)
gopigo.fwd()
time.sleep(1.1)
curr = absolute_line_pos()
def bwd_cm(dist=None):
'''
Move chassis bwd by a specified number of cm.
This function sets the encoder to the correct number
of pulses and then invokes bwd().
'''
if dist is not None:
pulse = int(cm2pulse(dist))
enc_tgt(1,1,pulse)
bwd()
def go_backwards(backwards_distance): gopigo.enable_encoders() encoder_steps_required = int(backwards_distance / DISTANCE_PER_ENCODER_STEP) gopigo.set_speed(DEFAULT_ROBOT_SPEED) gopigo.enc_tgt(1, 1, encoder_steps_required) gopigo.bwd() wait_till_encoder_target_reached() RobotPosePrediction.currentRobotPose = RobotPosePrediction.getPredictedRobotPose(currentRobotPose = RobotPosePrediction.currentRobotPose, movementType = RobotPosePrediction.MOVEMENT_TYPE_FORWARD, movementAmount = -1 * encoder_steps_required * DISTANCE_PER_ENCODER_STEP) RobotPosePrediction.currentRobotLocationUncertainty = RobotPosePrediction.getPredictedRobotUncertainty(currentRobotLocationUncertainty = RobotPosePrediction.currentRobotLocationUncertainty, movementType = RobotPosePrediction.MOVEMENT_TYPE_FORWARD, movementAmount = -1 * encoder_steps_required * DISTANCE_PER_ENCODER_STEP, currentRobotPose = RobotPosePrediction.currentRobotPose)
def right_deg(deg=None):
'''
Turn chassis right by a specified number of degrees.
DPR is the #deg/pulse (Deg:Pulse ratio)
This function sets the encoder to the correct number
of pulses and then invokes right().
'''
if deg is not None:
pulse= int(deg/DPR)
enc_tgt(0,1,pulse)
right()
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_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():
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 go_forward(forward_distance):
gopigo.enable_encoders()
encoder_steps_required = int(forward_distance / DISTANCE_PER_ENCODER_STEP)
gopigo.set_speed(DEFAULT_ROBOT_SPEED)
gopigo.enc_tgt(1, 1, encoder_steps_required)
gopigo.fwd()
wait_till_encoder_target_reached()
RobotPosePrediction.currentRobotPose = RobotPosePrediction.getPredictedRobotPose(
currentRobotPose=RobotPosePrediction.currentRobotPose,
movementType=RobotPosePrediction.MOVEMENT_TYPE_FORWARD,
movementAmount=encoder_steps_required * DISTANCE_PER_ENCODER_STEP)
RobotPosePrediction.currentRobotLocationUncertainty = RobotPosePrediction.getPredictedRobotUncertainty(
currentRobotLocationUncertainty=RobotPosePrediction.
currentRobotLocationUncertainty,
movementType=RobotPosePrediction.MOVEMENT_TYPE_FORWARD,
movementAmount=encoder_steps_required * DISTANCE_PER_ENCODER_STEP,
currentRobotPose=RobotPosePrediction.currentRobotPose)
def turn_around():
if msg_en:
print("Turn around")
if gpg_en:
gopigo.bwd()
curr = absolute_line_pos()
gopigo.enc_tgt(0, 1, 5)
gopigo.stop()
time.sleep(.5)
gopigo.set_speed(80)
gopigo.enc_tgt(1, 1, 35)
gopigo.left_rot()
time.sleep(.7)
gopigo.stop()
time.sleep(1)
curr = absolute_line_pos()
ic.turn += 1
def left(aTicks):
if checkTicks(aTicks):
gpg.enc_tgt(0, 1, aTicks)
gpg.left()
waitForTarget()
def leftRot(aTicks):
if checkTicks(aTicks):
gpg.enc_tgt(1, 1, aTicks)
gpg.left_rot()
waitForTarget()
def back(aTicks):
if checkTicks(aTicks):
gpg.enc_tgt(1, 1, aTicks)
gpg.bwd()
waitForTarget()
def fwd(aTicks):
if checkTicks(aTicks):
gpg.enc_tgt(1, 1, aTicks)
gpg.fwd()
waitForTarget()
def tank_right(degrees):
go.enc_tgt(0, 1, degrees)
go.right_rot()
def tank_left(degrees):
go.enc_tgt(0, 1, degrees)
go.left_rot()
def process_command(self, command):
parts = command.split("/")
if parts[1] == "poll":
print "poll"
self.us_dist = gopigo.us_dist(usdist_pin)
self.enc_status = gopigo.read_status()[0]
self.volt = gopigo.volt()
self.fw_ver = gopigo.fw_ver()
self.trim = gopigo.trim_read() - 100
if self.enc_status == 0:
self.waitingOn = None
elif parts[1] == "stop":
gopigo.stop()
elif parts[1] == "trim_write":
gopigo.trim_write(int(parts[2]))
self.trim = gopigo.trim_read()
elif parts[1] == "trim_read":
self.trim = gopigo.trim_read() - 100
elif parts[1] == "set_speed":
if parts[2] == "left":
self.left_speed = int(parts[3])
elif parts[2] == "right":
self.right_speed = int(parts[3])
else:
self.right_speed = int(parts[3])
self.left_speed = int(parts[3])
gopigo.set_left_speed(self.left_speed)
gopigo.set_right_speed(self.right_speed)
elif parts[1] == "leds":
val = 0
if parts[3] == "on":
val = 1
elif parts[3] == "off":
val = 0
elif parts[3] == "toggle":
val = -1
if parts[2] == "right" or parts[2] == "both":
if val >= 0:
self.ledr = val
else:
self.ledr = 1 - self.ledr
if parts[2] == "left" or parts[2] == "both":
if val >= 0:
self.ledl = val
else:
self.ledl = 1 - self.ledl
gopigo.digitalWrite(ledr_pin, self.ledr)
gopigo.digitalWrite(ledl_pin, self.ledl)
elif parts[1] == "servo":
gopigo.servo(int(parts[2]))
elif parts[1] == "turn":
self.waitingOn = parts[2]
direction = parts[3]
amount = int(parts[4])
encleft = 0 if direction == "left" else 1
encright = 1 if direction == "left" else 0
gopigo.enable_encoders()
gopigo.enc_tgt(encleft, encright, int(amount / DPR))
if direction == "left":
gopigo.left()
else:
gopigo.right()
elif parts[1] == "move":
self.waitingOn = int(parts[2])
direction = parts[3]
amount = int(parts[4])
gopigo.enable_encoders()
gopigo.enc_tgt(1, 1, amount)
if direction == "backward":
gopigo.bwd()
else:
gopigo.fwd()
elif parts[1] == "beep":
gopigo.analogWrite(buzzer_pin, self.beep_volume)
time.sleep(self.beep_time)
gopigo.analogWrite(buzzer_pin, 0)
elif parts[1] == "reset_all":
self.ledl = 0
self.ledr = 0
gopigo.digitalWrite(ledl_pin, self.ledl)
gopigo.digitalWrite(ledr_pin, self.ledr)
gopigo.analogWrite(buzzer_pin, 0)
# gopigo.servo(90)
gopigo.stop()
def rightRot(aTicks):
if checkTicks(aTicks):
gpg.enc_tgt(1, 1, aTicks)
gpg.right_rot()
waitForTarget()
#!/usr/bin/env python import gopigo # arg1 : motor left # arg2 : motor rigth # arg3 : steps (18 par tour) gopigo.enc_tgt(sys.argv[1], sys.argv[2], sys.argv[3])
def right(aTicks):
if checkTicks(aTicks):
gpg.enc_tgt(1, 0, aTicks)
gpg.right()
waitForTarget()
def enc_tgt(kargs):
r = {'return_value': gopigo.enc_tgt(int(kargs['m1']), int(kargs['m2']), int(kargs['target']))}
return r
