Hello again!!
On the occasion of my previous question, I wanted to ask something new.
According to the given library:
https://abyz.me.uk/rpi/pigpio/examples.html#Python%20code
Two scripts help me send pulses to the GPIOs and read the RPMs sent to the Raspberry Pi 4.
In the first the code is this:
#!/usr/bin/env python
# servo_demo.py
# 2016-10-07
# Public Domain
# servo_demo.py # Send servo pulses to GPIO 4.
# servo_demo.py 23 24 25 # Send servo pulses to GPIO 23, 24, 25.
import sys
import time
import random
import pigpio
NUM_GPIO=32
MIN_WIDTH=1000
MAX_WIDTH=2000
step = [0]*NUM_GPIO
width = [0]*NUM_GPIO
used = [False]*NUM_GPIO
pi = pigpio.pi()
if not pi.connected:
exit()
if len(sys.argv) == 1:
G = [4]
else:
G = []
for a in sys.argv[1:]:
G.append(int(a))
for g in G:
used[g] = True
step[g] = random.randrange(5, 25)
if step[g] % 2 == 0:
step[g] = -step[g]
width[g] = random.randrange(MIN_WIDTH, MAX_WIDTH+1)
print("Sending servos pulses to GPIO {}, control C to stop.".
format(' '.join(str(g) for g in G)))
while True:
try:
for g in G:
pi.set_servo_pulsewidth(g, width[g])
# print(g, width[g])
width[g] += step[g]
if width[g]<MIN_WIDTH or width[g]>MAX_WIDTH:
step[g] = -step[g]
width[g] += step[g]
time.sleep(0.1)
except KeyboardInterrupt:
break
print("\nTidying up")
for g in G:
pi.set_servo_pulsewidth(g, 0)
pi.stop()
This script needs some changes to make it send the pulses to the GPIO I want. For example, I want to send 23 GPIO pulses for the left wheel with specific μs (microsecond) and 24 GPIO for the right wheel.
Then, with the second script:
#!/usr/bin/env python
# read_RPM.py
# 2016-01-20
# Public Domain
import time
import pigpio # http://abyz.co.uk/rpi/pigpio/python.html
class reader:
"""
A class to read speedometer pulses and calculate the RPM.
"""
def __init__(self, pi, gpio, pulses_per_rev=1.0, weighting=0.0, min_RPM=5.0):
"""
Instantiate with the Pi and gpio of the RPM signal
to monitor.
Optionally the number of pulses for a complete revolution
may be specified. It defaults to 1.
Optionally a weighting may be specified. This is a number
between 0 and 1 and indicates how much the old reading
affects the new reading. It defaults to 0 which means
the old reading has no effect. This may be used to
smooth the data.
Optionally the minimum RPM may be specified. This is a
number between 1 and 1000. It defaults to 5. An RPM
less than the minimum RPM returns 0.0.
"""
self.pi = pi
self.gpio = gpio
self.pulses_per_rev = pulses_per_rev
if min_RPM > 1000.0:
min_RPM = 1000.0
elif min_RPM < 1.0:
min_RPM = 1.0
self.min_RPM = min_RPM
self._watchdog = 200 # Milliseconds.
if weighting < 0.0:
weighting = 0.0
elif weighting > 0.99:
weighting = 0.99
self._new = 1.0 - weighting # Weighting for new reading.
self._old = weighting # Weighting for old reading.
self._high_tick = None
self._period = None
pi.set_mode(gpio, pigpio.INPUT)
self._cb = pi.callback(gpio, pigpio.RISING_EDGE, self._cbf)
pi.set_watchdog(gpio, self._watchdog)
def _cbf(self, gpio, level, tick):
if level == 1: # Rising edge.
if self._high_tick is not None:
t = pigpio.tickDiff(self._high_tick, tick)
if self._period is not None:
self._period = (self._old * self._period) + (self._new * t)
else:
self._period = t
self._high_tick = tick
elif level == 2: # Watchdog timeout.
if self._period is not None:
if self._period < 2000000000:
self._period += (self._watchdog * 1000)
def RPM(self):
"""
Returns the RPM.
"""
RPM = 0.0
if self._period is not None:
RPM = 60000000.0 / (self._period * self.pulses_per_rev)
if RPM < self.min_RPM:
RPM = 0.0
return RPM
def cancel(self):
"""
Cancels the reader and releases resources.
"""
self.pi.set_watchdog(self.gpio, 0) # cancel watchdog
self._cb.cancel()
if __name__ == "__main__":
import time
import pigpio
import read_RPM
RPM_GPIO = 4
RUN_TIME = 60.0
SAMPLE_TIME = 2.0
pi = pigpio.pi()
p = read_RPM.reader(pi, RPM_GPIO)
start = time.time()
while (time.time() - start) < RUN_TIME:
time.sleep(SAMPLE_TIME)
RPM = p.RPM()
print("RPM={}".format(int(RPM+0.5)))
p.cancel()
pi.stop()
I can read the RPMs sent to the GPIOs and, with the appropriate algorithm, convert them to m/s (meter/second) on each wheel.
That is, to find the linear and angular velocities according to the following formulas:
Up to this point, am I wrong somewhere?
Is there a way to calculate the inverse kinematics?
That is, using the following formulas:
Thanks in advance!