Remove these files - they're not part of this test
This commit is contained in:
Danny Staple
parent
79e75b47b8
commit
53ceb98193
@ -1,39 +0,0 @@
|
||||
class PID:
|
||||
def __init__(self, proportional_k, integral_k, differential_k, set_point):
|
||||
self.proportional_k = proportional_k
|
||||
self.integral_k = integral_k
|
||||
self.differential_k = differential_k
|
||||
self.set_point = set_point
|
||||
|
||||
self.error_sum = 0
|
||||
self.last_value = 0
|
||||
self.min_output = -1
|
||||
self.max_output = 1
|
||||
|
||||
self.dead_zone = 0.3
|
||||
|
||||
def update(self, measurement, time_delta):
|
||||
error_value = measurement - self.set_point
|
||||
proportional = error_value * self.proportional_k
|
||||
|
||||
# calculate integral
|
||||
self.error_sum += error_value * time_delta
|
||||
# clamp it
|
||||
self.error_sum = min(self.max_output, self.error_sum)
|
||||
self.error_sum = max(self.min_output, self.error_sum)
|
||||
|
||||
integral = self.error_sum * self.integral_k
|
||||
|
||||
differentiated_error = (error_value - self.last_value) / time_delta
|
||||
differential = differentiated_error * self.differential_k
|
||||
self.last_value = error_value
|
||||
|
||||
output = proportional + integral + differential
|
||||
# clamp output
|
||||
if abs(output) < self.dead_zone:
|
||||
output = 0
|
||||
else:
|
||||
output = min(self.max_output, output)
|
||||
output = max(self.min_output, output)
|
||||
|
||||
return output
|
||||
@ -1,84 +0,0 @@
|
||||
import rp2pio
|
||||
import adafruit_pioasm
|
||||
import array
|
||||
|
||||
program = """
|
||||
; use the osr for count
|
||||
; input pins c1 c2
|
||||
|
||||
set y, 0 ; clear y
|
||||
mov osr, y ; and clear osr
|
||||
read:
|
||||
; x will be the old value
|
||||
; y the new values
|
||||
mov x, y ; store old Y in x
|
||||
in null, 32 ; Clear ISR - using y
|
||||
in pins, 2 ; read two pins into y
|
||||
mov y, isr
|
||||
jmp x!=y, different ; Jump if its different
|
||||
jmp read ; otherwise loop back to read
|
||||
|
||||
different:
|
||||
; x has old value, y has new.
|
||||
; extract the upper bit of X.
|
||||
in x, 31 ; get bit 31 - old p1 (remember which direction it came in)
|
||||
in null, 31 ; keep only 1 bit
|
||||
mov x, isr ; put this back in x
|
||||
jmp !x, c1_old_zero
|
||||
|
||||
c1_old_not_zero:
|
||||
jmp pin, count_up
|
||||
jmp count_down
|
||||
|
||||
c1_old_zero:
|
||||
jmp pin, count_down
|
||||
; fall through
|
||||
count_up:
|
||||
; for a clockwise move - we'll add 1 by inverting
|
||||
mov x, ~ osr ; store inverted OSR on x
|
||||
jmp x--, fake ; use jump to take off 1
|
||||
fake:
|
||||
mov x, ~ x ; invert back
|
||||
jmp send
|
||||
count_down:
|
||||
; for a clockwise move, just take one off
|
||||
mov x, osr ; store osr in x
|
||||
jmp x--, send ; dec and send
|
||||
send:
|
||||
; send x.
|
||||
mov isr, x ; send it
|
||||
push noblock ; put ISR into input FIFO
|
||||
mov osr, x ; put X back in OSR
|
||||
jmp read ; loop back
|
||||
"""
|
||||
|
||||
assembled = adafruit_pioasm.assemble(program)
|
||||
|
||||
|
||||
class QuadratureEncoder:
|
||||
def __init__(self, first_pin, second_pin, reversed=False):
|
||||
"""Encoder with 2 pins. Must use sequential pins on the board"""
|
||||
self.sm = rp2pio.StateMachine(
|
||||
assembled,
|
||||
frequency=0,
|
||||
first_in_pin=first_pin,
|
||||
jmp_pin=second_pin,
|
||||
in_pin_count=2,
|
||||
)
|
||||
self.reversed = reversed
|
||||
self._buffer = array.array("i", [0])
|
||||
self.previous_reading = 0
|
||||
|
||||
def read(self):
|
||||
while self.sm.in_waiting:
|
||||
self.sm.readinto(self._buffer)
|
||||
if self.reversed:
|
||||
return -self._buffer[0]
|
||||
else:
|
||||
return self._buffer[0]
|
||||
|
||||
def get_speed(self, delta_time):
|
||||
new_read = self.read()
|
||||
distance = new_read - self.previous_reading
|
||||
self.previous_reading = new_read
|
||||
return distance / delta_time
|
||||
@ -1,54 +0,0 @@
|
||||
import board
|
||||
import pwmio
|
||||
import pio_encoder
|
||||
import busio
|
||||
import adafruit_vl53l1x
|
||||
import adafruit_bno055
|
||||
|
||||
|
||||
motor_A1 = pwmio.PWMOut(board.GP17)
|
||||
motor_A2 = pwmio.PWMOut(board.GP16)
|
||||
motor_B1 = pwmio.PWMOut(board.GP18)
|
||||
motor_B2 = pwmio.PWMOut(board.GP19)
|
||||
|
||||
right_motor = motor_A1, motor_A2
|
||||
left_motor = motor_B1, motor_B2
|
||||
|
||||
right_encoder = pio_encoder.QuadratureEncoder(board.GP20, board.GP21, reversed=True)
|
||||
left_encoder = pio_encoder.QuadratureEncoder(board.GP26, board.GP27)
|
||||
|
||||
i2c0 = busio.I2C(sda=board.GP0, scl=board.GP1)
|
||||
# i2c1 = busio.I2C(sda=board.GP2, scl=board.GP3)
|
||||
|
||||
# right_distance = adafruit_vl53l1x.VL53L1X(i2c0)
|
||||
# left_distance = adafruit_vl53l1x.VL53L1X(i2c1)
|
||||
|
||||
imu = adafruit_bno055.BNO055_I2C(i2c0)
|
||||
|
||||
def stop():
|
||||
motor_A1.duty_cycle = 0
|
||||
motor_A2.duty_cycle = 0
|
||||
motor_B1.duty_cycle = 0
|
||||
motor_B2.duty_cycle = 0
|
||||
|
||||
|
||||
def set_speed(motor, speed):
|
||||
# Swap motor pins if we reverse the speed
|
||||
if speed < 0:
|
||||
direction = motor[1], motor[0]
|
||||
speed = -speed
|
||||
else:
|
||||
direction = motor
|
||||
speed = min(speed, 1) # limit to 1.0
|
||||
max_speed = 2 ** 16 - 1
|
||||
|
||||
direction[0].duty_cycle = int(max_speed * speed)
|
||||
direction[1].duty_cycle = 0
|
||||
|
||||
|
||||
def set_left(speed):
|
||||
set_speed(left_motor, speed)
|
||||
|
||||
|
||||
def set_right(speed):
|
||||
set_speed(right_motor, speed)
|
||||
Loading…
x
Reference in New Issue
Block a user