def trim_write(self, set_trim_to): _grab_read() try: gopigo.trim_write(int(set_trim_to)) except: pass _release_read()
def trim_write(self,set_trim_to): _ifMutexAcquire(self.use_mutex) try: gopigo.trim_write(int(set_trim_to)) except: pass _ifMutexRelease(self.use_mutex)
def __power_on(self): """ Initializes the state of the rover and ACC in order to prepare for the ACC to start controlling the rover. time.sleep calls are used here to try to reduce the chance of issues with writing to and reading from the pins used for interacting with the rover. The developers of the official python gopigo library did this in, so we figured it would be a good idea to do it in this function as it may reduce the chance of bad initial readings while only having a one time reduction in the operation time of the ACC. """ self.power_on = True # Set the trim value for the rover's motors. Even if the trim value is # 0 it should still be set in order to clear out any previous trim # setting. gopigo.trim_write(TRIM) # Get battery voltage in order to make it easy to tell when the battery # is getting low, and could thus effect performance of the rover time.sleep(0.1) volt = gopigo.volt() self.system_info.setStartupVoltage(volt) # Read the encoder tick amounts at startup to allow all future encoder # tick readings to be relative to the rover's state at the startup of # the ACC. This allows the ACC to maintain the rover's direction at # startup as straight. time.sleep(0.1) self.initial_ticks_left = gopigo.enc_read(gopigo.LEFT) time.sleep(0.1) self.initial_ticks_right = gopigo.enc_read(gopigo.RIGHT)
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 trim_write_button_OnButtonClick(self, event): global slider_val gopigo.trim_write(slider_val)
def trim_write_button_OnButtonClick(self,event): global slider_val gopigo.trim_write(slider_val)
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 main(): speed = INITIAL_SPEED gopigo.trim_write(TRIM) time.sleep(0.1) print("Volt: " + str(gopigo.volt())) time.sleep(0.1) initial_ticks_left = enc_read(LEFT) time.sleep(0.1) initial_ticks_right = enc_read(RIGHT) print("Initial\tL: " + str(initial_ticks_left) + "\tR: " + str(initial_ticks_right)) print("Critical: " + str(CRITICAL_DISTANCE)) print("Safe: " + str(SAFE_DISTANCE)) print("Alert: " + str(ALERT_DISTANCE)) dists = collections.deque(maxlen=SAMPLE_SIZE) dts = collections.deque(maxlen=SAMPLE_SIZE) elapsed_ticks_left = 0 elapsed_ticks_right = 0 try: gopigo.set_speed(0) gopigo.fwd() t = time.time() while True: if speed < 0: speed = 0 print("========================") #if not command_queue.empty(): # comm = command_queue.get() # global MAX_SPEED # global SAFE_DISTANCE # MAX_SPEED = comm[0] # SAFE_DISTANCE = comm[1] # print(comm) dt = time.time() - t t = time.time() #time.sleep(0.1) print("Time: " + str(dt)) dist = get_dist() print("Dist: " + str(dist)) if not isinstance(dist, str): dists.append(float(dist)) dts.append(float(dt)) rel_speed = None if len(dists) > 9: rel_speed = calculate_relative_speed(dists, dts) print("Rel speed: " + str(rel_speed)) if (isinstance(dist, str) and dist != NOTHING_FOUND) or dist < CRITICAL_DISTANCE: print("< Critical") stop_until_safe_distance() speed = 0 t = time.time() elif dist < SAFE_DISTANCE: print("< Safe") if speed > STOP_THRESHOLD: #speed = speed - dt * SPEED_DECCELLERATION speed = speed - dt * get_deccelleration(speed) else: speed = 0 elif speed > MAX_SPEED: print("Slowing down") speed = speed - dt * SLOWING_DECCELLERATION elif dist < ALERT_DISTANCE and rel_speed is not None: speed = handle_alert_distance(speed, rel_speed, dt) elif speed < MAX_SPEED: print("Speeding up") speed = speed + dt * SPEED_ACCELERATION #speed = speed - dt * get_deccelleration(speed) elapsed_ticks_left, elapsed_ticks_right = \ read_enc_ticks(initial_ticks_left, initial_ticks_right) print("L: " + str(elapsed_ticks_left) + "\tR: " + str(elapsed_ticks_right)) l_diff, r_diff = straightness_correction(speed, elapsed_ticks_left, elapsed_ticks_right) if elapsed_ticks_left >= 0 and elapsed_ticks_right >= 0: set_speed_lr(speed, l_diff, r_diff) else: set_speed_lr(speed, 0, 0) print("Speed: " + str(speed)) except (KeyboardInterrupt, Exception): traceback.print_exc() gopigo.stop() gopigo.stop()
SPEED = int(sys.argv[1]) INC = (SPEED / 100) * 10 #7#15 SAFE_DISTANCE = 45 USS = 15 TRIM = int(sys.argv[2]) CORRECTION = sys.argv[3] == "true" print(CORRECTION) # Might as well try this out, probably doesn't do anything though gopigo.disable_encoders() gopigo.enable_encoders() gopigo.trim_write(TRIM) ADDRESS = 0x08 ENC_READ_CMD = [53] import smbus bus = smbus.SMBus(1) def write_i2c_block(address, block): try: op = bus.write_i2c_block_data(address, 1, block) time.sleep(0.005) return op except IOError: return -1
# Sets the speed of the motors. # speed1: speed of motor1 # speed2: speed of motor2 def set_motors(speed1, speed2): go.motor1(1, speed1) go.motor2(1, speed2) # Get program parameters target_speed = input("Enter target speed ") stop_dist = input("Enter target stop distance ") #trim = input ("Enter trim ") print "Starting rover" go.trim_write(-10) #Make rover start moving set_motors(target_speed, target_speed) stopped = False while True: dist = go.us_dist(15) # If stopped but no object is too close, resume # Else if moving and an object is too close, stop if stopped and dist > stop_dist: set_motors(target_speed, target_speed) stopped = False elif not stopped and dist < stop_dist:
import gopigo as go import time def set_motors(s1, s2): go.motor1(1, s1) go.motor2(1, s2) go.trim_write(0) lwt = go.enc_read(0) rwt = go.enc_read(1) oldlwt = lwt oldrwt = rwt target_speed = input("Speed?") lms = target_speed rms = target_speed print "Starting" set_motors(target_speed, target_speed) while True: oldlwt = lwt oldrwt = rwt lwt = go.enc_read(0) rwt = go.enc_read(1) lwtdiff = lwt - oldlwt rwtdiff = rwt - oldrwt error = rwtdiff - lwtdiff print "L, R, E: ", lwtdiff, rwtdiff, error
def trim_write(kargs): r = {'return_value': gopigo.trim_write(int(kargs['value']))} return r