Robot Drift correction help

[Isaiah Foss]

We have a robot needed coding, we succeeded however the left wheel has an issue with drift. This was compiled by a professor that requires drift correction. He is overseas and uncontactable.... the code is as follows.

define a procedure to recalibrate rotational speeds when any wheel can be affected by random offset.

import matplotlib.pyplot as plt #enables plotting capabilities for the robot
                                #for this case this is the OUTPUT as the whole 
                                #robot is simulated through this notebook
import numpy as np #Numerical Python library as described in the Intro Python Notebook
import math #math module 
import scipy.constants as sc #shortcut for physical and mathematical constants 
                             #and units
import sympy as sym #SymPy is a Python library for symbolic mathematics
import decimal
decimal.getcontext().prec = 100
from math import sin, cos

class Robot(object):
    """Defines basic mobile robot properties. Defines __init__, step, print_xya, and 
    plot_robot methods. Is called in the main class of DDRobot."""
    def __init__(self):
        #initialization class. 
        self.pos_x  = 0.0 #placing bot at origin: (x,y) = (0,0)
        self.pos_y  = 0.0 
        self.angle  = 0.0 # IN RAD setting alpha = 0
        self.plot   = False #intializing no plot to show
        self._delta = 0.01 #
        self.step_plot = int(8) #plotting every 5 iterations
        self.mag_plot = 1.5 # arrow magnification
        self.angle_evol = []

    # Movement
    def step(self):
        """ updates the x,y and alpha """
        self.deltax()
        self.deltay()
        self.deltaa()
        self.angle_evol.append(self.angle)

    def move(self, seconds):
        """ Moves the robot for an 's' amount of seconds. Takes argument of seconds as type 
        int"""
        for i in range(int(seconds/self._delta)):
            self.step()
            if i % self.step_plot == 0 and self.plot: # plot path every 5 steps
                self.plot_xya() #plots a dot in the position of the robot 

    # Printing-and-plotting:
    def print_xya(self):
        """ prints the x,y position and angle """
        print ("x = " + str(self.pos_x) +" "+ "y = " + str(self.pos_y))
        print ("a = " + str(self.angle))
        
    def plot_robot(self):
        """ plots an arrow representation of the robot. """
        plt.arrow(self.pos_x, self.pos_y, 0.001
                  * cos(self.angle), 0.001 * sin(self.angle),
                  head_width=self.mag_plot*self.length, head_length=self.mag_plot*self.length,
                  fc='k', ec='k')

    def plot_xya(self):
        """ plots a dot in the position of the robot """
        plt.scatter(self.pos_x, self.pos_y, c='r', edgecolors='r')


class DDRobot(Robot):
    """Defines a differential drive robot. Is the main class. Moves the robot. 
    Class Robot is effectively created within class DDRobot as many of the 
    methods from Robot class are called within DDRobot class."""
    
    def __init__(self):
        """__init__ : Initialization class (for DDRobot class)"""
        Robot.__init__(self)
        self.radius = 0.1 #radius of bot wheel as defined in Fig 3
        self.length = 0.4 #length between bot wheels as defined in Fig 3
        self.rt_spd_left = 0.0 #initializing rotation speed, left wheel to 0
        self.rt_spd_right = 0.0 #initializing rotation speed, right wheel to 0
        a = 0
        while a == 0:
            a = np.random.randint(-1,2)
        self.drift = a*np.random.uniform(0.0,0.1)

    def deltax(self):
        """ update x position depending on L and R angular speeds """
        self.pos_x += self._delta * (self.radius*0.5) \
        * (self.rt_spd_right + (self.rt_spd_left + self.drift))*cos(self.angle)

    def deltay(self):
        """ update y position depending on l and r angular speeds """
        self.pos_y += self._delta * (self.radius*0.5) \
        * (self.rt_spd_right + (self.rt_spd_left + self.drift))*sin(self.angle)

    def deltaa(self):
        """ update angle depending on l and r angular speeds """
        self.angle += self._delta * (self.radius/self.length) \
        * (self.rt_spd_right - (self.rt_spd_left + self.drift))

def D2R(a):
    """function to convert degrees to radians. Takes single argument of degrees."""
    return float(((math.pi)*float(a))/180.0)
def R2D(a):
    """Function to convert radians to degrees. Takes single argument of radians."""
    return float((180.0*float(a)/(math.pi)))


plt.figure(figsize=(6,6))
pos_x_0 = 0
pos_y_0 = 0
wheelradius = 0.5
wheelseparation = 1
omega = 1
line_length = 10
Delta_t = 0.01

# Robot dimension and wheel rotation speed
mybot = DDRobot() # enables methods to be passed to the Robot and DDRobot classes as objects
mybot.radius = wheelradius
mybot.length = wheelseparation

#initialization block
mybot.pos_x = pos_x_0              # calling pos_x method to set x initial position - in center of plot
mybot.pos_y = pos_y_0              # calling pos_y method to set y initial position - in center of plot
mybot.angle = float(D2R(0))  # initial starting direction: 0 radians
mybot.plot = True            # turning on plot
mybot.mag_plot = 0.5           # coefficient of magnification of the arrow
mybot.step_plot = int(50 * 0.01 / Delta_t)
mybot.plot_robot()           # plotting the robot!
mybot._delta = Delta_t

# straight line
mybot.rt_spd_right = omega 
mybot.rt_spd_left = omega  .011
t = time_for_line(line_length,omega,mybot.radius)
mybot.move(t)



mybot.plot_robot()            # plot robot's entire path
plt.xlim([-1, 11])             # axis limits
plt.ylim([-11, 11])
plt.rc('font', family='serif')
plt.rc('xtick', labelsize=20)
plt.rc('ytick', labelsize=20)
plt.show()

print("final angle of the robot: %.5f" %R2D(mybot.angle))

### your correction here
#correction = 

### Demonstration that your correction worked (please modify approprietly)

mybot.pos_x = pos_x_0              # calling pos_x method to set x initial position - in center of plot
mybot.pos_y = pos_y_0              # calling pos_y method to set y initial position - in center of plot
mybot.angle = float(D2R(0))
mybot.rt_spd_right = omega 
mybot.rt_spd_left = omega 
# t = time_for_line(line_length,omega,mybot.radius)
mybot.move(t)




mybot.plot_robot()            # plot robot's entire path
plt.xlim([-1, 11])             # axis limits
plt.ylim([-11, 11])
plt.rc('font', family='serif')
plt.rc('xtick', labelsize=20)
plt.rc('ytick', labelsize=20)
plt.show()

print("final angle of the robot: %.5f" %R2D(mybot.angle))