CS 437 - Lab 1

lab1.py

import picar_4wd as fc
from picar_4wd import Ultrasonic, Pin
import time
import collections
import random

us = Ultrasonic(Pin('D8'), Pin('D9'))
power_val = 25


def run():
    # Circular buffer for distance readings
    dist_scan_list = collections.deque([], maxlen=4)
    fc.forward(power_val)

    while True:
        # Read distance data into circular buffer, then compute the average for the buffer
        reading = us.get_distance()
        dist_scan_list.append(reading)
        avg = sum(dist_scan_list) / len(dist_scan_list)

        if avg < 40.0 and len(dist_scan_list) == 4:
            print('Turn')
            # Move back
            fc.backward(power_val)
            time.sleep(.5)

            # Turn left or right
            direction = random.randint(0, 1)
            if direction == 0:
                fc.turn_left(power_val)
            else:
                fc.turn_right(power_val)
            time.sleep(1.35)

            # Stop and clear the buffer so we can move forward
            fc.stop()
            dist_scan_list = collections.deque([], maxlen=4)
        else:
            fc.forward(power_val)
        time.sleep(.1)


if __name__ == '__main__':
    try:
        run()
    except KeyboardInterrupt:
        fc.stop()
        exit(0)

lab2.py

import picar_4wd as fc
from picar_4wd import Ultrasonic, Pin, Servo, PWM
import time
import numpy as np
import math

ANGLE_RANGE = 180
STEP = 10
us_step = STEP
angle_distance = [0, 0]
current_angle = 0
max_angle = ANGLE_RANGE / 2
min_angle = -ANGLE_RANGE / 2
scan_list = []

servo = Servo(PWM("P0"), offset=0)
us = Ultrasonic(Pin('D8'), Pin('D9'))


def get_distance_at(angle):
    global angle_distance
    servo.set_angle(angle)
    time.sleep(0.04)
    distance = us.get_distance()
    angle_distance = [angle, distance]
    return distance


def get_status_at(angle, ref1=50, ref2=1):
    dist = get_distance_at(angle)
    if dist >= ref1 or dist == -2:
        return -2
    elif dist > ref2:
        return dist
    else:
        return 0


def scan_step():
    global scan_list, current_angle, us_step
    current_angle += us_step
    if current_angle >= max_angle:
        current_angle = max_angle
        us_step = -STEP
    elif current_angle <= min_angle:
        current_angle = min_angle
        us_step = STEP

    status = get_status_at(current_angle)

    scan_list.append((current_angle, status))  # TODO: add 90
    if current_angle == min_angle or current_angle == max_angle:
        if us_step < 0:
            scan_list.reverse()
        tmp = scan_list.copy()
        scan_list = []
        return tmp
    else:
        return False



def run():
    fc.stop()
    servo.set_angle(0)
    time.sleep(1)
    while True:
        arr = np.zeros((50, 100))
        x = scan_step()
        if x is not False:
            for deg, distance in x:
                if distance == -2:
                    distance = 0
                x = 50 + int(distance * math.sin(math.radians(deg)))
                y = int(distance * math.cos(math.radians(deg)))  # TODO: abs
                arr[y][x] = 1
            for _x in arr[::-1]:
                for _y in _x[::-1]:
                    print(int(_y), end='')
                print()
            print('\n')


if __name__ == '__main__':
    try:
        run()
    except KeyboardInterrupt:
        servo.set_angle(0)
        time.sleep(0.04)
        exit(0)