コード例 #1
0
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)
コード例 #2
0
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()
コード例 #3
0
ファイル: control.py プロジェクト: bilykigor/snoop
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()
コード例 #4
0
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)
コード例 #5
0
ファイル: control.py プロジェクト: bilykigor/snoop
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()
コード例 #6
0
 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
コード例 #7
0
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)
コード例 #8
0
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
コード例 #9
0
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)
コード例 #10
0
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
コード例 #11
0
ファイル: __init__.py プロジェクト: tsceats/gopigo
def left(aTicks):
    if checkTicks(aTicks):
        gpg.enc_tgt(0, 1, aTicks)
        gpg.left()
        waitForTarget()
コード例 #12
0
ファイル: __init__.py プロジェクト: tsceats/gopigo
def leftRot(aTicks):
    if checkTicks(aTicks):
        gpg.enc_tgt(1, 1, aTicks)
        gpg.left_rot()
        waitForTarget()
コード例 #13
0
ファイル: __init__.py プロジェクト: tsceats/gopigo
def back(aTicks):
    if checkTicks(aTicks):
        gpg.enc_tgt(1, 1, aTicks)
        gpg.bwd()
        waitForTarget()
コード例 #14
0
ファイル: __init__.py プロジェクト: tsceats/gopigo
def fwd(aTicks):
    if checkTicks(aTicks):
        gpg.enc_tgt(1, 1, aTicks)
        gpg.fwd()
        waitForTarget()
コード例 #15
0
 def tank_right(degrees):
     go.enc_tgt(0, 1, degrees)
     go.right_rot()
コード例 #16
0
 def tank_left(degrees):
     go.enc_tgt(0, 1, degrees)
     go.left_rot()
コード例 #17
0
ファイル: main.py プロジェクト: donniet/gopigo-scratch
    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()
コード例 #18
0
ファイル: __init__.py プロジェクト: tsceats/gopigo
def rightRot(aTicks):
    if checkTicks(aTicks):
        gpg.enc_tgt(1, 1, aTicks)
        gpg.right_rot()
        waitForTarget()
コード例 #19
0
ファイル: encode.py プロジェクト: theplatypus/GoPiGo_Java
#!/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])
コード例 #20
0
ファイル: __init__.py プロジェクト: tsceats/gopigo
def right(aTicks):
    if checkTicks(aTicks):
        gpg.enc_tgt(1, 0, aTicks)
        gpg.right()
        waitForTarget()
コード例 #21
0
ファイル: gopi_proxy.py プロジェクト: sumsted/gopi
def enc_tgt(kargs):
    r = {'return_value': gopigo.enc_tgt(int(kargs['m1']), int(kargs['m2']), int(kargs['target']))}
    return r