main.py File Reference

This file combines the Encoder, Motor, and TouchPanel drivers we created for previous labs with the kinematic and dynamic analysis of the platform from previous labs to balance a ball on the platform. More...

Functions
def	main.nFaultISR (self, pin)

Variables
	main.TPD = touchy(pyb.Pin.board.PA7, pyb.Pin.board.PA1, pyb.Pin.board.PA6, pyb.Pin.board.PA0, 0.959, 3.937, 3.480, 1.979)
	Initialize touch Panel and corresponding pins.
int	main.x = 0
	x position of ball on platform
int	main.x_dot = 0
	x velocity of ball on platform
int	main.y = 0
	y position of ball on platform
int	main.y_dot = 0
	y velocity of ball on platfrom
	main.Encoder1APin = pyb.Pin(pyb.Pin.cpu.B6, pyb.Pin.IN)
	Encoder 1 pin.
	main.Encoder1BPin = pyb.Pin(pyb.Pin.cpu.B7, pyb.Pin.IN)
	Encoder 1 pin.
	main.Encoder2APin = pyb.Pin(pyb.Pin.cpu.C6, pyb.Pin.IN)
	Encoder 2 pin.
	main.Encoder2BPin = pyb.Pin(pyb.Pin.cpu.C7, pyb.Pin.IN)
	Encoder 2 pin.
int	main.TimerNum1 = 4
	Encoder 1 timer number.
int	main.TimerNum2 = 8
	Encoder 2 timer number.
int	main.Period = 65535
	Timer period that both encoders will use.
int	main.Prescaler = 0
	Timer prescaler that both encoders will use.
	main.Encoder1 = EncoderDriver(Encoder1APin, Encoder1BPin, TimerNum1, Prescaler, Period)
	Encoder 1 object.
	main.Encoder2 = EncoderDriver(Encoder2APin, Encoder2BPin, TimerNum2, Prescaler, Period)
	Encoder 2 object.
int	main.theta_x = 0
	Angular position about x axis.
int	main.theta_dot_x = 0
	Angular velocity about x axis.
int	main.theta_y = 0
	Angular position about y axis.
int	main.theta_dot_y = 0
	Angular velocity about y axis.
	main.pin_nSLEEP = pyb.Pin(pyb.Pin.cpu.A15, pyb.Pin.OUT_PP)
	CPU pin which is connected to DRV8847 nSLEEP pin.
	main.pin_IN1 = pyb.Pin.board.PB4;
	CPU pin which is connected to DRV8847 IN1 pin.
	main.pin_IN2 = pyb.Pin.board.PB5;
	CPU pin which is connected to DRV8847 IN2 pin.
	main.pin_IN3 = pyb.Pin.board.PB0;
	CPU pin which is connected to DRV8847 IN3 pin.
	main.pin_IN4 = pyb.Pin.board.PB1;
	CPU pin which is connected to DRV8847 IN4 pin.
	main.time = pyb.Timer(3,freq = 20000);
	Timer object used for PWM generation.
	main.Motor1 = MotorDriver(pin_nSLEEP, pin_IN1, pin_IN2, time, 1, 2)
	Motor 1 object.
	main.Motor2 = MotorDriver(pin_nSLEEP, pin_IN3, pin_IN4, time, 3, 4)
	Motor 2 object.
	main.nFaultInt = pyb.ExtInt(pyb.Pin.cpu.B2, pyb.ExtInt.IRQ_FALLING, pyb.Pin.PULL_UP, nFaultISR)
	External interrupt on nFault pin.
	main.FaultInt
float	main.K1 = -3.0
	Gain for velocity.
int	main.K2 = -.075
	Gain for angular velocity.
int	main.K3 = -30
	Gain for position.
float	main.K4 = -4.2
	Gain for angular position.
float	main.K_duty = 13.3454
	Conversion factor from motor torque to duty cycle (K_duty = R/(Vdc * Kt) = 2.21 ohms /(12V * 12.8 mN/A))
int	main.GearRatio = 4
	Gear ratio between motor and encoder (12 teeth on motor gear and 48 teeth on encoder gear; 48/12 = 4)
int	main.old_time1 = 0
	Placeholder for last time1.
int	main.old_time2 = 0
	Placeholder for last time2.
int	main.new_time1 = utime.ticks_us() / 1000000
	Most recent time1.
int	main.period1 = new_time1 - old_time1
	Time since last call that is used as a period for velocity calucations.
int	main.y_new = TPD.y_read() / 1000
	Y-axis reading in meters.
	main.theta_x_new = Encoder1.get_position() * (math.pi / 1000)
	Encoder 1 angular position in radians.
tuple	main.Torque_A = ((-K1 * y_dot) - (K2 * theta_dot_x) - (K3 * y) - (K4 * -theta_x))
	Torque value for motor 1.
	main.Duty_Cycle_A = int(K_duty * Torque_A * GearRatio)
	Duty cycle for motor 1.
	main.nFault
int	main.new_time2 = utime.ticks_us() / 1000000
	Most recent time2.
int	main.period2 = new_time2 - old_time2
	Time since last call that is used as a period for velocity calucations.
int	main.x_new = TPD.x_read() / 1000
	X-axis reading in meters.
	main.theta_y_new = Encoder2.get_position() * (math.pi / 1000)
	Encoder 2 angular position in radians.
tuple	main.Torque_B = ((-K1 * x_dot) - (K2 * theta_dot_y) - (K3 * x) - (K4 * -theta_y))
	Torque value for motor 2.
	main.Duty_Cycle_B = int(K_duty * Torque_B * GearRatio)
	Duty cycle for motor 2.

Detailed Description

This file combines the Encoder, Motor, and TouchPanel drivers we created for previous labs with the kinematic and dynamic analysis of the platform from previous labs to balance a ball on the platform.

First we initialize the touchpad with device specific values. Then, we initialzie the two encoders and two motors. We also create an interrupt and interrupt service routine to prevent any mechanical faults from causing damage. Lastly, we continuously run the two motor operations, which are similar. The motor operations calculate a period, reads from the touch panel to find position and velocity, and reads from the encoder to find angular position and velocity. It then applies the gains to the values found to find the motor torque. Lastly, it converts the motor torque to a duty cycle using the gear ratio and the motor torque conversion factor, which is then set as the motor's duty cycle.

Author

Ryan McMullen and David Meyenberg

Date

March 17, 2021