def find_max_and_min(
    servo_number
):  #Used as a testing function, wasn't impimented in The final demonstration
    global chan0, chan1, chan2, chan3, chan4, chan5, chan6, chan7, servo
    chan = get_SPI_channel(servo_number)
    servo_speed(servo_number, 7)
    maximum = 0
    minimum = 5
    run_time = 40
    t0 = time.clock()
    while (True):
        value = chan.voltage
        if value > maximum:
            maximum = value
        if value < minimum:
            minimum = value
        t1 = time.clock()
        if (t1 - t0) > run_time:
            servo[servo_number]["max voltage"] = maximum
            servo[servo_number]["min voltage"] = minimum
            print('Maximum Voltage for Servo ', servo_number, ' is ', maximum,
                  "V")
            print('Minimum Voltage for Servo ', servo_number, ' is ', minimum,
                  "V")
            servo_speed(servo_number, 0)
            break
def button_control(servo_number,
                   input_value):  #Moves a Servo on a button press

    global servo

    if input_value == 1:
        servo_speed(servo_number, -15)

    elif input_value == 0:
        servo_speed(servo_number, 0)
def trigger_control(
    servo_number, event_code, input_value
):  #Used for testing only, moves a servo when the trigger threshold is exceded

    global left_trigger_threshold, right_trigger_threshold2, servo

    if event_code == 2:
        threshold = left_trigger_threshold

        if input_value >= threshold:
            servo_speed(servo_number, 15)
        else:
            servo_speed(servo_number, 0)

    elif event_code == 5:

        threshold = right_trigger_threshold2
        if input_value >= threshold:
            move_standard_servo(3, -3)
    else:
        raise ValueError("Improper event code was given to trigger_control")
def joystick_control(
    servo_number, event_code, input_value
):  #The algorithm that determines how much a joystick moves a servo motor

    global left_joystick_x_threshold, left_joystick_y_threshold, right_joystick_x_threshold, right_joystick_y_threshold, joysitck_division_number

    if event_code == 0:
        threshold = list(left_joystick_x_threshold)

    elif event_code == 1:
        threshold = list(left_joystick_y_threshold)

    elif event_code == 3:
        threshold = list(right_joystick_x_threshold)

    elif event_code == 4:
        threshold = list(right_joystick_y_threshold)

    else:
        raise ValueError("Improper event code was given to joystick_control")

    if input_value < threshold[0] and input_value > threshold[1]:
        servo_speed(servo_number, 0)

    elif input_value > threshold[0]:
        x = (input_value - threshold[0]) / joystick_division_number
        round(x, 1)
        servo_speed(servo_number, x)

    elif input_value < threshold[1]:
        x = (input_value + threshold[1]) / (joystick_division_number)
        round(x, 1)
        servo_speed(servo_number, x)
def move_servo_position(
    servo_number
):  #Used with the calibrate function, was designed to move a 360 servo to a certain position
    global servo, servo_speed
    buff = 20
    main_speed = 0
    speed_threshold = servo[servo_number]['calibrate speed']
    direction = servo[servo_number]['direction']
    up_speed = speed_threshold * direction
    down_speed = -speed_threshold * direction
    start_speed = 0
    cur_val = feedback(servo_number)
    goal_val = servo[servo_number]['starting angle']
    set_val = servo[servo_number]['shift angle']
    max_val = servo[servo_number]['max angle']
    min_val = servo[servo_number]['min angle']

    if goal_val <= cur_val + buff and goal_val >= cur_val - buff:
        print(goal_val, " degrees was found!")
        servo_speed(servo_number, 0)
        return goal_val

    elif max_val > goal_val and min_val < goal_val:
        if cur_val < goal_val:
            print(" We landed at part 0")
            main_speed = up_speed
        else:
            main_speed = down_speed
            print(" We landed at part 1")

    elif max_val == goal_val or min_val == goal_val:
        raise ValueError(
            "The condition of set_val == goal_val has been neglected by the programmer"
        )

    else:
        if goal_val < max_val:
            if cur_val < goal_val or cur_val > min_val:
                main_speed = up_speed
                print(" We landed at part 2")
            else:
                main_speed = down_speed
                print(" We landed at part 3")
        elif goal_val > min_val:
            if cur_val < goal_val and cur_val > min_val:
                main_speed = up_speed
                print(" We landed at part 4")
            else:
                main_speed = down_speed
                print(" We landed at part 5")

    servo_speed(servo_number, main_speed)
    while (True):
        new_val = feedback(servo_number)
        if goal_val <= new_val + buff and goal_val >= new_val - buff:
            servo_speed(servo_number, 0)
            print(goal_val, "degrees was found!")
            break
    return goal_val
Exemple #6
0
def initialize_servos():  #Stops Servos before main
    servo_speed(0, 0)
    servo_speed(1, 3)
    servo_speed(2, 0)
Exemple #7
0
def stop_servos():  #Stops programs once the code
    servo_speed(0, 0)
    servo_speed(1, 0)
    servo_speed(2, 0)
def jaw_control(
    servo_number_one, servo_number_two, event_code, input_value
):  #The function that allows for two motors to drive opposite of each other

    global servo, left_trigger_threshold, right_trigger_threshold, trigger_division_number

    if event_code == 2:
        threshold = left_trigger_threshold

    elif event_code == 5:
        threshold = right_trigger_threshold

    else:
        raise ValueError("Improper event code was given to trigger_control")

    if input_value < threshold:
        servo_speed(servo_number_one, 0)
        servo_speed(servo_number_two, 0)

    elif input_value >= threshold:
        x = (input_value - threshold) / trigger_division_number
        neg_x = -x + -2
        x = round(x, 0)
        neg_x = round(neg_x, 0)

        if event_code == 5:
            servo_speed(servo_number_one, x)
            servo_speed(servo_number_two, neg_x)

        elif event_code == 2:
            servo_speed(servo_number_one, neg_x)
            servo_speed(servo_number_two, x)