|
ME 405
|
|
|
ME 405
|
|
This script balances the ME 405 term project platform @detials The script operates in two modes. More...
Namespaces | |
| balance | |
Variables | |
| balance.pin_E1CH1 = pyb.Pin(pyb.Pin.cpu.B6) | |
| The pin object connected to Encoder 1's channel 1. More... | |
| balance.pin_E1CH2 = pyb.Pin(pyb.Pin.cpu.B7) | |
| The pin object connected to Encoder 1's channel 2. More... | |
| balance.e1 = EncDriver('E1', pin_E1CH1, 1, pin_E1CH2, 2, 4, 1000, False) | |
| An encoder object representing the encoder attached to Motor 1. More... | |
| balance.pin_E2CH1 = pyb.Pin(pyb.Pin.cpu.C6) | |
| The pin object connected to Encoder 2's channel 1. More... | |
| balance.pin_E2CH2 = pyb.Pin(pyb.Pin.cpu.C7) | |
| The pin object connected to Encoder 2's channel 2. More... | |
| balance.e2 = EncDriver('E2', pin_E2CH1, 1, pin_E2CH2, 2, 8, 1000, False) | |
| An encoder object representing the encoder attached to Motor 2. More... | |
| int | balance.enc_conv = -60/110 |
| Conversion factor for encoder angle to platform angle. More... | |
| balance.pin_nSLEEP = pyb.Pin(pyb.Pin.cpu.A15, pyb.Pin.OUT_PP) | |
| The pin object used for the H-bridge sleep pin. More... | |
| balance.pin_nFAULT = pyb.Pin(pyb.Pin.board.PB2, pyb.Pin.IN) | |
| The pin object used for the motor driver fault pin. More... | |
| balance.pin_IN1 = pyb.Pin(pyb.Pin.cpu.B4) | |
| The pin object used for the Motor 1's H-bridge input 1. More... | |
| balance.pin_IN2 = pyb.Pin(pyb.Pin.cpu.B5) | |
| The pin object used for the Motor 1's H-bridge input 2. More... | |
| balance.pin_IN3 = pyb.Pin(pyb.Pin.cpu.B0) | |
| The pin object used for the Motor 2's H-bridge input 1. More... | |
| balance.pin_IN4 = pyb.Pin(pyb.Pin.cpu.B1) | |
| The pin object used for the Motor 2's H-bridge input 2. More... | |
| balance.tim = pyb.Timer(3,freq=20000); | |
| The timer object used for PWM generation. More... | |
| balance.mA = MotorDriver(pin_nSLEEP, pin_nFAULT, pin_IN1, 1, pin_IN2, 2, tim, new_db=2.5, dead_band=(-45,45), debug=False, primary=True) | |
| A motor object passing in the pins and timer. More... | |
| balance.mB = MotorDriver(pin_nSLEEP, pin_nFAULT, pin_IN3, 3, pin_IN4, 4, tim, new_db=2.5, dead_band=(-45,45)) | |
| A second motor object passing in the pins and timer. More... | |
| balance.PIN_xm = Pin(Pin.cpu.A0) | |
| The Nucleo pin connected to RTP xm. More... | |
| balance.PIN_xp = Pin(Pin.cpu.A1) | |
| The Nucleo pin connected to RTP xp. More... | |
| balance.PIN_ym = Pin(Pin.cpu.A6) | |
| The Nucleo pin connected to RTP ym. More... | |
| balance.PIN_yp = Pin(Pin.cpu.A7) | |
| The Nucleo pin connected to RTP yp. More... | |
| balance.RTP = RTP(PIN_xm,PIN_xp,PIN_ym,PIN_yp,(176,100),(88,50),filt='fir',debug=False) | |
| The RTP objct. More... | |
| balance.i2c = I2C(-1, scl=Pin('PB8'), sda=Pin('PB9'), freq=115200) | |
| Initializes the I2C bus. More... | |
| balance.imu = BNO055(i2c) | |
| Initializes the bno055 driver using the I2C object. More... | |
| list | balance.Kplaty = [-14.4129, -0.00223331] |
| Y axis reduced system gains. More... | |
| list | balance.Kplatx = [-8.4129, -0.00223331] |
| X axis reduced system gains. More... | |
| list | balance.Ksysy = [-14.17333, -14.4129, -1.254464, -0.00223331] |
| Y axis full system gains. More... | |
| list | balance.Ksysx = [-14.17333, -10.4129, -2.54464, -0.00223331] |
| X axis full system gains. More... | |
| int | balance.Vdc = 18 |
| Motor nominal voltage [V]. More... | |
| float | balance.Kt = 13.8/10**3 |
| Motor torque constant [Nm/A]. More... | |
| float | balance.R = 2.21 |
| Motor terminal resistance [Ohms]. More... | |
| float | balance.conv = R/(Kt*Vdc) |
| Torque to PWM duty conversion factor. More... | |
| balance.plat_y = FSFB('KPY', Kplaty,100, conv=conv) | |
| FSFB controller for the y-axis of the platform only system. More... | |
| balance.plat_x = FSFB('KPX', Kplatx, 100, conv=conv) | |
| FSFB controller for the x-axis of the platform only system. More... | |
| balance.sys_y = FSFB('KY', Ksysy, 100, conv=conv) | |
| FSFB controller for the y-axis of the full system. More... | |
| balance.sys_x = FSFB('KX', Ksysx, 100, conv=conv) | |
| FSFB controller for the x-axis of the full system. More... | |
| int | balance.x_duty = 0 |
| PWM duty sent to motor A controlling platform x-axis. More... | |
| int | balance.y_duty = 0 |
| PWM duty sent to motor B controlling platform y-axis. More... | |
| int | balance.bal_lim = 20 |
| Hand balancing limits in degrees. More... | |
| int | balance.runs = 1 |
| Number of runs. More... | |
| balance.start_time = utime.ticks_us() | |
| Start time of the data collection. More... | |
| balance.last_time = start_time | |
| Last time of the data collection. More... | |
| balance.curr_time = start_time | |
| Current time of the data collection. More... | |
| balance.el_time = curr_time - start_time | |
| Elapsed time. More... | |
| int | balance.dt = 5*10**3 |
| Desired controller time step. More... | |
| balance.next_time = start_time | |
| Time of next run. More... | |
| int | balance.dt_meas = 1 |
| Measured controller time interval. More... | |
| int | balance.dt_avg = 1 |
| Average measued time interval. More... | |
| balance.uart = UART(2) | |
| UART communication line for debugging. More... | |
| tuple | balance.pos = (0,0,0) |
| Ball Position from the touch pannel (x,y,z) in [m]. More... | |
| tuple | balance.prev_pos = (0,0,0) |
| Previous ball Position from the touch pannel (x,y,z) in [m]. More... | |
| tuple | balance.vel = (0,0,0) |
| Ball Velocity from the touch pannel in [m/s]. More... | |
| tuple | balance.thta = (0,0,0) |
| Platform angle [rad]. More... | |
| tuple | balance.omga = (0,0,0) |
| Platform angular velocity [rad/s]. More... | |
| int | balance.steady_count = 0 |
| A counter that increments when the platform is stable. More... | |
| int | balance.contact_count = 0 |
| A counter that increments when the ball is off of the platform. More... | |
| balance.offset = curr_time | |
| Elapsed time offset, recorded on the first run. More... | |
This script balances the ME 405 term project platform @detials The script operates in two modes.
The first occurs when there is no contact detected on the touch pannel. In this state, the system is controlled using the gains derrived from the platform only model, and the orientation measurements from the BNO055 IMU. This allows for repeatable and automatic absolute leveling. When in this mode, the time that the platform is stable (no omega) is tracked. If there has been no motion for 50 controller cycles the encoders are zeroed.
The second mode begins when contact is detected on the touch pannel. The controller waits for 100 cycles with positive contact before changing the control system to incorperate the ball position and velosity. This was done to allow the ball position signal to stabalize before using it for control. The FIR filter and averaging scheme that I applied to reduce noise and accidental loss of contact also acts on the real contact changes. The easiest way to handle this was to add an artificial settling time before changing control state. In this mode, platform orientation data is read from the encoders. This allows for much faster controller speeds, and as a result, better performance.
The source code for this file can be found here:
https://bitbucket.org/ewnichol/me305-me405_labs/src/master/me405/me405%20Term%20Project/balance.py
1.8.20