adds first PoC wise PiD controller, which should probably hold the RPM
This commit is contained in:
Robert Schauklies
parent
38ea428824
commit
e87293a6e2
3 changed files with 352 additions and 79 deletions
168
src/main.py
168
src/main.py
|
|
@ -2,6 +2,7 @@
|
|||
# MIT License; Copyright (c) 2017 Jeffrey N. Magee
|
||||
from ili934xnew import ILI9341, color565
|
||||
from machine import Pin, SPI,Timer, UART,PWM
|
||||
from machine import Counter
|
||||
from ST7735 import TFT,TFTColor
|
||||
import m5stack
|
||||
import tt14
|
||||
|
|
@ -16,6 +17,9 @@ from rotary_irq_esp import RotaryIRQ
|
|||
fonts = [glcdfont,tt14,tt24,tt32]
|
||||
from sysfont import sysfont
|
||||
import json
|
||||
from pid import PID
|
||||
|
||||
COILS = 14
|
||||
# from enum import Enum
|
||||
|
||||
# class EditMenu(Enum):
|
||||
|
|
@ -210,41 +214,56 @@ async def update_motor():
|
|||
global state
|
||||
#we want to use 18 for the SPI-Bus!
|
||||
#https://docs.micropython.org/en/latest/esp32/quickref.html#hardware-spi-bus
|
||||
dshot = Dshot(pin=Pin(25))
|
||||
rpm_pid = PID(
|
||||
Kp=config["PID"]["Kp"],
|
||||
Ki=config["PID"]["Ki"],
|
||||
Kd=config["PID"]["Kd"],
|
||||
setpoint=0,
|
||||
sample_time=None,
|
||||
output_limits=(0.0, 1.0),
|
||||
# proportional_on_measurement=True,
|
||||
)
|
||||
while True:
|
||||
rpm_pid.setpoint = state["target_rpm"]
|
||||
# dshot = Dshot(pin=Pin(25))
|
||||
# rpm_pid = PID(
|
||||
# Kp=config["PID"]["Kp"],
|
||||
# Ki=config["PID"]["Ki"],
|
||||
# Kd=config["PID"]["Kd"],
|
||||
# setpoint=0,
|
||||
# sample_time=None,
|
||||
# output_limits=(0.0, 1.0),
|
||||
# # proportional_on_measurement=True,
|
||||
# )
|
||||
# while True:
|
||||
# rpm_pid.setpoint = state["target_rpm"]
|
||||
|
||||
# read ESC telemetry
|
||||
if uart.any() >= 10:
|
||||
telemetry = decode_ESC_telemetry(uart.read())
|
||||
if telemetry is not None:
|
||||
state["rpm"] = telemetry[5]
|
||||
throttle = rpm_pid(state["rpm"])
|
||||
print(
|
||||
"Throttle:",
|
||||
throttle,
|
||||
"pid components:",
|
||||
rpm_pid.components,
|
||||
"RPM:",
|
||||
state["rpm"],
|
||||
)
|
||||
# # read ESC telemetry
|
||||
# if uart.any() >= 10:
|
||||
# telemetry = decode_ESC_telemetry(uart.read())
|
||||
# if telemetry is not None:
|
||||
# state["rpm"] = telemetry[5]
|
||||
# throttle = rpm_pid(state["rpm"])
|
||||
# print(
|
||||
# "Throttle:",
|
||||
# throttle,
|
||||
# "pid components:",
|
||||
# rpm_pid.components,
|
||||
# "RPM:",
|
||||
# state["rpm"],
|
||||
# )
|
||||
|
||||
if state["target_rpm"] == 0 and state["rpm"] < 1000:
|
||||
throttle = 0
|
||||
rpm_pid.reset()
|
||||
dshot.set_throttle(throttle)
|
||||
# if state["target_rpm"] == 0 and state["rpm"] < 1000:
|
||||
# throttle = 0
|
||||
# rpm_pid.reset()
|
||||
# dshot.set_throttle(throttle)
|
||||
|
||||
# await uasyncio.sleep_ms(1)
|
||||
old_value=0
|
||||
current_rpm=0
|
||||
def timer_callback(f):
|
||||
global old_value
|
||||
global current_rpm
|
||||
val = counter.value()
|
||||
current_rpm = val-old_value
|
||||
old_value = val
|
||||
|
||||
# print(current_rpm)
|
||||
def kill_me(f):
|
||||
state["target_rpm"] = 0
|
||||
|
||||
await uasyncio.sleep_ms(1)
|
||||
def update_motor_pwm():
|
||||
global timer3
|
||||
global current_rpm
|
||||
pwm = PWM(Pin(26))
|
||||
rpm_pid = PID(
|
||||
Kp=config["PID"]["Kp"],
|
||||
|
|
@ -256,24 +275,19 @@ def update_motor_pwm():
|
|||
# proportional_on_measurement=True,
|
||||
)
|
||||
pwm.freq(50)
|
||||
# pwm.duty_u16(1638)
|
||||
# pwm.duty_u16(8192)
|
||||
# set_throttle(pwm,1055)
|
||||
|
||||
timer3.init(mode=Timer.PERIODIC, period=100, callback=timer_callback)
|
||||
pwm.duty_ns(1055*10**3)
|
||||
time.sleep(1)
|
||||
pwm.duty_ns(1070*10**3)
|
||||
|
||||
time.sleep(10)
|
||||
global rpm_counter
|
||||
rpm_counter= 0
|
||||
pwm.duty_ns(1200*10**3)
|
||||
time.sleep(10)
|
||||
global rpm_counter
|
||||
rpm_counter= 0
|
||||
pwm.duty_ns(1400*10**3)
|
||||
time.sleep(60)
|
||||
print((current_rpm*60)/COILS)
|
||||
pwm.duty_ns(1055*10**3)
|
||||
|
||||
#pwm.duty_ns(1200*10**3)
|
||||
#global rpm_counter
|
||||
#rpm_counter= 0
|
||||
#time.sleep(10)
|
||||
|
||||
# time.sleep(10) # keep low signal to arm
|
||||
# # for i in range(1060,1800):
|
||||
# # pwm.duty_u16(i*3)
|
||||
|
|
@ -281,16 +295,21 @@ def update_motor_pwm():
|
|||
# pwm.duty_ns(1070*10**3)
|
||||
# pwm.duty_u16(1700)
|
||||
# time.sleep(200)
|
||||
while True:
|
||||
pwm.duty_ns(1060*10**3)
|
||||
state["target_rpm"] = 5000
|
||||
timer2.init(mode=Timer.PERIODIC, period=30000, callback=kill_me)
|
||||
|
||||
while(True):
|
||||
|
||||
print(f'target RPM:{state["target_rpm"]}')
|
||||
rpm_pid.setpoint = state["target_rpm"]
|
||||
|
||||
# read ESC telemetry
|
||||
if uart.any() >= 10:
|
||||
telemetry = decode_ESC_telemetry(uart.read())
|
||||
if telemetry is not None:
|
||||
state["rpm"] = telemetry[5]
|
||||
throttle = rpm_pid(state["rpm"])
|
||||
print(
|
||||
telemetry = (current_rpm*600)/COILS
|
||||
if telemetry is not None:
|
||||
state["rpm"] = telemetry
|
||||
throttle = rpm_pid(state["rpm"])
|
||||
print(
|
||||
"Throttle:",
|
||||
throttle,
|
||||
"pid components:",
|
||||
|
|
@ -299,9 +318,21 @@ def update_motor_pwm():
|
|||
state["rpm"],
|
||||
)
|
||||
|
||||
if state["target_rpm"] == 0 and state["rpm"] < 1000:
|
||||
if state["target_rpm"] == 0:
|
||||
print("Killing Coater!")
|
||||
throttle = 0
|
||||
rpm_pid.reset()
|
||||
pwm.duty_ns(1055*10**3)
|
||||
return
|
||||
print(throttle)
|
||||
new_duty = 1060+(int(660*throttle))
|
||||
print(new_duty)
|
||||
pwm.duty_ns(new_duty*10**3)
|
||||
|
||||
pwm.duty_ns(1050*10**3)
|
||||
pwm.duty_ns(1055*10**3)
|
||||
|
||||
|
||||
def set_throttle(pwm,us):
|
||||
hertz = 50
|
||||
#we now have to calculate the duty_cycle!
|
||||
|
|
@ -320,10 +351,7 @@ def debounce_button(p):
|
|||
timer0 = Timer(0)
|
||||
timer0.init(period=20, mode=Timer.ONE_SHOT, callback=lambda t: on_button_press(p))
|
||||
|
||||
rpm_counter = 0
|
||||
def rpm_count(p):
|
||||
global rpm_counter
|
||||
rpm_counter+=1
|
||||
|
||||
|
||||
|
||||
def on_button_press(p):
|
||||
|
|
@ -419,12 +447,14 @@ def save_config():
|
|||
|
||||
|
||||
|
||||
text = 'Now is the time for all good men to come to the aid of the party.'
|
||||
|
||||
|
||||
power = Pin(m5stack.TFT_LED_PIN, Pin.OUT)
|
||||
power.value(1)
|
||||
timer1 = Timer(1)
|
||||
timer2 = Timer(2)
|
||||
counter = Counter(0,Pin(19,Pin.IN))
|
||||
timer3 = Timer(3)
|
||||
|
||||
# No need to change the software. It's just a matter of different names.. Use this translation:
|
||||
|
||||
|
|
@ -457,7 +487,6 @@ display = tft
|
|||
tft.initr()
|
||||
tft.rgb(True)
|
||||
#tftprinttest()
|
||||
uart = UART(1, baudrate=115200, rx=5) # to receive ESC telemetry
|
||||
rotary = RotaryIRQ(
|
||||
pin_num_clk=25,
|
||||
pin_num_dt=13,
|
||||
|
|
@ -469,8 +498,6 @@ rotary = RotaryIRQ(
|
|||
button = Pin(15, Pin.IN, Pin.PULL_UP)
|
||||
button.irq(trigger=Pin.IRQ_FALLING, handler=on_button_press)
|
||||
#interrupt for rpm sensor!
|
||||
rpm_pin = Pin(19, Pin.IN, Pin.PULL_UP)
|
||||
rpm_pin.irq(trigger=Pin.IRQ_FALLING, handler=rpm_count)
|
||||
|
||||
state = {
|
||||
"view": start_view,
|
||||
|
|
@ -492,27 +519,10 @@ tft.fill(TFT.BLACK)
|
|||
# display.set_font(ff)
|
||||
# display.print(text)
|
||||
update_motor_pwm()
|
||||
print(rpm_counter)
|
||||
# event_loop = uasyncio.get_event_loop()
|
||||
# event_loop.create_task(update_display())
|
||||
|
||||
# event_loop.create_task(update_motor_pwm())
|
||||
# #event_loop.create_task(update_motor_pwm())
|
||||
# event_loop.run_forever()
|
||||
# pwm = PWM(Pin(26))
|
||||
# rpm_pid = PID(
|
||||
# Kp=config["PID"]["Kp"],
|
||||
# Ki=config["PID"]["Ki"],
|
||||
# Kd=config["PID"]["Kd"],
|
||||
# setpoint=0,
|
||||
# sample_time=None,
|
||||
# output_limits=(0.0, 1.0),
|
||||
# # proportional_on_measurement=True,
|
||||
# )
|
||||
# pwm.freq(50)
|
||||
# # pwm.duty_u16(1638)
|
||||
# # pwm.duty_u16(8192)
|
||||
# # set_throttle(pwm,1055)
|
||||
# pwm.duty_ns(1055*10**3)
|
||||
# time.sleep(3)
|
||||
# pwm.duty_ns(1070*10**3)
|
||||
|
||||
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue