def set_velocity(self,pwmotor,motor_id): [pins,dma_ch] = self.motor_list[motor_id] c = [0,0] c[0] = PWM.get_channel_subcycle_time_us(dma_ch[0])/(100.0*PWM.get_pulse_incr_us()) #coefficient to convert duty to period c[1] = PWM.get_channel_subcycle_time_us(dma_ch[1])/(100.0*PWM.get_pulse_incr_us()) #coefficient to convert duty to period try: PWM.clear_channel_gpio(dma_ch[0], pins[0]) PWM.clear_channel_gpio(dma_ch[1], pins[1]) except: pass if pwmotor>0: PWM.add_channel_pulse(dma_ch[1],pins[1],0,0) PWM.add_channel_pulse(dma_ch[0],pins[0],0,int(abs(pwmotor)*c[0])) else: PWM.add_channel_pulse(dma_ch[0],pins[0],0,0) PWM.add_channel_pulse(dma_ch[1],pins[1],0,int(abs(pwmotor)*c[1]))
def set_speed(pin, chan, speed, speed_0):
pulse_inc = PWM.get_pulse_incr_us()
cycle_dur = PWM.get_channel_subcycle_time_us(chan)
num_pulses = int(cycle_dur * speed / pulse_inc)
if speed >= 0.99:
num_pulses -= 1
PWM.add_channel_pulse(chan, pin, 0, num_pulses)
def set_velocity(self, pwmotor, motor_id):
[pins, dma_ch] = self.motor_list[motor_id]
c = [0, 0]
c[0] = PWM.get_channel_subcycle_time_us(
dma_ch[0]) / (100.0 * PWM.get_pulse_incr_us()
) #coefficient to convert duty to period
c[1] = PWM.get_channel_subcycle_time_us(
dma_ch[1]) / (100.0 * PWM.get_pulse_incr_us()
) #coefficient to convert duty to period
try:
PWM.clear_channel_gpio(dma_ch[0], pins[0])
PWM.clear_channel_gpio(dma_ch[1], pins[1])
except:
pass
if pwmotor > 0:
PWM.add_channel_pulse(dma_ch[1], pins[1], 0, 0)
PWM.add_channel_pulse(dma_ch[0], pins[0], 0,
int(abs(pwmotor) * c[0]))
else:
PWM.add_channel_pulse(dma_ch[0], pins[0], 0, 0)
PWM.add_channel_pulse(dma_ch[1], pins[1], 0,
int(abs(pwmotor) * c[1]))
def setW(self, W):
"Checks W% is between limits than sets it"
import RPIO.PWM as PWM
PW = 0
self.__W = W
if self.__W < self.__WMin:
self.__W = self.__WMin
if self.__W > self.__WMax:
self.__W = self.__WMax
#on time[us]/granularity[us]
PW = ((PWM.get_channel_subcycle_time_us(0) * self.__W)/100)/PWM.get_pulse_incr_us()
# Set servo to xxx us
if self.powered:
self.__IO.set_servo(self.__pin, PW)
def setW(self, W):
"Checks W% is between limits than sets it"
import RPIO.PWM as PWM
PW = 0
self.__W = W
if self.__W < self.__WMin:
self.__W = self.__WMin
if self.__W > self.__WMax:
self.__W = self.__WMax
#on time[us]/granularity[us]
PW = ((PWM.get_channel_subcycle_time_us(0) * self.__W) /
100) / PWM.get_pulse_incr_us()
# Set servo to xxx us
if self.powered:
self.__IO.set_servo(self.__pin, PW)
def main():
# Set up Frequency in Hertz
FREQUENCY = 1000
SUBCYCLE_US = ((1 / FREQUENCY) * 1000000)
CHANNEL = 0
# Set duty_cycle 0 -> 100
DUTY_CYCLE = 50
# Set Pin
PINO = PIN.CEL_A_1
# Setup PWM and DMA channel 0
PWM.setup()
PWM.init_channel(channel=CHANNEL, subcycle_time_us=SUBCYCLE_US)
# Test initialization
if not (PWM.is_channel_initialized(CHANNEL)):
print("ERROR: Channel could not be initialized!")
return -1
# Test Frequency
if not (PWM.get_channel_subcycle_time_us(CHANNEL) == SUBCYCLE_US):
print("ERROR: Frequency could not be setted!")
return -1
# Add pwm Pulse
PWM.add_channel_pulse(dma_channel=CHANNEL,
gpio=PINO,
start=0,
width=((SUBCYCLE_US / 10) * (DUTY_CYCLE / 100)))
# fake while
print("Press any key to stop")
input()
# Stop PWM for specific GPIO on channel 0
PWM.clear_channel_gpio(0, PINO)
# Shutdown all PWM and DMA activity
PWM.cleanup()
return 0
def init_pwm(channel, frequency):
# channel_id = 2
# channel: pwm['A'][channel_id], pwm['B']channel_id, pwm['C']channel_id, pwm['D']channel_id
# frequency: Frequency in Hz
subcycle = (1/frequency)*1000000
# if not already initialized by other pwms initialize channeç
if not(PWM.is_channel_initialized(channel)):
PWM.init_channel(channel, subcycle)
# Test initialization
if not(PWM.is_channel_initialized(channel)):
print("ERROR: Channel could not be initialized!")
return -1
# Test Frequency
if not(PWM.get_channel_subcycle_time_us(channel) == subcycle):
print("ERROR: Frequency could not be setted!")
return -1
return 1
if PWM.is_setup():
print ("setup canale ", i, "chiamato")
else:
print ("setup canale ", i, " NON chiamato")
try:
while True:
# LED per vedere se tutto funziona
for t in led_time:
for i in gpio_port:
RPIO.output(i, RPIO.HIGH)
sleep(t)
RPIO.output(i, RPIO.LOW)
sleep(t)
for d in canale_dma:
subcycle_T = PWM.get_channel_subcycle_time_us(d)
incr_T = PWM.get_pulse_incr_us()
print("CANALE ", d, "subcycle_T =", subcycle_T, "--- incr_T =", incr_T)
sleep(2)
# if PWM.is_setup(): #è stato fatto il setup d'inizializzazione
# granularity=granularity*10 #se sì: incremento la granularity
# PWM.setup(granularity,0) #rifaccio il setup
except KeyboardInterrupt:
pass
for i in canale_dma:
PWM.clear_channel(i)
PWM.clear_channel_gpio(i,gpio_port(i))
PWM.cleanup()
print "STOP!"
print "Ciao!"
if PWM.is_setup():
print ("setup canale ", k, "chiamato")
else:
print ("setup canale ", k, " NON chiamato")
try:
while True:
# LED per vedere se tutto funziona
for t in led_time:
for p in gpio_port:
RPIO.output(p, RPIO.HIGH)
sleep(t)
RPIO.output(p, RPIO.LOW)
sleep(t)
for d in canale_dma:
subcycle_T = PWM.get_channel_subcycle_time_us(d)
incr_T = PWM.get_pulse_incr_us()
print("CANALE ", d, "subcycle_T =", subcycle_T, "--- incr_T =", incr_T)
sleep(2)
# if PWM.is_setup(): #è stato fatto il setup d'inizializzazione
# granularity=granularity*10 #se sì: incremento la granularity
# PWM.setup(granularity,0) #rifaccio il setup
except KeyboardInterrupt:
RPIO.setwarnings(False)
for i in canale_dma:
PWM.clear_channel(i)
PWM.clear_channel_gpio(i,gpio_port(i))
PWM.cleanup()
print "STOP!"
print "Ciao!"
