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
def initialize_servos(): #Stops Servos before main
servo_speed(0, 0)
servo_speed(1, 3)
servo_speed(2, 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)