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)