def __init__(self, position, coordinate, timer, EN_Pin, pin1, pin2, Kp, Ki,
Kd, saturation):
''' Construct an encoder task function by initilizing any shared
variables and initialize the encoder object
@param position The shared variable between tasks that contains the Encoder readings
@param coordinate The desired coordinate to which to move the motor
@param timer The Motor's timer channel
@param EN_pin The Motor's __?__ pin
@param pin1 The Motor's first pin, Pin A
@param pin2 The Motor's second pin, Pin B
@param Kp The Motor Controller's proportional gain
@param Ki The Motor Controller's integral gain
@param Setpoint Where the motor is desired to go
@param saturation The anti wind up saturation limit
'''
self.position = position
self.coordinate = coordinate
self.Motor = motor.MotorDriver(timer, EN_Pin, pin1, pin2)
self.Controller = controller.Controller(Kp, Ki, Kd, saturation)
def task2_roll_motor():
## Closed Loop object is created and used throughout
cL = loop.Closed_Loop()
## Setpoint is set to 0, as this indicated the table being level
cL.set_setpoint(0)
## The gains for our PI controller were determined through trial and error
cL.set_cont_gain(3.7, 0.07)
mot2 = motor.MotorDriver("B")
while True:
## if numbers in queue are available
## store value in variable curr_pos
curr_pos = q3.get()
#print(str(curr_pos))
actuation = cL.control(curr_pos)
q5.put(actuation)
mot2.set_duty_cycle(actuation)
q7.put(cL.error_sum)
yield (0)
def task_motor2():
''' Function which runs for Task 1, which toggles twice every second in a
way which is only slightly silly. '''
control = controller.Controller(0.1, 0)
motor2 = motor.MotorDriver(pyb.Pin.board.PA1, pyb.Pin.board.PA0,
pyb.Pin.board.PC1, pyb.Timer(2))
encoder2 = encoder.Encoder(pyb.Pin.board.PC7, pyb.Pin.board.PC6,
pyb.Timer(8))
state = STOPPED
start_count = utime.ticks_ms()
running_count = utime.ticks_ms()
while True:
pwm = control.calculate(encoder2.get_position())
motor2.set_duty_cycle(pwm)
running_count = utime.ticks_ms()
if state == STOPPED:
control.clear_list()
if (running_count > (start_count + 10000)):
control.set_setpoint(5000)
start_count = utime.ticks_ms()
state = GOING
elif state == GOING:
if (running_count > (start_count + 300)):
control.set_setpoint(0)
encoder2.zero()
start_count = utime.ticks_ms()
state = PRINT
elif state == PRINT:
control.print_results()
control.clear_list()
print("print end2")
state = STOPPED
yield (state)
@author: Jason Grillo, Thomas Goehring, Trent Peterson
"""
#Yellow Encoder Cable: B7
#Gray Encoder Cable: B6
import pyb
import motor
import controller
import utime
import BNO055
import micropython
# Create all objects
DC_Motor_1 = motor.MotorDriver(5, pyb.Pin.board.PC1, pyb.Pin.board.PA0,
pyb.Pin.board.PA1)
IMU = BNO055.bno055(pyb.I2C(1, pyb.I2C.MASTER, baudrate=100000),
micropython.const(0x28))
Controller_1 = controller.Controller(1.35, 0.01) ####
# Setup/Initialization
def setup():
''' This initializes the encoder and motor to zero values. '''
IMU.zero_Euler_vals() #
DC_Motor_1.set_duty_cycle(0)
Controller_1.set_setpoint(10)
# Continuous loop
def loop():
# utime.sleep_ms(200)
# x = imu.get_sensor_reading(bn.EULER_H)
# x = x/16
# y = imu.get_sensor_reading(bn.EULER_R)
# y = y/16
# z = imu.get_sensor_reading(bn.EULER_P)
# z = z/16
# print("Heading : " + str(x) + " Roll Val: " + str(y) + " Pitch Val: " + str(z))
cL = loop.Closed_Loop()
cL.set_setpoint(0)
while (1):
#print("Enter a gain: ")
gain_set = input()
cL.set_cont_gain(float(gain_set))
mot1 = motor.MotorDriver()
last_time = utime.ticks_ms()
times = []
positions = []
while (1):
utime.sleep_ms(10)
#times.append(utime.ticks_ms())
pos = imu.get_sensor_reading(bn.EULER_H)
pos = (pos / 16)
if (pos > 180):
pos = -1 * (360 - pos)
print(pos)
#positions.append(pos)
actuation = cL.control(pos)
mot1.set_duty_cycle(actuation)
# utime.sleep_ms(200)
# x = imu.get_sensor_reading(bn.EULER_H)
# x = x/16
# y = imu.get_sensor_reading(bn.EULER_R)
# y = y/16
# z = imu.get_sensor_reading(bn.EULER_P)
# z = z/16
# print("Heading : " + str(x) + " Roll Val: " + str(y) + " Pitch Val: " + str(z))
cL = loop.Closed_Loop()
cL.set_setpoint(0)
while (1):
#print("Enter a gain: ")
gain_set = input()
cL.set_cont_gain(float(gain_set))
mot1 = motor.MotorDriver("A")
last_time = utime.ticks_ms()
times = []
positions = []
while (1):
utime.sleep_ms(100)
#times.append(utime.ticks_ms())
#if(pos > 180):
#pos = -1*(360 - pos)
x = imu.get_sensor_reading(bn.EULER_H)
x = x / 16
y = imu.get_sensor_reading(bn.EULER_R)
y = y / 16
z = imu.get_sensor_reading(bn.EULER_P)
z = z / 16
def motor1_fun():
# A motor object
pinENA = pyb.Pin(pyb.Pin.board.PA10, pyb.Pin.OUT_PP)
pinIN1A = pyb.Pin(pyb.Pin.board.PB4, pyb.Pin.OUT_PP)
pinIN2A = pyb.Pin(pyb.Pin.board.PB5, pyb.Pin.OUT_PP)
mot = motor.MotorDriver([pinIN1A, pinIN2A, pinENA], 3, [1, 2])
# An encoder object
enc = encoder.Encoder([pyb.Pin.board.PB6, pyb.Pin.board.PB7], 4, [1, 2])
# A controller object
cont = controller.Controller(K_p=0.10)
setup = True
state = 0
while (True):
# Stopped
if state == 0:
# If key is pressed, set state to 1
# if vcp.read() == b'\x31':
if pos_ctrl1.get():
state = 1
elif step_rsp1.get():
state = 2
elif get_data1.get():
state = 3
else:
mot.set_duty_cycle(0)
# Position Control
elif state == 1:
if pos_ctrl1.get():
cont.set_setpoint(setpoint1.get())
mot.set_duty_cycle(cont.run(enc.read()))
else:
state = 0
# Step Response
elif state == 2:
if setup:
cont.clear_data()
cont.set_setpoint(setpoint1.get())
enc.zero()
stop = utime.ticks_add(utime.ticks_ms(), 1000)
setup = False
elif utime.ticks_diff(stop, utime.ticks_ms()) > 0:
mot.set_duty_cycle(cont.run(enc.read()))
else:
step_rsp1.put(False)
setup = True
state = 0
# Get Data
elif state == 3:
for datum in cont.get_data():
print(str(datum[0]) + ', ' + str(datum[1]))
get_data1.put(False)
state = 0
yield (state)
def task_motor():
''' Function which runs for Task 1, and controls how the motors will rotate. '''
control = controller.Controller(20, 50, .1, BALANCE_SET_POINT)
motor1 = motor.MotorDriver(pyb.Pin.board.PB4, pyb.Pin.board.PB5,
pyb.Pin.board.PA10, pyb.Timer(3))
encoder1 = encoder.Encoder(pyb.Pin.board.PB7, pyb.Pin.board.PB6,
pyb.Timer(4))
motor2 = motor.MotorDriver(pyb.Pin.board.PA1, pyb.Pin.board.PA0,
pyb.Pin.board.PC1, pyb.Timer(2))
encoder2 = encoder.Encoder(pyb.Pin.board.PC7, pyb.Pin.board.PC6,
pyb.Timer(8))
aye = pyb.I2C(1, pyb.I2C.MASTER)
accelo = acc.Acc(aye, 107)
filter0 = filter1.Filter(accelo)
state = BALANCE
while True:
x_theta = (filter0.updated_x() + THETA_OFFSET)
pwm = control.calculate(x_theta)
clear()
if state == BALANCE:
'''Include what will make this robot balance, accelo info and filters, motor duty cycle'''
control.set_setpoint(BALANCE_SET_POINT)
motor1.set_duty_cycle(pwm)
motor2.set_duty_cycle(pwm)
if (BACK_SENSOR == 1 or BLUETOOTH == "Forward"):
''' go forward'''
state = FORWARD
elif (FRONT_SENSOR == 1 or BLUETOOTH == "Backward"):
''' go backwards'''
state = BACKWARD
elif (RIGHT_SENSOR == 1 or BLUETOOTH == "TurnLeft"):
''' turn left'''
state = TURNLEFT
elif (LEFT_SENSOR == 1 or BLUETOOTH == "TurnRight"):
''' Turn right'''
state = TURNRIGHT
elif state == FORWARD:
''' Include what will make this robot move forward'''
control.set_setpoint(FRONT_SET_POINT)
motor1.set_duty_cycle(pwm)
motor2.set_duty_cycle(pwm)
if (FRONT_SENSOR == 1 or BLUETOOTH == "Backward"):
''' go backwards'''
state = BACKWARD
elif (RIGHT_SENSOR == 1 or BLUETOOTH == "TurnLeft"):
''' turn left'''
state = TURNLEFT
elif (LEFT_SENSOR == 1 or BLUETOOTH == "TurnRight"):
''' Turn right'''
state = TURNRIGHT
elif (BACK_SENSOR == 0):
state = BALANCE
elif state == BACKWARD:
''' Include what will make this robot move backwards'''
control.set_setpoint(FRONT_SET_POINT)
motor1.set_duty_cycle(pwm)
motor2.set_duty_cycle(pwm)
if (BACK_SENSOR == 1 or BLUETOOTH == "Forward"):
''' go forward'''
state = FORWARD
elif (RIGHT_SENSOR == 1 or BLUETOOTH == "TurnLeft"):
''' turn left'''
state = TURNLEFT
elif (LEFT_SENSOR == 1 or BLUETOOTH == "TurnRight"):
''' Turn right'''
state = TURNRIGHT
elif (FRONT_SENSOR == 0):
state = BALANCE
elif state == TURNLEFT:
''' Include what will make this robot turn left'''
control.set_setpoint(FRONT_SET_POINT)
motor1.set_duty_cycle(pwm)
motor2.set_duty_cycle(pwm)
if (BACK_SENSOR == 1 or BLUETOOTH == "Forward"):
''' go forward'''
state = FORWARD
elif (FRONT_SENSOR == 1 or BLUETOOTH == "Backward"):
''' go backwards'''
state = BACKWARD
elif (LEFT_SENSOR == 1 or BLUETOOTH == "TurnRight"):
''' Turn right'''
state = TURNRIGHT
elif (RIGHT_SENSOR == 0):
state = BALANCE
elif state == TURNRIGHT:
''' Include what will make this robot turn right'''
control.set_setpoint(FRONT_SET_POINT)
motor1.set_duty_cycle(pwm)
motor2.set_duty_cycle(pwm)
if (BACK_SENSOR == 1 or BLUETOOTH == "Forward"):
''' go forward'''
state = FORWARD
elif (FRONT_SENSOR == 1 or BLUETOOTH == "Backward"):
''' go backwards'''
state = BACKWARD
elif (RIGHT_SENSOR == 1 or BLUETOOTH == "TurnLeft"):
''' turn left'''
state = TURNLEFT
elif (LEFT_SENSOR == 0):
state = BALANCE
yield (state)
@author: Jason Grillo, Thomas Goehring, Trent Peterson
"""
#Yellow Encoder Cable: B7
#Gray Encoder Cable: B6
import pyb
import encoder
import motor
import controller
import utime
import BNO055
import micropython
# Create all objects
tilt_Motor = motor.MotorDriver(5, pyb.Pin.board.PC1, pyb.Pin.board.PA0,
pyb.Pin.board.PA1)
Encoder_1 = encoder.Encoder(4, pyb.Pin.board.PB6, pyb.Pin.board.PB7)
#IMU = BNO055.bno055 (pyb.I2C (1, pyb.I2C.MASTER, baudrate = 100000), micropython.const(0x28))
Controller_1 = controller.Controller(.02, 0, 25000)
DC_Motor_1 = motor.MotorDriver(3, pyb.Pin.board.PA10, pyb.Pin.board.PB4,
pyb.Pin.board.PB5)
tilt_Motor.set_duty_cycle(0)
# Setup/Initialization
def setup():
''' This initializes the encoder and motor to zero values. '''
Encoder_1.zero_encoder()
DC_Motor_1.set_duty_cycle(0)
Controller_1.set_setpoint(200)
def on_get(self, req, resp):
driver = motor.MotorDriver()
driver.setLeftSpeed(int(req.params["LEFT"]))
driver.setRightSpeed(int(req.params["RIGHT"]))
Created on Thu Jan 25 19:31:26 2018
@author: Jason Grillo, Thomas Goehring, Trent Peterson
"""
#Yellow Encoder Cable: B7
#Gray Encoder Cable: B6
import pyb
import encoder
import motor
import controller
import utime
# Create all objects
DC_Motor_1 = motor.MotorDriver(3,pyb.Pin.board.PA10, pyb.Pin.board.PB4, pyb.Pin.board.PB5 )
Encoder_1 = encoder.Encoder(4,pyb.Pin.board.PB6,pyb.Pin.board.PB7)
Controller_1 = controller.Controller(.019,1)
# Setup/Initialization
def setup():
''' This initializes the encoder and motor to zero values. '''
Encoder_1.zero_encoder()
DC_Motor_1.set_duty_cycle(0)
Controller_1.set_setpoint(3000)
# Continuous loop
def loop():
''' This is the main loop that handles the DC Motor step response
functionality. '''
Created on Thu Jan 25 19:31:26 2018
@author: Jason Grillo, Thomas Goehring, Trent Peterson
"""
#Yellow Encoder Cable: B7
#Gray Encoder Cable: B6
import pyb
import encoder
import motor
import controller
import utime
# Create all objects
DC_Motor_1 = motor.MotorDriver()
Encoder_1 = encoder.Encoder(4,pyb.Pin.board.PB6,pyb.Pin.board.PB7)
Controller_1 = controller.Controller(.01,40000)
# Setup/Initialization
def setup():
''' This initializes the encoder and motor to zero values. '''
Encoder_1.zero_encoder()
DC_Motor_1.set_duty_cycle(0)
# Continuous loop
def loop():
''' This is the main loop that handles the DC Motor step response
functionality. '''
# Wait for User Input
#
# @copyright License Info
#
# @date January 31, 2019
import controller
import encoder
import motor
import pyb
import utime
## A motor object
pinENA = pyb.Pin(pyb.Pin.board.PA10, pyb.Pin.OUT_PP)
pinIN1A = pyb.Pin(pyb.Pin.board.PB4, pyb.Pin.OUT_PP)
pinIN2A = pyb.Pin(pyb.Pin.board.PB5, pyb.Pin.OUT_PP)
motor = motor.MotorDriver([pinIN1A, pinIN2A, pinENA], 3, [1, 2])
## An encoder object
encoder = encoder.Encoder([pyb.Pin.board.PB6, pyb.Pin.board.PB7], 4, [1, 2])
## A controller object
controller = controller.Controller(K_p=0.10)
# # Positioner
# while(True):
# motor.set_duty_cycle(controller.run(encoder.read()))
# utime.sleep_ms(10)
# Step repsonse
while (True):
controller.clear_data()
input('Press "enter" to run a step response test!')
controller.set_setpoint(4000)
