P-Controller for robot control
Below I present a simple implementation of the P-Controller in python. The presented function can be used in a loop and will allow the movement of the robot from the start position to the goal. You can chain the whole path and input the goal of next path points, when one is reached.
import math def run_p_controller(pos, goal, heading, speed=70, ksi=0.8, precision=80): """Function for running robot motors using P-controller Parameters ---------- pos: float tuple The current position of the robot goal: float tuple The goal position heading: float The current heading of the robot speed: int The initial speed at which wheels should be turned ksi: float The value of the steering scalar precision: int The precision of the P-controller orientation """ # Direction estimation based on robot and goal positions delta_x = goal - pos delta_y = goal - pos direction = math.atan2(delta_x, delta_y) / math.pi * 180 # Orientation conversion into the interval [0...179;-180,...,-1] orient = heading - direction if orient > 180: orient -= 360 if orient < -180: orient += 360 # P-controller left_wheel = speed right_wheel = speed if orient <= 0: if abs(orient) > precision: left_wheel = -right_wheel else: left_wheel = right_wheel + (ksi * orient) if orient > 0: if orient > precision: right_wheel = -left_wheel else: right_wheel = left_wheel - (ksi * orient) # Return wheel power values return left_wheel, right_wheel