Uncategorized

pid motor control and rpm measurement

Asked 
Viewed 10 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
  • I’d suggest you lesrn read your encoders before you use pid. This would also make your code sample easier to read so that we could understand it better to help you. – NomadMaker 1 hour ago
  • Could you explain how things are misbehaving? – NomadMaker 1 hour ago
  • 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 59 mins ago
  • 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 12 mins ago   
  • 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 7 mins ago   
  • 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 2 mins ago   Edit

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: