def turn_slight_right(): if msg_en: print("Turn slight right") if gpg_en: gopigo.set_right_speed(fwd_speed) gopigo.set_left_speed(slight_turn_speed) gopigo.fwd()
def set_left_speed(self,new_speed): _ifMutexAcquire(self.use_mutex) try: gopigo.set_left_speed(new_speed) except: pass _ifMutexRelease(self.use_mutex)
def turn_fast_right(): if msg_en: print("Turn fast left") if gpg_en: gopigo.set_right_speed(fast_turn_speed) gopigo.set_left_speed(fwd_speed) gopigo.fwd()
def set_left_speed(self, new_speed): _grab_read() try: gopigo.set_left_speed(new_speed) except: pass _release_read()
def actualize_power(self): if self.stop: gopigo.set_left_speed(0) gopigo.set_right_speed(0) else: gopigo.set_left_speed(self.left_power) gopigo.set_right_speed(self.right_power)
def setMotorJoystickPos( self, joystickX, joystickY ): joystickX, joystickY = self.normaliseJoystickData( joystickX, joystickY ) if debug: print "Left joy",joystickX, joystickY print self.speed_l*joystickY gopigo.set_left_speed(int(self.speed_l*joystickY)) gopigo.fwd()
def __init__(self, recordFreq=0.5, camera=None, stream=None): """this function intializes everything, including setting the speed""" go.set_right_speed(50) go.set_left_speed(50) go.enable_encoders() self.camera = camera self.stream = stream
def set_speed_lr(speed, l_diff, r_diff): if speed >= MIN_SPEED: gopigo.set_left_speed(int(speed + l_diff)) gopigo.set_right_speed(int(speed + r_diff)) else: gopigo.set_left_speed(0) gopigo.set_right_speed(0)
def turn_slight_left(): if msg_en: print "Turn slight left" if gpg_en: gopigo.set_right_speed(slight_turn_speed) gopigo.set_left_speed(fwd_speed) gopigo.fwd()
def rescue(): gopigo.stop() print("Rescue") time.sleep(1) gopigo.set_left_speed(130) gopigo.set_right_speed(130) gopigo.fwd() time.sleep(0.2)
def rotate_left(self): _grab_read() try: gopigo.set_right_speed(25) gopigo.set_left_speed(0) except Exception as e: print("idk wtf") pass
def forwardTicks(distance, speed): distance = max(1, min(distance, 500)) speed = max(50, min(speed, 200)) leftTicks = distance rightTicks = distance right_speed = speed left_speed = speed set_left_speed(left_speed) set_right_speed(right_speed) leftStart = gpg.enc_read(0) rightStart = gpg.enc_read(1) leftTarget = leftStart + leftTicks rightTarget = rightStart + rightTicks isLeftMoving = False isRightMoving = False adjustment_interval = 1 last_left_check = leftStart last_right_check = rightStart while(True): leftReading = gpg.enc_read(0) rightReading = gpg.enc_read(1) leftToEnd = leftTarget - leftReading rightToEnd = rightTarget - rightReading if leftReading >= leftTarget and rightReading >= rightTarget: gpg.stop() break elif leftReading < leftTarget and rightReading < rightTarget: new_left_speed = speed new_right_speed = speed if (leftToEnd > rightToEnd + 1): extraFactor = float(leftToEnd - rightToEnd) / leftToEnd extraFactor = max(0.02, min(0.15, extraFactor)) new_left_speed = int(speed * (1.0 + extraFactor)) elif (rightToEnd > leftToEnd + 1): extraFactor = float(rightToEnd - leftToEnd) / rightToEnd extraFactor = max(0.02, min(0.15, extraFactor)) new_right_speed = int(speed * (1.0 + extraFactor)) if (left_speed != new_left_speed): set_left_speed(new_left_speed) left_speed = new_left_speed if (right_speed != new_right_speed): set_right_speed(new_right_speed) right_speed = new_right_speed if (not isLeftMoving) or (not isRightMoving): print "Forward!" gpg.motor_fwd() isLeftMoving = True isRightMoving = True
def get(self): left = int(self.get_argument("left")) right = int(self.get_argument("right")) gopigo.set_left_speed(abs(left)) gopigo.set_right_speed(abs(right)) if (left > 0): gopigo.fwd() else: gopigo.bwd()
def main(self, motor1, motor2): """ Takes in 2 ints and rotates accordingly """ gopigo.set_left_speed(motor1) gopigo.set_right_speed(motor2) if motor1 >= 0 and motor2 >= 0: gopigo.fwd() else: gopigo.bwd()
def turn_right(self): gopigo.set_right_speed(0) gopigo.set_left_speed(25) gopigo.forward() time.sleep(2) 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 follow_line(self, fwd_speed = 80): slight_turn_speed=int(.7*fwd_speed) while True: pos = self.read_position() debug(pos) if pos == "center": gopigo.forward() elif pos == "left": gopigo.set_right_speed(0) gopigo.set_left_speed(slight_turn_speed) elif pos == "right": gopigo.set_right_speed(slight_turn_speed) gopigo.set_left_speed(0) elif pos == "black": gopigo.stop() elif pos == "white": gopigo.stop()
def around(): gopigo.set_left_speed(250) gopigo.set_right_speed(250) gopigo.left_rot() time.sleep(1.30) while True: time.sleep(0.2) marker, t = aruco.get_result() if t > 0.01: break gopigo.stop() gopigo.set_left_speed(250) gopigo.set_right_speed(250) gopigo.left_rot() time.sleep(0.2) gopigo.stop() time.sleep(0.2)
def turn_around(self): gopigo.set_right_speed(35) gopigo.set_left_speed(35) gopigo.backward() time.sleep(1) gopigo.stop() time.sleep(1) gopigo.left_rot() time.sleep(2) while True: self.read_position() self.read_position() pos = self.read_position() print(pos) debug(pos) if pos == "center": gopigo.stop() break
def __actualize_power(self, l_diff, r_diff): """ Applies the decided motor powers to the motors of the rover. If the desired motor power is below the minimum allowed motor power, then the motors are stopped to prevent issues with motor differences at low motor powers. :param l_diff: The additional motor power to apply to just the left motor. (power units) :param r_diff: The additional motor power to apply to just the right motor. (power units) """ if self.speed >= MIN_SPEED: gopigo.set_left_speed(int(self.speed + l_diff)) gopigo.set_right_speed(int(self.speed + r_diff)) else: gopigo.set_left_speed(0) gopigo.set_right_speed(0)
def calibrate(speed): gpg.trim_write(0) sleep(2) set_left_speed(speed) set_right_speed(speed) left = -1 right = 1 sp = 0 while left != right: both = oneRun() both2 = oneRun() left = both[0] + both2[0] right = both[1] + both2[1] if left > right + 1: sp -= 1 elif (left + 1 < right): sp += 1 print left, right, sp gpg.trim_write(sp) sleep(2)
def do_turn(d): global turn_angle, compass gopigo.set_left_speed(210) gopigo.set_right_speed(210) time.sleep(0.1) compass.turn_c(d) if d == "left": gopigo.turn_left_wait_for_completion(turn_angle) elif d == "around": around() gopigo.stop() else: try: gopigo.turn_right_wait_for_completion(turn_angle) except TypeError: do_turn(d) time.sleep(0.1) gopigo.fwd() drive_forwards(0.5)
def follow_line(self, fwd_speed=30): slight_turn_speed = int(35) print("FOLLOWING LINE") while True: self.read_position() self.read_position() #print(self.read()) pos = self.read_position() #print(pos) debug(pos) if pos == "center": gopigo.set_speed(30) gopigo.forward() elif pos == "wayleft": gopigo.set_right_speed(25) gopigo.set_left_speed(40) elif pos == "wayright": gopigo.set_right_speed(40) gopigo.set_left_speed(25) elif pos == "left": gopigo.set_right_speed(25) gopigo.set_left_speed(slight_turn_speed) elif pos == "right": gopigo.set_right_speed(slight_turn_speed) gopigo.set_left_speed(25) elif pos == "white": print("white Brake") gopigo.stop() break elif pos == "intersection": gopigo.stop() break elif pos == "unknown": print("Unknown???") gopigo.stop() break else: break
def correctLeft(speed): print "Correcting LEFT!" go.set_left_speed(speed - correctionNeeded(speed)) #Corrects wheel size offset by multiplying by 2. go.set_right_speed(speed + correctionNeeded(speed) * 2)
def correctRight(speed): print "Correcting RIGHT!" go.set_left_speed(speed + correctionNeeded(speed)) go.set_right_speed(speed - correctionNeeded(speed))
def set_left_speed(speed=100): go.set_left_speed(speed)
def set_slave(speed): #go.set_right_speed(speed) go.set_left_speed(speed)
def main(): global network, state, prev_marker, algoInstance if network is None: # Setup network ip = "localhost" with open("server.ip") as f: ip = f.read() x, y = 1, 0 network = netemuclient.NetEmuClient(rec, ip, 8080) network.start() network.waitForMaze() network.position(x, y) network.txpower(0.02) if algoInstance is None: # If the program is started with arguments, this will load the last saved state if len(sys.argv) > 1: with open("last_state.pickle", "rb") as f_pickle: algoInstance = pickle.loads(f_pickle.read()) algoInstance.restoreState(network) network.position(*algoInstance.position) # Start the algorithm without a saved state else: algoInstance = algo.Algorithm(network, (x, y)) # Give everything time to warm up time.sleep(2) gopigo.set_left_speed(0) gopigo.set_right_speed(0) gopigo.fwd() save_timer = time.time() # Save the latest encoder reading save_enc = (0, 0) while True: # Move in the alorithm algoInstance.step() # Call the latest camera results (marker, t) = aruco.get_result() # GoPiGo is not very stable, this block is just to make it stable if save_timer + 3 < time.time(): try: new_enc = (gopigo.enc_read(0), gopigo.enc_read(1)) except TypeError: print( "GoPiGo breaks when you enc read sometimes just restart the main, the state should be fine" ) gopigo.stop() time.sleep(0.2) gopigo.stop() main() # We have been stopping while we should be driving if new_enc == save_enc and state == State.DRIVE: rescue() save_enc = new_enc # Update the state state = change_state(marker, t) if state == State.DRIVE: drive_forwards(t) elif state == State.STOP: gopigo.stop() elif state == State.TURN_LEFT: do_turn("left") elif state == State.TURN_RIGHT: do_turn("right") elif state == State.TURN_AROUND: do_turn("around") time.sleep(0.001) else: raise ValueError
def turn_right_around(self): gopigo.set_right_speed(0) gopigo.set_left_speed(50) time.sleep(5)
def follow_line(self): slight_turn_speed = int(50) default_speed = int(35) wayoff_turn_speed = int(115) print("FOLLOWING LINE") gopigo.set_speed(default_speed) gopigo.forward() time.sleep(1) while True: self.read_position() self.read_position() #print(self.read()) pos = self.read_position() #print(pos) debug(pos) if pos == "center": gopigo.set_speed(default_speed) gopigo.forward() elif pos == "wayleft": gopigo.set_right_speed(default_speed) gopigo.set_left_speed(wayoff_turn_speed) elif pos == "wayright": gopigo.set_right_speed(wayoff_turn_speed) gopigo.set_left_speed(default_speed) elif pos == "left": gopigo.set_right_speed(default_speed) gopigo.set_left_speed(slight_turn_speed) elif pos == "right": gopigo.set_right_speed(slight_turn_speed) gopigo.set_left_speed(default_speed) elif pos == "white": print("white Brake") gopigo.stop() time.sleep(1) whiteTest = self.read_position() whiteTest = self.read_position() if whiteTest == "white": print("really white") break else: gopigo.forward() elif pos == "intersection": gopigo.stop() time.sleep(1) gopigo.set_right_speed(40) gopigo.set_left_speed(40) gopigo.forward() time.sleep(1.2) gopigo.stop() time.sleep(1) intersectionTest = self.read_position() intersectionTest = self.read_position() print("Reading found!" + intersectionTest) gopigo.backward() time.sleep(1.5) gopigo.stop() if intersectionTest != "white": print("interesection hit!") else: print("T intersection hit!") break elif pos == "left corner" or "right corner": gopigo.stop() time.sleep(1) gopigo.set_right_speed(40) gopigo.set_left_speed(40) gopigo.forward() time.sleep(1.2) gopigo.stop() time.sleep(1) intersectionTest = self.read_position() intersectionTest = self.read_position() print("Reading found! " + intersectionTest) gopigo.backward() time.sleep(1.5) gopigo.stop() if intersectionTest != "white": print("T interesection hit!") break else: print("corner! turning!") if pos == "left corner": self.turn_left() else: self.turn_right() gopigo.forward() time.sleep(.5) elif pos == "unknown": print("Unknown???") #gopigo.stop() #break else: break print(pos) gopigo.stop()
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 drive_forwards(target): left, right = get_control_out(target) time.sleep(0.001) gopigo.set_left_speed(int(left)) gopigo.set_right_speed(int(right))
dist = data['us1'] except Exception as e: # Something went wrong extracting the JSON. dist = -1 # Handle the situation. print(e) pass if dist != -1: # If a JSON was correctly extracted, continue. # Print received to the console print("LS1: ", ls1, "LS2: ", ls2, "LS3: ", ls3, "LS4: ", ls4, "LS5: ", ls5, "DIST: ", dist) # 0 = black # 1 = white # Line following logic goes here if ls5 == 1 and ls4 == 1: go.set_left_speed(gospeed) go.set_right_speed(gospeed) go.fwd() elif ls5 == 1 and ls4 != 1: go.set_left_speed(gospeed) go.set_right_speed(gospeed + 15) go.fwd() elif ls5 != 1 and ls4 == 1: go.set_left_speed(gospeed + 15) go.set_right_speed(gospeed) go.fwd() else: go.fwd()
def defaultCorrection(speed): print "Correcting!" go.set_left_speed(speed - correctionNeeded(speed)) go.set_right_speed(speed + correctionNeeded(speed))
right_speed -= rotate_value right_speed = min(max(right_speed, -1), 1) left_speed = min(max(left_speed, -1), 1) left_speed *= copysign(255, move_value) right_speed *= copysign(255, move_value) return [left_speed, right_speed] while(True): left_speed = 0 right_speed = 0 if controller.read(): move_value = controller.y rotate_value = controller.x [left_speed, right_speed] = calc_speed(move_value, rotate_value) if left_speed != 0 or right_speed != 0: print "controls: ({0}, {1})".format(controller.x, controller.y) dir = "forward" if move_value >= 0 else "back" print "{0} left:{1} right:{2}".format(dir, left_speed, right_speed) gopigo.set_left_speed(int(abs(left_speed))) gopigo.set_right_speed(int(abs(right_speed))) if left_speed > 0: if right_speed > 0: gopigo.fwd() else: gopigo.right_rot() else: if right_speed > 0: gopigo.left_rot() else: gopigo.bwd()
def set_left_speed(kargs): r = {'return_value': gopigo.set_left_speed(int(kargs['speed']))} return r
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': gopigo.led_off() conn.send("Led OFF") elif data=='set_right_speed': gopigo.set_right_speed() conn.send("Set Right Speed") elif data=='set_left_speed': gopigo.set_left_speed() conn.send("Set Left Speed") else: print ("Invalid command") conn.send("Invalid command") conn.close()
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()
#!/usr/bin/env python # arguemtn : speed : 0 - 255 import gopigo gopigo.set_left_speed(sys.argv[1])
while dist < SAFE_DISTANCE: dist = us_dist(USS) time.sleep(0.01) gopigo.fwd() #time.sleep(0.1) elapsed_ticks_left = enc_read(LEFT) - initial_ticks_left #time.sleep(0.1) elapsed_ticks_right = enc_read(RIGHT) - initial_ticks_right print("L: " + str(elapsed_ticks_left) + "\tR: " + str(elapsed_ticks_right)) if elapsed_ticks_left > elapsed_ticks_right: print("RIGHT SLOW") if CORRECTION: gopigo.set_left_speed(SPEED - INC) gopigo.set_right_speed(SPEED + INC) elif elapsed_ticks_left < elapsed_ticks_right: print("LEFT SLOW") if CORRECTION: gopigo.set_left_speed(SPEED + INC) gopigo.set_right_speed(SPEED - INC) else: print("Equal") if CORRECTION: gopigo.set_left_speed(SPEED) gopigo.set_right_speed(SPEED) print(time.time() - t) except Exception, e: print(e)