'DIY car not moving in a straight line

I'm working on a project, in which I have assembled a 4 wheeled small DIY car, it has 4 DC motors, a raspberry pi 3b+, an L289D H bridge and two optical wheel encoders, one on each wheel on each side.

The problem that I'm currently facing is that the car won't drive in a straight path. I have used PWM to correct it as well but it would still deviate and would not drive straight.

Initially I was just playing around with the PWM values using trial and error, but now I'm using the input from the encoders to modify the PWM values in real time.

Below is the code that I'm using to test the process:

import RPi.GPIO as GPIO
import time
import math
from time import sleep

GPIO.setmode(GPIO.BOARD)

GPIO.setup(11, GPIO.IN, pull_up_down=GPIO.PUD_UP)  #Right encoder
GPIO.setup(7, GPIO.IN, pull_up_down=GPIO.PUD_UP)   #Left encoder

GPIO.setup(13, GPIO.OUT)   #clockwise left
GPIO.setup(15, GPIO.OUT)   #anticlockwise right
GPIO.setup(16, GPIO.OUT)   #anticlockwise left
GPIO.setup(18, GPIO.OUT)   #clockwise right

GPIO.setup(22, GPIO.OUT)   #enabler h bridge
GPIO.setup(29, GPIO.OUT)   #enabler h bridge

p=GPIO.PWM(22,100)        #for right wheel
q=GPIO.PWM(29,100)        #for left wheel

stateLast1 = GPIO.input(11)
rotationCount1=0
stateCount1=0

stateLast2 = GPIO.input(7)
rotationCount2=0
stateCount2=0

circ=62.4*math.pi #mm
statesPerRotation=20
distancePerStep= circ/statesPerRotation

p.start(100)
q.start(100)

GPIO.output(13, GPIO.HIGH)
GPIO.output(18, GPIO.HIGH)

distance1 = 0
distance2 = 0
stateCountTotal1=0
stateCountTotal2=0
stateLast1 = 0
stateLast2 = 0

while distance1 <= 300 or distance2 <= 300 :

    stateCurrent1 = GPIO.input(11)
    stateCurrent2 = GPIO.input(7)

    if stateCurrent1 != stateLast1:
        stateLast1 = stateCurrent1
        stateCount1 += 1
        stateCountTotal1 += 1
    
    if stateCount1 == statesPerRotation:
        rotationCount1 += 1
        stateCount1 = 0
    
    
    if stateCurrent2 != stateLast2:
        stateLast2 = stateCurrent2
        stateCount2 += 1
        stateCountTotal2 += 1
    
    if stateCount2 == statesPerRotation:
        rotationCount2 += 1
        stateCount2 = 0    

    distance1 = distancePerStep * stateCountTotal1      #distance travelled by right enc
    distance2 = distancePerStep * stateCountTotal2      #distance travelled by left enc

    if distance1 > distance 2 : 
        p.ChangeDutyCycle(95)        #reduce power of right wheels
            
    if distance2 > distance 1 : 
        q.ChangeDutyCycle(95)        #reduce power of left wheels

GPIO.output(13, GPIO.LOW)
GPIO.output(18, GPIO.LOW)

except KeyboardInterrupt:
GPIO.cleanup()

I've messed with the PWM settings in changedutycycle. Upon printing distances 1 & 2 at the end, I either get constant values of either distance 1 or 2, while the other distance would have a difference of about 3cm-10cm. Only sometimes would I get both distances to be about the same, the least difference between them being less than 1cm, and in only those cases would the car move in a near perfect straight line which is good enough. I don't understand why I'm not getting consistent results.

I would appreciate any help. Thanks.



Sources

This article follows the attribution requirements of Stack Overflow and is licensed under CC BY-SA 3.0.

Source: Stack Overflow

Solution Source