f57add5106
Including: - replacing guassian with simpler triangle distribution - Timing calculations -resulting in using only 50 points. - Changes (which may need to be reconsidered) for optimization. These are likely to be simplified back.
76 lines
2.1 KiB
Python
76 lines
2.1 KiB
Python
import asyncio
|
|
import json
|
|
import time
|
|
|
|
import pid_controller
|
|
import robot
|
|
|
|
|
|
class RpmSpeedController:
|
|
def __init__(self, encoder, motor_fn):
|
|
self.encoder = encoder
|
|
self.motor_fn = motor_fn
|
|
self.pid = pid_controller.PIDController(3, 0, 1)
|
|
self.speed = 0
|
|
self.reset()
|
|
|
|
def reset(self):
|
|
self.last_ticks = self.encoder.read()
|
|
self.pwm = 0
|
|
self.pid.reset()
|
|
|
|
def update(self, dt):
|
|
current_ticks = self.encoder.read()
|
|
speed_in_ticks = (current_ticks - self.last_ticks) / dt
|
|
self.last_ticks = current_ticks
|
|
# calculate the error
|
|
error = self.speed - speed_in_ticks
|
|
# calculate the control signal
|
|
control_signal = self.pid.calculate(error, dt)
|
|
self.pwm += control_signal
|
|
self.motor_fn(self.pwm)
|
|
|
|
|
|
class MotionControl:
|
|
def __init__(self):
|
|
self.left_speed_controller = RpmSpeedController(robot.left_encoder, robot.set_left)
|
|
self.right_speed_controller = RpmSpeedController(robot.right_encoder, robot.set_right)
|
|
|
|
|
|
def plan_motion(self, turn1, distance, turn2):
|
|
"""Plan a motion - based on making a turn,
|
|
moving forward a distance,
|
|
and then making another turn."""
|
|
|
|
pass
|
|
|
|
def uodate(self, dt):
|
|
self.left_speed_controller.update(dt)
|
|
self.right_speed_controller.update(dt)
|
|
|
|
|
|
async def main_loop(dt):
|
|
motion_control = MotionControl()
|
|
while True:
|
|
await asyncio.sleep(dt)
|
|
current_time = time.monotonic()
|
|
dt = current_time - last_time
|
|
last_time = current_time
|
|
motion_control.uodate(dt)
|
|
|
|
|
|
async def command_handler():
|
|
main_loop = None
|
|
motion_control = MotionControl()
|
|
robot = robot.Robot()
|
|
while True:
|
|
command = json.loads(input())
|
|
if command["command"] == "start":
|
|
main_loop = asyncio.create_task(main_loop(0.2))
|
|
elif command["command"] == "get_speed":
|
|
print(json.dumps(robot.get_speed()))
|
|
else:
|
|
print("Unknown command")
|
|
|
|
asyncio.run(command_handler())
|