I am using PyGame to simulate the motion of a differential drive robot. So far, I have used the standard diff drive kinematics, multiplying by delta time, and I have the motion working as expected.
What I want to do next is build a PID controller to control the robot in trajectory planning, but I feel that I should build a more realistic model (or rather, I would like to). I understand PID control well, but I am a little unsure about how to build the actual robot model and what I should control as I am new to mobile robotics.
From my understanding, on a real robot (diff drive) you send a control signal to each wheel setting it to a certain velocity. The velocity is read by the wheel encoders, position is estimated using that value in the kinematic equation, and the feedback loop (or whatever control style being used) checks the error on the position to the desired position and adjusts the control signal.
As I stated, in my simulation so far I just have the kinematic models to get the robot position, and a set_velocity function to set the velocity of each wheel. I am confused as to how I can incorporate the other aspects of the real-world examples into my simulation, ie setting the velocities and checking them with the encoders (I think I have an idea of how to simulate an encoder in general since I know how to convert ticks to velocity).
Right now, my velocities are simply a class variable of the robot, so I am having trouble understanding how I might do this...
import pygame
import math
class Robot:
def __init__(self, image_of_robot, starting_location, width, initial_velocity, max_velocity):
# Define the width between wheels
self.width = width
# Define the intial conditions of the robot and define states
self._x_position = starting_location[0]
self._y_position = starting_location[1]
self._direction_angle = starting_location[2]
self._left_wheel_velocity = initial_velocity
self._right_wheel_velocity = initial_velocity
# Define the limits on the velocity of the robot
self.max_velocity = max_velocity
# Create the robot image, the image to be edited, and a bounding rectangle for position
self.original_image = pygame.image.load(image_of_robot)
self.rotated_image = self.original_image
self.bounding_rectangle = self.rotated_image.get_rect(center=(self.x_position, self.y_position))
# Get the current robot image and bounding rectangle for blit
def get_image(self):
return (self.rotated_image, self.bounding_rectangle)
# Set the velocity of the wheels.
# Right_wheel = 1 -> right wheel velocity to be set
# Right_wheel = 0 -> left wheel velocity to be set
def set_velocity(self, right_wheel=1, value=0):
if right_wheel:
if abs(self.right_wheel_velocity + value) <= self.max_velocity:
self.right_wheel_velocity += value
else:
if abs(self.left_wheel_velocity + value) <= self.max_velocity:
self.left_wheel_velocity += value
# Update the states (position, direction) of the robot using the equations of motion
def update_states(self, time):
self.x_position += ((self.left_wheel_velocity + self.right_wheel_velocity)/2
*math.cos(self.direction_angle)*time)
self.y_position -= ((self.left_wheel_velocity + self.right_wheel_velocity)/2
*math.sin(self.direction_angle)*time)
self.direction_angle += ((self.right_wheel_velocity - self. left_wheel_velocity)
/ self.width * time)
# Update the images based on current states
def update_image(self):
self.rotated_image = pygame.transform.rotozoom(self.original_image, math.degrees(self.direction_angle),1)
self.bounding_rectangle = self.rotated_image.get_rect(center=(self.x_position, self.y_position))
# Update the robot by updating the states and images to reflect current status
def update(self, time):
self.update_states(time)
self.update_image()