Ejemplo n.º 1
0
 def trim_write(self, set_trim_to):
     _grab_read()
     try:
         gopigo.trim_write(int(set_trim_to))
     except:
         pass
     _release_read()
Ejemplo n.º 2
0
 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)
Ejemplo n.º 3
0
 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)
Ejemplo n.º 4
0
    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)
Ejemplo n.º 5
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 trim_write_button_OnButtonClick(self, event):
     global slider_val
     gopigo.trim_write(slider_val)
Ejemplo n.º 7
0
 def trim_write_button_OnButtonClick(self,event):
     global slider_val
     gopigo.trim_write(slider_val)
Ejemplo n.º 8
0
    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()
Ejemplo n.º 9
0
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()
Ejemplo n.º 10
0
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
Ejemplo n.º 11
0
# 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:
Ejemplo n.º 12
0
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
Ejemplo n.º 13
0
def trim_write(kargs):
    r = {'return_value': gopigo.trim_write(int(kargs['value']))}
    return r