def motor_2 ():
''' Define a task to run both motors in order. Configure the motor using the motor_driver.py file. Print the data to the serial port and plot the data in pc_main_lab3'''
# call class MotorDriver()
motor_2 = motor_driver.MotorDriver('C1', 20000)
# call class Controller()
ctr_2 = controller.Controller(0.01, 16000, 30)
## define the encoder that is used to read the motor position
encC = enc.Encoder('C')
# define x to be True to collect half of the data points an allow more time for printing data
x = True
while True:
# initialize the motor controller using class Controller()
motor_2.set_duty_cycle(ctr_2.outputValue(encC.read()))
# create a string of data to be plotted
data = '2, ' + str(encC.read()) + ', ' + str(utime.ticks_ms()) + '\r\n'
# print data every other pass through the while loop
if x == True:
print_task.put (data)
x = not x
# yield to another task and resume at this line
yield (0)
def run_fun (self):
""" Run function for the @c BusyTask. This function doesn't do much
except to verify that a class member can be a task function. """
while True:
print_task.put ('[' + str (self.ser_num) + ']')
yield (0)
def motor_1 ():
''' Define a task to run both motors in order. Configure the motor using the motor_driver.py file. Print the data to the serial port and plot the data in pc_main_lab3'''
# call class MotorDriver()
motor = motor_driver.MotorDriver('A10', 20000)
# call class Controller()
ctr = controller.Controller(0.01, 16000, 30)
## define the encoder that is used to read the motor position
encB = enc.Encoder('B')
# run the proportional contorller on the motor and feed the data into the shares and queues using put()
x = True
while True:
# set the motor duty cycle using class Controller()
motor.set_duty_cycle(ctr.outputValue(encB.read()))
# create a string of data to be plotted
data = '1, ' + str(encB.read()) + ', ' + str(utime.ticks_ms()) + '\r\n'
# print data every other pass through the while loop
if x == True:
print_task.put (data)
# yield to another task and resume at this line
yield (0)
def task1_fun():
''' Function which runs for Task 1, which toggles twice every second in a
way which is only slightly silly. '''
state = STOPPED
counter = 0
while True:
if state == GOING:
print_task.put('GOING\n')
state = STOPPED
elif state == STOPPED:
print_task.put('STOPPED\n')
state = GOING
else:
raise ValueError('Illegal state for task 1')
# Periodically check and/or clean up memory
counter += 1
if counter >= 60:
counter = 0
print_task.put(' Memory: {:d}\n'.format(gc.mem_free()))
yield (state)
def move_motor_fun():
""" Function to move the motor to each of the 9 squares that we need coordinates for during our calibration run"""
heading_motor = ProjectClasses.MotorDriver(3, pyb.Pin.board.PA10,
pyb.Pin.board.PB4,
pyb.Pin.board.PB5)
heading_motor.set_duty_cycle(0)
pitch_motor = ProjectClasses.MotorDriver(5, pyb.Pin.board.PC1,
pyb.Pin.board.PA0,
pyb.Pin.board.PA1)
pitch_motor.set_duty_cycle(0)
state = 1
pitch_adj = 2
heading_adjust = 2
while True:
if state == 1:
char = vcp.read(1)
if char is not None:
if char == b'w':
print_task.put('w')
pitch_setpoint_share.put(pitch_setpoint_share.get() +
pitch_adj)
elif char == b'a':
print_task.put('a')
heading_setpoint_share.put(heading_setpoint_share.get() -
heading_adjust)
elif char == b's':
print_task.put('s')
pitch_setpoint_share.put(pitch_setpoint_share.get() -
pitch_adj)
elif char == b'd':
print_task.put('d')
heading_setpoint_share.put(heading_setpoint_share.get() +
heading_adjust)
elif char == b' ':
set_coord_share.put(1)
if calibrated_share.get() == 1:
state = 2
yield (0)
elif state == 2:
yield (0)
def Control3():
''' Proportional Control of Motor 3, which controls the z-axis '''
global mdone1
global mdone2
global mdone3
global NEWVAL3
Controls = INPUT
counter = 0
cont_count = 0
md = md3.MD3(pyb.Pin.board.PB0, pyb.Pin.board.PB8, pyb.Pin.board.PB9, 3)
enc = encoder.Encoder(pyb.Pin.board.PA8, pyb.Pin.board.PA9, 1)
motorvoltage = 12
controller = pcontroller.Pcontroller(motorvoltage)
enc.zero()
md.set_duty_cycle(0)
kp = 0.1
ki = 0
# psat = 0.15*motorvoltage/ki
# nsat = -psat
psat = nsat = 0
controller.setgain(kp, ki, psat, nsat)
location = 0
#motor initialize stuff
###change to new motors
while True:
if Controls == INPUT: #if in INPUT state
# mdone3 = md_3.get(False)
ref = sm3.get(False)
# enc.zero()
controller.setsetpoint(ref)
#print('I3')
if NEWVAL3 == 1:
Controls = GOING
NEWVAL3 = 0
#print('set')
elif Controls == GOING:
# if NEWVAL3 == 1:
# ref = sm3.get(False)
# controller.setsetpoint(ref)
# NEWVAL3 = 0
location = enc.read()
pwm = controller.control(location)
md.set_duty_cycle(pwm)
cont_count += 1
if abs(location - ref) <= 50:
mdone3 = 1
if mdone3 == 1 and mdone2 == 1 and mdone1 == 1:
md.set_duty_cycle(0)
Controls = INPUT
print('3' + ',' + str(ref) + ',' + str(location))
else:
raise ValueError('Illegal state for task 1')
# Periodically check and/or clean up memory
counter += 1
if counter >= 60:
counter = 0
print_task.put(' Memory: {:d}\n'.format(gc.mem_free()))
#print(Controls)
yield (Controls)
def Control2():
''' Proportional Control of Motor 2, which controls the 2nd link
which holds the paintbrush'''
global mdone1
global mdone2
global mdone3
global NEWVAL2
Controls = INPUT
counter = 0
cont_count = 0
md = motordriver.MotorDriver(pyb.Pin.board.PC1, pyb.Pin.board.PA0,
pyb.Pin.board.PA1, 5)
enc = encoder.Encoder(pyb.Pin.board.PC6, pyb.Pin.board.PC7, 8)
motorvoltage = 12
controller = pcontroller.Pcontroller(motorvoltage)
enc.zero()
md.set_duty_cycle(0)
kp = 0.15
ki = 0
# psat = 0.15*motorvoltage/ki
# nsat = -psat
psat = nsat = 0
controller.setgain(kp, ki, psat, nsat)
#motor initialize stuff
###change to new motors
while True:
if Controls == INPUT: #if in INPUT state
# mdone2 = md2.get(False)
ref = sm2.get(False)
# enc.zero()
controller.setsetpoint(ref)
if NEWVAL2 == 1:
Controls = GOING
NEWVAL2 = 0
elif Controls == GOING:
# if NEWVAL2 == 1:
# ref = sm2.get(False)
# controller.setsetpoint(ref)
# NEWVAL2 = 0
location = enc.read()
pwm = controller.control(location)
if pwm >= 25:
pwm = 25
elif pwm <= -25:
pwm = -25
md.set_duty_cycle(pwm)
cont_count += 1
if ((abs(location - ref) <= 10)):
mdone2 = 1
if mdone3 == 1 and mdone2 == 1 and mdone1 == 1:
md.set_duty_cycle(0)
Controls = INPUT
else:
mdone2 = 0
#print(str(ref) + ',' + str(mdone2) + ',' + str(NEWVAL2))
print('2' + ',' + str(ref) + ',' + str(location))
else:
raise ValueError('Illegal state for task 1')
# Periodically check and/or clean up memory
counter += 1
if counter >= 60:
counter = 0
print_task.put(' Memory: {:d}\n'.format(gc.mem_free()))
yield (Controls)
def Control1():
''' Proportional Control of Motor 1, which controls the first link '''
Controls = INPUT #Change state to INPUT
counter = 0
cont_count = 0
global mdone1
global mdone2
global mdone3
global NEWVAL1
#motor initialize stuff
###change to new motors
motorvoltage = 12
md = motordriver.MotorDriver(pyb.Pin.board.PA10, pyb.Pin.board.PB4,
pyb.Pin.board.PB5, 3)
enc = encoder.Encoder(pyb.Pin.board.PB6, pyb.Pin.board.PB7, 4)
controller = pcontroller.Pcontroller(motorvoltage)
enc.zero()
md.set_duty_cycle(0)
kp = 0.15
ki = 0
# psat = 0.15*motorvoltage/ki
# nsat = -psat
psat = nsat = 0
controller.setgain(kp, ki, psat, nsat)
while True:
if Controls == INPUT: #if in INPUT state
# mdone1 = md1.get(False)
ref = sm1.get(False)
# enc.zero()
#print('I1')
controller.setsetpoint(ref)
if NEWVAL1 == 1:
Controls = GOING
NEWVAL1 = 0
#print('SET')
#print(NEWVAL1)
elif Controls == GOING:
# if NEWVAL1 == 1:
# ref = sm1.get(False)
# controller.setsetpoint(ref)
# NEWVAL1 = 0
location = enc.read()
pwm = controller.control(location)
if pwm >= 25:
pwm = 25
elif pwm <= -25:
pwm = -25
md.set_duty_cycle(pwm)
cont_count += 1
if abs(location - ref) <= 10:
mdone1 = 1
if mdone3 == 1 and mdone2 == 1 and mdone1 == 1:
md.set_duty_cycle(0)
Controls = INPUT
else:
mdone1 = 0
#print(str(ref) + ',' + str(location) + ',' + str(abs(location-ref)) + ',' + str(mdone1) + ',' + str(pwm))
#print(str(ref) + ',' + str(mdone1) + ',' + str(NEWVAL1))
print('1' + ',' + str(ref) + ',' + str(location))
else:
raise ValueError('Illegal state for task 1')
# Periodically check and/or clean up memory
counter += 1
if counter >= 60:
counter = 0
print_task.put(' Memory: {:d}\n'.format(gc.mem_free()))
yield (Controls)
