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 self._y_position = starting_location self._direction_angle = starting_location 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()