PI controller threading & strange issue

Viewed 35

I have a attached a rotary encoder to a DC motor shaft in hopes of creating a python script on a PI4 in which one could set a desired angle, motor will move CW at a 10 - duty cycle, and motor will stop and hold position on desired angle that once set angle is read back into code from rotary encoder (@ 800 pulses per revolution - reading out 0.45 deg increments).

The PI controller will ideally then hold the set angle (set in code but later GUI) and will not move regardless of outside force on the shaft.

Lastly i am using if statments after my PI control signal 'PI' into a duty cycle ouput thus controlling the speed of the motor as it reaches the set point (either slowing down or speeding up tp get to the set point) with error PI polarity governing directional output....if this could be done better i'd appreciate any suggestions

I am only getting 1 error i cant figure out and cant find a solution online...but i feel i am close to getting this code right. Any criticism is welcome. The error is regarding the "PI" output of the Pi equation:

"Value of type "int" is not indexable"

from RPi import GPIO
import time
import threading



#Encoder GPIO Pins

clk = 15 #  GRN/YLW Z+
dt = 11 # BLU/GRN Z-

GPIO.setmode(GPIO.BOARD)
GPIO.setwarnings(False)
GPIO.setup(clk, GPIO.IN, pull_up_down=GPIO.PUD_DOWN)
GPIO.setup(dt, GPIO.IN, pull_up_down=GPIO.PUD_DOWN)


#Motor GPIO Pins
PWMPin = 18 # PWM Pin connected to ENA.
Motor1 = 16 # Connected to Input 1.
Motor2 = 18 # Connected to Input 2.

GPIO.setup(PWMPin, GPIO.OUT) # We have set our pin mode to output
GPIO.setup(Motor1, GPIO.OUT)
GPIO.setup(Motor2, GPIO.OUT)

GPIO.output(Motor1, GPIO.LOW)# When program start then all Pins will be LOW.
GPIO.output(Motor2, GPIO.LOW)


def MotorClockwise():
    GPIO.output(Motor1, GPIO.LOW) # Motor will move in clockwise direction.
    GPIO.output(Motor2, GPIO.HIGH)
   
    
def MotorAntiClockwise():
    GPIO.output(Motor1, GPIO.HIGH) # Motor will move in anti-clockwise direction.
    GPIO.output(Motor2, GPIO.LOW)
   
#Sim Values

duty_c = 10 #duty Cycle
PwmValue = GPIO.PWM(PWMPin, 10000) # We have set our PWM frequency to 5000.
PwmValue.start(duty_c) # That's the maximum value 100 %.

error = 0
Set_point = 1.8
encoder_counter = 0
encoder_angle = 0
Kp = 2
Ki = 1
PI = 0

#Sim Parameters
Ts = .1 #sampling time
Tstop = 200 #end simulation time
N = int(Tstop/Ts)#simulation length


def ReadEncoder():

    global encoder_counter
    global encoder_angle
    clkLastState = GPIO.input(clk)

    while True:
        clkState = GPIO.input(clk)
        dtState = GPIO.input(dt)
        encoder_angle = float(encoder_counter/800)*360
 
        if clkState != clkLastState:
            if dtState != clkState:
                encoder_counter += 1
            else:
                encoder_counter -= 1
                
            #print("encoder_counter", encoder_counter )
            print("encoder_angle", encoder_angle )
            time.sleep(1) 
            
        clkState = clkLastState
        # some delay needed in order to prevent too much CPU load,
        # however this will limit the max rotation speed detected
        time.sleep(.001)                           
                


def PI_function():
    
    global PI
    global duty_c
    global error
    global Kp
    global Ki
    global encoder_angle
    
    for k in range(N+1):
    
        error = int(Set_point) - encoder_angle
        print (f"{error} Error Detected")

        PI = PI[k-1] + Kp*(error[k] - error[k-1]) + (Kp/Ki)*error[k] #PI equation
        print (f"{PI} PI value")
        
    if (PI > 0):
        MotorClockwise()
    else:
        MotorAntiClockwise()
        
    if ((PI > duty_c) & (duty_c <100)):
        duty_c += 1
    if ((PI < duty_c) & (duty_c > 10)):
        duty_c -= 1
        
    return()


def main_function():
    
    if (N != 0):
        
        encoder_thread = threading.Thread( target=ReadEncoder)
        encoder_thread.start()
        encoder_thread.join()

        PI_thread = threading.Thread( target=PI_function)
        PI_thread.start()
        PI_thread.join()
        
        PwmValue.ChangeDutyCycle(duty_c)
        
    else:
        
        PwmValue.ChangeDutyCycle(0)
        print(f"Motor Stopped & Holding at {encoder_angle} Degrees")
        
0 Answers
Related