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")
Related
I am having problems with a module that I am trying to import in python.
It is being run through a RPi 4b, driving 4 motors through 2 L298N h bridges.
When I run this code (FourMotorClassWithTurn) there is no problem and the motors perform as expected.
import RPi.GPIO as GPIO
from time import sleep
GPIO.setmode(GPIO.BCM)#setup the pin out to be board numbers not BCM
GPIO.setwarnings(False)#stops set warnings from showing
class Motor():
def __init__(self, EnFL, In1FL, In2FL, EnFR, In3FR, In4FR,
EnBR, In1BR, In2BR, EnBL, In3BL, In4BL):
self.EnFL = EnFL
self.In1FL = In1FL
self.In2FL = In2FL
#Front left motor
self.EnFR = EnFR
self.In3FR = In3FR
self.In4FR = In4FR
#Front right motor
self.EnBR = EnBR
self.In1BR = In1BR
self.In2BR = In2BR
#Back right motor
self.EnBL = EnBL
self.In3BL = In3BL
self.In4BL = In4BL
#Back left motor
GPIO.setup(self.EnFL,GPIO.OUT)
GPIO.setup(self.In1FL,GPIO.OUT)
GPIO.setup(self.In2FL,GPIO.OUT)
GPIO.setup(self.EnFR,GPIO.OUT)
GPIO.setup(self.In3FR,GPIO.OUT)
GPIO.setup(self.In4FR,GPIO.OUT)
GPIO.setup(self.EnBR,GPIO.OUT)
GPIO.setup(self.In1BR,GPIO.OUT)
GPIO.setup(self.In2BR,GPIO.OUT)
GPIO.setup(self.EnBL,GPIO.OUT)
GPIO.setup(self.In3BL,GPIO.OUT)
GPIO.setup(self.In4BL,GPIO.OUT)
self.pwmFL = GPIO.PWM(self.EnFL,100);
self.pwmFL.start(0);
self.pwmFR = GPIO.PWM(self.EnFR,100);
self.pwmFR.start(0);
self.pwmBR = GPIO.PWM(self.EnBR,100);
self.pwmBR.start(0);
self.pwmBL = GPIO.PWM(self.EnBL,100);
self.pwmBL.start(0);
#-------------------------------------------------------------------------
def move(self, speed=0.5, turn=0, time=0):
"""
Sets the motor to move
Parameters
----------
speed : TYPE, float
DESCRIPTION. Sets the speed of the motor 0 - 1. The default is 0.
turn : TYPE, float
DESCRIPTION. Sets the turn direction and rate of the
platform (-1) - 1. Full left = -1, full right = 1 The default is 0.
time : TYPE, float
DESCRIPTION. Sets the run time of the motor in seconds.
The default is 0.
Returns
-------
None.
"""
speed *= 100 #converts speed in PWM duty cycle value
turn *= 100 #converts turn in PWM duty cycle value
leftSpeed = speed + turn #adjusts the motor speed on each side for
#differential steering
rightSpeed = speed - turn
if leftSpeed>100: leftSpeed=100
elif leftSpeed<-100: leftSpeed=-100
if rightSpeed>100: rightSpeed=100
elif rightSpeed<-100: rightSpeed=-100 #PWM can only accept a maximum
#of 100 so values need limiting
self.pwmFL.ChangeDutyCycle(abs(leftSpeed))
self.pwmFR.ChangeDutyCycle(abs(rightSpeed))
self.pwmBR.ChangeDutyCycle(abs(rightSpeed))
self.pwmBL.ChangeDutyCycle(abs(leftSpeed))
#PWm can only accept positive values
if leftSpeed>0:
GPIO.output(self.In1FL, GPIO.LOW)
GPIO.output(self.In2FL, GPIO.HIGH)
GPIO.output(self.In3BL, GPIO.LOW)
GPIO.output(self.In4BL, GPIO.HIGH)
#left motor forward = In1_LOW & In3_LOW
# In2_HIGH & IN4_HIGH
else:
GPIO.output(self.In1FL, GPIO.HIGH)
GPIO.output(self.In2FL, GPIO.LOW)
GPIO.output(self.In3BL, GPIO.HIGH)
GPIO.output(self.In4BL, GPIO.LOW)
#left motor backwards = In1_HIGH & In3_HIGH
# In2_LOW & IN4_LOW
if rightSpeed>0:
GPIO.output(self.In3FR, GPIO.LOW)
GPIO.output(self.In4FR, GPIO.HIGH)
GPIO.output(self.In1BR, GPIO.LOW)
GPIO.output(self.In2BR, GPIO.HIGH)
#right motor forward = In1_LOW & In3_LOW
# In2_HIGH & IN4_HIGH
else:
GPIO.output(self.In3FR, GPIO.HIGH)
GPIO.output(self.In4FR, GPIO.LOW)
GPIO.output(self.In1BR, GPIO.HIGH)
GPIO.output(self.In2BR, GPIO.LOW)
#right motor backwards = In1_HIGH & In3_HIGH
# In2_LOW & IN4_LOW
print(f'Right speed is {rightSpeed}.')
print(f'Left speed is {leftSpeed}.')
sleep(time)
#-------------------------------------------------------------------------
def stop(self, time):
"""
Stops the motor
Parameters
----------
t : TYPE, optional
DESCRIPTION. Sets the stop time of the motor. The default is 0.
Returns
-------
None.
"""
self.pwmFL.ChangeDutyCycle(0)
self.pwmFR.ChangeDutyCycle(0)
self.pwmBR.ChangeDutyCycle(0)
self.pwmBL.ChangeDutyCycle(0)
sleep(time)
#----End of class--------------------------------------------------------------
def main():
"""
Test script if motor class is run in main
Returns
-------
None.
"""
motor1 = Motor(13,3,4,19,22,27,21,20,16,14,15,18)
motor1.stop(2)
motor1.move(0.4,-0.6,2)
GPIO.cleanup()
#-----------------------------------------------------------------------------
if __name__ == '__main__':
main()
However when I try to import the class into another module
from FourMotorClassWithTurn import Motor
import RPi.GPIO as GPIO
from time import sleep
GPIO.setmode(GPIO.BCM)#setup the pin out to be board numbers not BCM
GPIO.setwarnings(False)#stops set warnings from showing
motor1 = Motor(13,3,4,19,22,27,21,20,16,14,15,18)
motor1.stop(2)
motor1.move(0.4,-0.6,2)
GPIO.cleanup()
The motors will run once correctly but when I try to run it a second time I get a PWM fault
motor1 = Motor(13,3,4,19,22,27,21,20,16,14,15,18)
line 58, in init
self.pwmFL = GPIO.PWM(self.EnFL,100);
RuntimeError: A PWM object already exists for this GPIO channel
Anyone have an idea how to solve this?
I have a class for a DC-motor. It has attributes that can e.g. change the set rotational speed.
In order to control the actual speed I would like to run a thread that guards the acceleration. So if a user sets a new speed the thread will slowly increase the actual speed to match the set speed.
My approach:
import time
import threading
import RPi.GPIO as GPIO
class motorController:
#constructors
def __init__(self,maxPower,powerPin,directionPin,direction=True):
self.maxPower = abs(maxPower)
self.power = 0 #initially off
self.setPower = 0
#set pins motors are connected to
self.powerPin = powerPin
self.directionPin = directionPin
self.direction = direction
#initialize PWM
GPIO.setmode(GPIO.BCM)
#initialize powerPin
GPIO.setup(self.powerPin, GPIO.OUT) # set pin to output
self.powerPwm = GPIO.PWM(self.powerPin, 100) # Initialize PWM on pwmPin 100Hz frequency, max is 8kHz, min is 10Hz
self.powerPwm.start(self.power) # Start PWM with 0% duty cycle
#initialize directionPin
GPIO.setup(self.directionPin, GPIO.OUT,initial=self.direction) # set pin to output
#initialize controll deamon
self.__lastCall = time.time()
self.setPointIsNotReached = threading.Condition() #currently unused
self.deamonTimeLock = threading.Lock()
self.controllThread = threading.Thread(target=self.__powerControllerDeamon())
self.controllThread.setDaemon(True)
self.controllThread.start()
def setTargetPower(self,setPower):
setPower = min(setPower,self.maxPower)
self.setPower = max(setPower,-self.maxPower)
with self.deamonTimeLock:
self.__lastDeamonCall = t
def __setPower(self,power):
#limit power to maxPower
self.power = min(abs(power),self.maxPower)
self.powerPwm.ChangeDutyCycle(self.power)
#set rotation direction
if power < 0:
self.direction = False
else:
self.direction = True
GPIO.output(self.directionPin, self.direction) #set to 3.3V if direction is true, or 0V if direction is false
def __powerControllerDeamon(self):
#private method that controls the power of the motor
t = time.time()
dt = t-self.__lastCall
if self.power > self.setPower:
m = -50 #ramp from 100 to 0 in minimal 2 sec
else:
m = 20 # ramp from 0 to 100 in minimal 5sec
newPower = self.power + m * dt
#detect if power is reched through change of sign
dP = self.setPower - self.power
dP2 = self.setPower - newPower
if dP*dP2 <= 0: #change of sign or one is zero
newPower = self.setPower #set point reached
else:
with self.setPointIsNotReached
self.setPointIsNotReached.notify()#set point NOT reached
print(newPower)
self.__setPower(newPower)
with self.deamonTimeLock:
self.__lastDeamonCall = t
and
from motorController import motorController as mC
import time
try:
m1 = mC(100,18,17)
m1.setTargetPower(100)
time.sleep(10) #should have powered up by now
except BaseException as e:
import RPi.GPIO as GPIO
GPIO.cleanup()
raise(e)
In this example, the thread function is called once and then never again.
I'm new to threading and python. My questions:
Can I create a thread from a constructor and have it running until the main program ends (that's why i set deamon true)?
If the answer is yes, what's my mistake?
Your __powerControllerDeamon method should contain a loop that runs until it reaches the desired speed or until it is told to stop.
Since it doesn't contain a loop, it will run once and exit.
Note that this loop should also contain a small amount of sleep, to prevent it from hogging the CPU.
I'm working on automation of a vision based machine driven by stepper motor. Structure of my program is as followed.
import ImageProcessing
import Mechanices
import cv2
def rotate_motor(steps,direction):
global g_motor_ploc
DIR = 20 # Direction GPIO Pin
STEP = 21 # Step GPIO Pin
delay = .00055
GPIO.setmode(GPIO.BCM)
GPIO.setup(DIR, GPIO.OUT)
GPIO.setup(STEP, GPIO.OUT)
GPIO.output(DIR, direction)
for x in range(steps):
GPIO.output(STEP, GPIO.HIGH)
sleep(delay)
GPIO.output(STEP, GPIO.LOW)
sleep(delay)
GPIO.cleanup()
def findImage():
####################################
< Very Fast Image Processing Code >
####################################
return position_of_object
def calculate_steps(position):
####################################
< Very Fast Steps and Direction Calculation Code >
####################################
return steps_required,direction_of_rotation
def main():
while True:
position=findImage()
steps, direction = calculate_steps
rotate_motor(steps,directions) # Very Slow Process
if __name__ == "__main__":
main()
Despite very fast processing of images, whole program has to wait for rotate_motor to complete before processing next frame. In mean time if object moves backward it fails to see the change unless previously calculated position is achieved.
I'm looking forward to run rotate_motor in a separate thread and update the number_of_steps and direction required for motion. So that even if previously calculated position is not achieved, it should know how many more steps are required or change of direction is required.
So I modified the program as followed:
import multiprocessing as mp
import cv2
(r, g_w) = mp.Pipe(True)
g_motor_ploc = 0
def rotate_motor(steps,direction):
global g_motor_ploc
DIR = 20 # Direction GPIO Pin
STEP = 21 # Step GPIO Pin
delay = .00055
GPIO.setmode(GPIO.BCM)
GPIO.setup(DIR, GPIO.OUT)
GPIO.setup(STEP, GPIO.OUT)
GPIO.output(DIR, direction)
for x in range(steps):
GPIO.output(STEP, GPIO.HIGH)
sleep(delay)
GPIO.output(STEP, GPIO.LOW)
sleep(delay)
if direction==1:
g_motor_ploc -= 1
else:
g_motor_ploc += 1
GPIO.cleanup()
def findImage():
####################################
< Very Fast Image Processing Code >
####################################
return position_of_object
def calculate_steps(position):
####################################
< Very Fast Steps and Direction Calculation Code >
####################################
return steps_required,direction_of_rotation ## (Clockwise, Counter Clockwise)
def linear_motion(r):
global g_motor_ploc
while True:
loc = r.recv()
steps = loc - g_motor_ploc
if steps < 0:
direction = 1
else:
direction = 0
rotate_motor(abs(steps),direction)
def main():
while True:
position=findImage()
steps = calculate_steps
g_w.send(steps)
if __name__ == "__main__":
motor_proc = mp.Process(target=linear_motion, args=(r,))
motor_proc.daemon = True
motor_proc.start()
main()
Using the mp.Pipe() it interrupts the linear_motion() as soon as new position is send to the pipe. However this process if so fast that linear_motion never reach to the rotate_motor part.
Any suggestions, I'm confused.
I have Python code running on my raspberry pi 2b and a light sensor, which measures the amount of time it takes for the capacitor of the light sensor to charge and send the pin high:
import RPi.GPIO as GPIO
import time
GPIO.setmode(GPIO.BOARD)
pin_to_circuit = 7
def rc_time (pin_to_circuit):
count = 0
#Output on the pin for
GPIO.setup(pin_to_circuit, GPIO.OUT)
GPIO.output(pin_to_circuit, GPIO.LOW)
time.sleep(0.1)
#Change the pin back to input
GPIO.setup(pin_to_circuit, GPIO.IN)
#Count until the pin goes high
while (GPIO.input(pin_to_circuit) == GPIO.LOW):
count += 1
if count > 1000000:
return True
else:
return count
#Catch when script is interrupted, cleanup correctly
try:
# Main loop
while True:
print rc_time(pin_to_circuit)
except KeyboardInterrupt:
pass
finally:
GPIO.cleanup()
I want when the count goes higher than 1000000, a MG90S, that I have also connected to the pi and a 4AA battery pack, moves about 90 degrees.
The code I was trying to integrate to move the servo:
import RPi.GPIO as GPIO
import time
GPIO.setmode(GPIO.BOARD)
GPIO.setup(12, GPIO.OUT)
p.ChangeDutyCycle(7.5) # turn towards 90 degree
time.sleep(1) # sleep 1 second
p.stop()
GPIO.cleanup()
I want to combine these two Python codes. I tried for a bit, but I have almost no Python experience.
The code is now:
import RPi.GPIO as GPIO
import time
GPIO.setmode(GPIO.BOARD)
pin_to_circuit = 7
def move_90_degrees():
GPIO.setmode(GPIO.BOARD)
GPIO.setup(12, GPIO.OUT)
p = GPIO.PWM(12, 50)
p.start(7.5)
p.ChangeDutyCycle(7.5) # turn towards 90 degree
time.sleep(1) # sleep 1 second
p.stop()
def rc_time (pin_to_circuit):
count = 0
#Output on the pin for
GPIO.setup(pin_to_circuit, GPIO.OUT)
GPIO.output(pin_to_circuit, GPIO.LOW)
time.sleep(0.1)
#Change the pin back to input
GPIO.setup(pin_to_circuit, GPIO.IN)
#Count until the pin goes high
while (GPIO.input(pin_to_circuit) == GPIO.LOW):
count += 1
if count > 1000000:
return True
move_90_degrees()
else:
return count
#Catch when script is interrupted, cleanup correctly
try:
# Main loop
while True:
print rc_time(pin_to_circuit)
except KeyboardInterrupt:
pass
finally:
GPIO.cleanup()
The code does print True when the count exceeds 1000000, but the servo still doesn't move. The servo code on its own does however work correctly. (I forgot a bit of the servo code, that's why p wasn't defined, sorry.)
You could just integrate the code block you are using to move the MG90S into a function, insert it before or below your def rc_time (pin_to_circuit): (but first you have to define p inside, its not really clear what p refers to):
New Function from second code block:
def move_90_degrees():
p = '???'
GPIO.setup(12, GPIO.OUT)
p.ChangeDutyCycle(7.5) # turn towards 90 degree
time.sleep(1) # sleep 1 second
p.stop()
After defining this function, call it inside your first block like:
if count > 1000000:
move_90_degrees()
return True
else:
return count
That should work.
I've made a mistake in the code. I changed the order of the function call inside of the
if count > 1000000:
return True
move_90_degrees()
else:
return count
block to :
if count > 1000000:
move_90_degrees()
return True
else:
return count
otherwise, the code returns before the function call is executed. Is it working now?
So basically we're using raspberry pi for a project and a part of it includes the usage of ultrasonic sensors. We have three, and we've been able to get readings from all of them (I actually came from multi-threading but decided to move to multiprocessing). Here is my code:
#Libraries
from multiprocessing import Process, Lock
import RPi.GPIO as GPIO
import time
#GPIO Mode (BOARD / BCM)
GPIO.setmode(GPIO.BCM)
GPIO.setwarnings(False)
#set GPIO Pins
GPIO_TRIGGER1 = 23
GPIO_ECHO1 = 24
GPIO_TRIGGER2 = 27
GPIO_ECHO2 = 22
GPIO_TRIGGER3 = 25
GPIO_ECHO3 = 17
#set GPIO direction (IN / OUT)
GPIO.setup(GPIO_TRIGGER1, GPIO.OUT)
GPIO.setup(GPIO_ECHO1, GPIO.IN)
GPIO.setup(GPIO_TRIGGER2, GPIO.OUT)
GPIO.setup(GPIO_ECHO2, GPIO.IN)
GPIO.setup(GPIO_TRIGGER3, GPIO.OUT)
GPIO.setup(GPIO_ECHO3, GPIO.IN)
def sense_distance(lock, processName):
#lock.acquire()
gpio_echo_var = ''
gpio_trigger_var = ''
if processName == "Sensor-1":
gpio_echo_var = GPIO_ECHO1
gpio_trigger_var = GPIO_TRIGGER1
elif processName == "Sensor-2":
gpio_echo_var = GPIO_ECHO2
gpio_trigger_var = GPIO_TRIGGER2
elif processName == "Sensor-3":
gpio_echo_var = GPIO_ECHO3
gpio_trigger_var = GPIO_TRIGGER3
print "%s process created." % (processName)
try:
while True:
# set Trigger to HIGH
GPIO.output(gpio_trigger_var, True)
# set Trigger after 0.01ms to LOW
time.sleep(0.00001)
GPIO.output(gpio_trigger_var, False)
StartTime = time.time()
StopTime = time.time()
# save StartTime
while GPIO.input(gpio_echo_var) == 0:
StartTime = time.time()
# save time of arrival
while GPIO.input(gpio_echo_var) == 1:
StopTime = time.time()
# time difference between start and arrival
TimeElapsed = StopTime - StartTime
# multiply with the sonic speed (34300 cm/s)
# and divide by 2, because there and back
distance = (TimeElapsed * 34300) / 2
if distance <= 10:
print "%s has read less than 10 cm." % (processName)
else:
pass
# Reset by pressing CTRL + C
except KeyboardInterrupt:
print("Measurement stopped by User")
GPIO.cleanup()
#lock.release()
if __name__ == '__main__':
lock = Lock()
Process(target=sense_distance, args=(lock, "Sensor-1")).start()
Process(target=sense_distance, args=(lock, "Sensor-2")).start()
Process(target=sense_distance, args=(lock, "Sensor-3")).start()
It successfully reads the input and prints out the text when input goes smaller than 10 cm. However, after some time, they stop. I have run out of ideas and have searched all over only to come up short. Any sort of help will be appreciated.