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")