Uncategorized

pid motor controller

Asked 
Active today
Viewed 35 times
1

I’m trying to count rpm of dc and controlling its rpm using PID controller motor which I have done successfully in my code as shown below.

But when I try to count RPM of 2 DC motors simultaneously in a single code motors keeps on misbehaving and i am not able to count the RPM.

import RPi.GPIO as GPIO
import time
import math
import numpy as np

sensorRight = 18 
sensorLeft = 21 

##

Left motor
motorLeft_1 = 12
motorLeft_2 = 20

##Right motor
motorRight_1 = 19
motorRight_2 = 26


GPIO.setwarnings(False)
GPIO.setmode(GPIO.BCM)

GPIO.setup(sensorLeft,GPIO.IN)
GPIO.setup(sensorRight,GPIO.IN)

GPIO.setup(motorLeft_1,GPIO.OUT)
GPIO.setup(motorLeft_2,GPIO.OUT)
GPIO.setup(motorRight_1,GPIO.OUT)
GPIO.setup(motorRight_2,GPIO.OUT)

pwmLeftForward = GPIO.PWM(motorLeft_1,1000)
pwmLeftForward.start(0)
pwmRightForward = GPIO.PWM(motorRight_1,1000)
pwmRightForward.start(1)

dutyCycleLeft=30
pwmLeftForward.ChangeDutyCycle(dutyCycleLeft) ##Left_motor
GPIO.output(motorLeft_2,False)
time.sleep(0.1)

dutyCycleRight=30
pwmRightForward.ChangeDutyCycle(dutyCycleRight) ##left_motor
GPIO.output(motorRight_2,False)
time.sleep(0.1)

countLeft = 0
rpmLeft = 0
deltaLeft = 0
startLeft = 0
endLeft = 0
rpmLeftDes=60
previousErrorLeft=0
totalErrorLeft=0

countRight = 0
rpmRight = 0
deltaRight = 0
startRight = 0
endRight = 0
previousErrorRight=0
totalErrorRight=0
rpmRightDes = 60

sample=10



def get_rpmLeft(d):
    global countLeft
    global rpmLeft
    global startLeft
    global endLeft
    global previousErrorLeft
    global totalErrorLeft

    if not countLeft:
        startLeft = time.time()
        countLeft = countLeft +1

    else:
        countLeft = countLeft+1

    if countLeft == sample:
        endLeft = time.time()
        deltaLeft = endLeft - startLeft
        deltaLeft = deltaLeft/60
        rpmLeft = (sample/deltaLeft)/341.4
        print("rpmLeft Left =", rpmLeft)

        controller_LeftOut=controller_Left(rpmLeft,previousErrorLeft,totalErrorLeft)
        dutyCycleLeft=controller_LeftOut[0]
        previousErrorLeft=controller_LeftOut[1]
        totalErrorLeft=totalErrorLeft+previousErrorLeft

        pwmLeftForward.ChangeDutyCycle(dutyCycleLeft) ##Left_motor
        GPIO.output(motorLeft_2,False)
        time.sleep(0.1)
        countLeft=0
    return rpmLeft

def get_rpm_right(c):
    global countRight
    global rpmRight
    global startRight
    global endRight
    global previousErrorRight
    global totalErrorRight

    if not countRight:
        startRight = time.time()
        countRight = countRight +1

    else:
        countRight = countRight+1

    if countRight == sample:
        endRight = time.time()
        deltaRight = endRight - startRight
        deltaRight = deltaRight/60
        rpmRight = (sample/deltaRight)/341.4
        print("rpmRight Right =", rpmRight)


        controller_RightOut=controller_Right(rpmRight,previousErrorRight,totalErrorRight)
        dutyCycleRight=controller_RightOut[0]
        previousErrorRight=controller_RightOut[1]
        totalErrorRight=totalErrorRight+previousErrorRight

        pwmRightForward.ChangeDutyCycle(dutyCycleRight) ##left_motor
        GPIO.output(motorRight_2,False)
        time.sleep(0.1)
        countRight=0
    return rpmRight

def controller_Left(rpmLeft,previousErrorLeft,totalErrorLeft):
    global dutyCycleLeft
    kp=0.25
    kd=0.05
    ki=0.2
    errorLeft=rpmLeftDes-rpmLeft

    dutyCycleLeft=kp*errorLeft+kd*(errorLeft-previousErrorLeft)+ki*totalErrorLeft
    if dutyCycleLeft>98:
        dutyCycleLeft=98
    elif dutyCycleLeft< 4:
        dutyCycleLeft= 4

    return dutyCycleLeft,errorLeft

def controller_Right(rpmRight,previousErrorRight,totalErrorRight):
    global dutyCycleRight
    kp=0.25
    kd=0.05
    ki=0.2
    error=rpmRightDes-rpmRight

    dutyCycleRight=kp*error+kd*(error-previousErrorRight)+ki*totalErrorRight
    if dutyCycleRight>98:
        dutyCycleRight=98
    elif dutyCycleRight<1:
        dutyCycleRight=1

    return dutyCycleRight,error

GPIO.add_event_detect(sensorLeft,GPIO.RISING, callback = get_rpmLeft)
GPIO.add_event_detect(sensorRight,GPIO.RISING, callback = get_rpm_right)

try:
    while True:
        time.sleep(0.1)

except KeyboardInterrupt:
    print("QUIT")
    GPIO.cleanup()
 New contributor
  • Could you explain how things are misbehaving? – NomadMaker yesterday
  • 1
    Whenever I see code (in whatever language) posted without any comments I wonder – but don’t feel obliged to make the effort the author didn’t. – Milliways yesterday
  • Hi @Ravinder singh, Welcome and nice to meet you. You remind me in my Arduino Decimilla days using C++ to pid control DC gear motor and quadrature encoder speeddometering. I skimmed your program and found it structured and clear. Obviously you are doing two independent things: (1) pid moving motor, (2) measuring rpm. The measuring part is event driven. So the two parts are clear cuttingly separated. In other words, one can work without the other. Now your mover function is working OK, so you can ask the mover to take a break and use your finger instead to move the motor. / to continue, … – tlfong01 yesterday   
  • Using optical or Hall effect quadrature encoding or not, you can sense GPIO level change (don’t use edge triggering for now) by stupidly looping (don’t use interrupt driven call back functions which are difficult to troubleshoot. I usually start with looping to detect level trigger. By hand you can move forward and backward at the trigger point, and by level means no bouncing problem to bother. – tlfong01 yesterday   
  • I know you worry that you will lose face if your bad friends discover that you are not using event driven edge triggering. Me too. So I almost always fakingly tell my bad friends that I don’t know nothing about looping and level triggering, because those newbie methods damage my high class brain, … 🙂 – tlfong01 yesterday   
  • @tlfong01 People lose face by being stupid. It is bad advice to suggest looping in the questioners case. The only thing which can work for his problem is the use of callbacks. – joan yesterday
  • @NomadMaker for single motor my code is working perfectly but for two motors it is not able to calculate number of edges correclty therefore RPM keeps on fluctuating (misbehaving). – Ravinder singh 10 hours ago

0

I would like to suggest that you redo your code somewhat.

In your sensor interrupt routines do as little as possible. Just increment the variable countLeft or countRight as applicable. Don’t do the PID calculations here. The reason you do this is to allow you to complete an interrupt before another one comes along.

Do the speed calculations in the main code loop. Do your best to work with local variables so that the pwm interrupt routine can’t get bad copies of this data. If the calculations are fast enough, it could be done in the pid interrupt routine.

Do the pid calculations in a timer interrupt that goes off every 0.05 seconds or so. Pid loops work best if they are are updated at very regular intervals.

I don’t know if python has any functions to synchronize your variables so they don’t change on you during one of these interrupts.

The goal is make sure these routines are fast enough to complete and that they don’t overwrite variables used by other interrupt routines.

I know how I’d do this in java or c or even rust, but I’ve never gotten this deep into python.

I hope I’m being clear.

Question: Do you wheel encoders have one or two signals?

Categories: Uncategorized

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out /  Change )

Google photo

You are commenting using your Google account. Log Out /  Change )

Twitter picture

You are commenting using your Twitter account. Log Out /  Change )

Facebook photo

You are commenting using your Facebook account. Log Out /  Change )

Connecting to %s

This site uses Akismet to reduce spam. Learn how your comment data is processed.

%d bloggers like this: