def __init__(self, motor_id, polarity=1):
self.motor_id = motor_id
if polarity < 0:
self.DIRECTION_POLARITY = -1
if self.motor_id:
self.pwm = Pwm(1)
self.pin_a = Gpio(26)
self.pin_b = Gpio(47)
self.encoder = Eqep(2)
else:
self.pwm = Pwm(0)
self.pin_a = Gpio(36)
self.pin_b = Gpio(62)
self.encoder = Eqep(1)
self.pwm.export()
self.pwm.set_period(self.MAX_VOLTAGE_LEVEL * self.___VOLTAGE_FACTOR)
self.pwm.set_duty_cycle(0)
self.pwm.enable()
self.pin_a.export()
self.pin_a.set_direction_out()
self.pin_a.set_low()
self.pin_b.export()
self.pin_b.set_direction_out()
self.pin_b.set_low()
self.set_encoder(0)
self.encoder.enable()
def __init__(self, port, stopDelay=0):
'''
Constructor
@param port: PWM port where the buzzer is connected to
@param stopDelay: Delay in seconds to separate notes
'''
self._stopDelay = stopDelay
self._pwm = Pwm(port)
def __init__(self, port):
'''
Constructor.
Inits servo to neutral value, but a different value can be set after construtor call.
@param port: PWM port the server is connected to
'''
self._pwm = Pwm(port)
self._pwm.setPeriod(Servo.PERIOD)
self._pwm.setDuty(Servo.NEUTRAL_DUTY)
def elaborate(self, platform):
platform.add_resources([
Resource("speaker", 0, Pins("1 2", dir='o', conn=("j", "1")),
Attrs(IO_TYPE="LVCMOS33"))
])
speaker = platform.request("speaker", 0)
led = platform.request("led", 0)
m = Module()
pll = m.submodules.pll = Pll(25e6, 20, 1, reset_less_input=True)
pll.output_domain("eth", 4)
os.environ["NMIGEN_nextpnr_opts"] = "--timing-allow-fail"
eth = m.submodules.eth = DomainRenamer("eth")(LiteEth(
platform.request(
"eth",
0,
dir={io.name: "-"
for io in platform.lookup("eth", 0).ios}),
platform.request("eth_clocks",
0,
dir={
io.name: "-"
for io in platform.lookup("eth_clocks", 0).ios
})))
pwm = m.submodules.pwm = Pwm(0)
m.d.comb += speaker.o[0].eq(pwm.output)
m.d.comb += speaker.o[1].eq(~pwm.output)
m.submodules.clocking_i2s = ClockDebug("eth")
m.submodules.clocking_sync = ClockDebug("sync", reset_less=True)
return m
def test_pwm_output(self):
min = 10000
max = 20000
zero = (min + max)/2.0
period = 100000
p = Pwm("PWM99A", min, max, period) # Use round numbers to make things easier
speed = p.set_speed(0)
self.assertEqual(speed, zero)
self.assertEqual(zero, float(open(self.PWM99A_dir + Pwm.DUTY).read()))
speed = p.set_speed(0.5)
self.assertEqual(speed, (max+zero)/2.0)
self.assertEqual((max+zero)/2.0, float(open(self.PWM99A_dir + Pwm.DUTY).read()))
speed = p.set_speed(-1.0)
self.assertEqual(speed, min)
self.assertEqual(min, float(open(self.PWM99A_dir + Pwm.DUTY).read()))
speed = p.set_speed(1.0)
self.assertEqual(speed, max)
self.assertEqual(max, float(open(self.PWM99A_dir + Pwm.DUTY).read()))
class Buzzer(object):
'''
Makes a buzzer playing notes
'''
def __init__(self, port, stopDelay=0):
'''
Constructor
@param port: PWM port where the buzzer is connected to
@param stopDelay: Delay in seconds to separate notes
'''
self._stopDelay = stopDelay
self._pwm = Pwm(port)
def playNote(self, freq, time):
'''
Plays a frequency sound during a time
@param freq: Frequency of the played sound. If this value is 0 a silence will be performed.
@param time: Time the sound will be played
'''
if freq > 0.0:
self._pwm.setFreq(freq)
self._pwm.setDutyPerc(50)
self._pwm.start()
sleep(time - self._stopDelay)
self._pwm.stop()
sleep(self._stopDelay)
else:
sleep(time)
def cleanup(self):
'''
Frees all using resources
'''
self._pwm.cleanup()
def __init__(self):
self.red = Pwm(self.gpioRed)
self.green = Pwm(self.gpioGreen)
self.blue = Pwm(self.gpioBlue)
# Time between each fade step
self.fadeTickSeconds = 1
def __init__(self, pwm_pin):
self.pwm = Pwm(pwm_pin, 50)
self.direction = 0
self.position = 0 # for stepper motor
class RGB:
gpioRed = 27
gpioGreen = 17
gpioBlue = 22
startValue = 0
startValueRed = startValue
startValueGreen = startValue
startValueBlue = startValue
# -- Fade settings --
# A state variable to determine where we are in the color fade
fadeState = 1
# Number of steps to ad each fade
step = 1
# The maximum value of any color
maxValue = 999
# List of colors to send to the lights
color = {"red": 0, "green": 0, "blue": 0}
def __init__(self):
self.red = Pwm(self.gpioRed)
self.green = Pwm(self.gpioGreen)
self.blue = Pwm(self.gpioBlue)
# Time between each fade step
self.fadeTickSeconds = 1
def setR(self, pulseWidth):
self.red.setPulseWidth(pulseWidth)
def setG(self, pulseWidth):
self.green.setPulseWidth(pulseWidth)
def setB(self, pulseWidth):
self.blue.setPulseWidth(pulseWidth)
def setRGB(self, rgbValues):
try:
self.setR(int(rgbValues["red"]))
except:
print "RGB error: red"
pass
try:
self.setG(int(rgbValues["green"]))
except:
print "RGB error: green"
pass
try:
self.setB(int(rgbValues["blue"]))
except:
print "RGB error: blue"
pass
# Run the fade until it finnishes or is stopped
def fade(self):
# 1. First get a new color depending on fadeState...
# if (self.fadeState == 0): #do nothing!
# Red
if (self.fadeState == 1):
self.color["red"] = self.color["red"] + self.step
if (self.color["red"] > 500):
self.fadeState = 2
# Orange -> white
if (self.fadeState == 2):
# Red from 500 -> 800
self.color["red"] = self.color["red"] + self.step
# Green from 0 -> 300
self.color["green"] = self.color["green"] + self.step
if (self.color["red"] > 800):
self.fadeState = 3
# White
if (self.fadeState == 3):
self.color["red"] = min(self.color["red"] + self.step, self.maxValue)
self.color["green"] = min(self.color["green"] + self.step, self.maxValue)
self.color["blue"] = min(self.color["blue"] + self.step, self.maxValue)
if (self.color["blue"] >= self.maxValue):
self.fadeState = 4
# 2. Then send that color...
self.setR(self.color["red"])
self.setG(self.color["green"])
self.setB(self.color["blue"])
# Final fadeState, clear colors and return to state 0
if (self.fadeState == 4):
self.fadeState = 1
# reset colors for next wakeup
self.clear()
print "Fade ended"
return True
else:
return False
# Function to turn off all colors
def clear(self):
self.color["red"] = 0
self.color["green"] = 0
self.color["blue"] = 0
def __init__(self):
Pyro.core.ObjBase.__init__(self)
self.adc = Adc() #adc.Adc()
self.pwm = Pwm() #pwm.OmapPwm()
self.gpio = Gpio() #gpio.Gpio()
from robot import Robot
from commands import Commands
from input import Input
from pwm import Pwm
import os
def main():
pid = str(os.getpid())
file("/var/run/rotacaster.pid", "w").write(pid)
robot = Robot()
input = Input(robot)
commands = Commands(robot, input)
# keep the daemonising threads alive
while True:
pass
if __name__ == "__main__":
try:
main()
except Exception as e:
print e
pa = Pwm(Robot.MOTOR_A_PWM)
pb = Pwm(Robot.MOTOR_B_PWM)
pc = Pwm(Robot.MOTOR_C_PWM)
pa.set_speed(0.0)
pb.set_speed(0.0)
pc.set_speed(0.0)
print "Exception Thrown, all motors stopped"
class Motor:
MAX_VOLTAGE_LEVEL = 100
MIN_VOLTAGE_LEVEL = 0
ENCODER_RESOLUTION = 1200
___VOLTAGE_FACTOR = 10000
DIRECTION_POLARITY = 1
def __init__(self, motor_id, polarity=1):
self.motor_id = motor_id
if polarity < 0:
self.DIRECTION_POLARITY = -1
if self.motor_id:
self.pwm = Pwm(1)
self.pin_a = Gpio(26)
self.pin_b = Gpio(47)
self.encoder = Eqep(2)
else:
self.pwm = Pwm(0)
self.pin_a = Gpio(36)
self.pin_b = Gpio(62)
self.encoder = Eqep(1)
self.pwm.export()
self.pwm.set_period(self.MAX_VOLTAGE_LEVEL * self.___VOLTAGE_FACTOR)
self.pwm.set_duty_cycle(0)
self.pwm.enable()
self.pin_a.export()
self.pin_a.set_direction_out()
self.pin_a.set_low()
self.pin_b.export()
self.pin_b.set_direction_out()
self.pin_b.set_low()
self.set_encoder(0)
self.encoder.enable()
def close(self):
self.pwm.set_duty_cycle(0)
self.pwm.disable()
self.pwm.unexport()
self.pin_a.unexport()
self.pin_b.unexport()
self.encoder.disable()
def __set_absolute_v_level(self, new_level):
if new_level > self.MAX_VOLTAGE_LEVEL:
new_level = self.MAX_VOLTAGE_LEVEL
elif new_level < 0:
new_level = self.MIN_VOLTAGE_LEVEL
new_level *= self.___VOLTAGE_FACTOR
self.pwm.set_duty_cycle(new_level)
def __get_absolute_v_level(self):
return int(self.pwm.get_duty_cycle() / self.___VOLTAGE_FACTOR)
def __set_direction(self, direction):
direction *= self.DIRECTION_POLARITY
if direction > 0:
self.pin_a.set_low()
self.pin_b.set_high()
elif direction < 0:
self.pin_a.set_high()
self.pin_b.set_low()
else:
self.pin_a.set_low()
self.pin_b.set_low()
def __get_direction(self):
a = int(self.pin_a.get_value())
b = int(self.pin_b.get_value())
return (b - a) * self.DIRECTION_POLARITY
def set_encoder(self, value):
self.encoder.set_position(value)
def get_encoder(self):
return int(self.encoder.get_position())
def get_radians(self):
return self.get_encoder() * 2 * 3.14159265359 / self.ENCODER_RESOLUTION
def set_voltage(self, level):
self.__set_direction(level)
self.__set_absolute_v_level(int(abs(level)))
def get_voltage(self):
return self.__get_absolute_v_level() * self.__get_direction()
class Servo(object):
'''
Servo driver class
'''
PERIOD = 20000000
MIN_DUTY = 1000000
NEUTRAL_DUTY = 1500000
MAX_DUTY = 2000000
def __init__(self, port):
'''
Constructor.
Inits servo to neutral value, but a different value can be set after construtor call.
@param port: PWM port the server is connected to
'''
self._pwm = Pwm(port)
self._pwm.setPeriod(Servo.PERIOD)
self._pwm.setDuty(Servo.NEUTRAL_DUTY)
def start(self):
'''
Starts the servo
'''
self._pwm.start()
def stop(self):
'''
Stops using servo
'''
self._pwm.stop()
def cleanup(self):
'''
Frees used resources
'''
self._pwm.cleanup()
def setValue(self, perc):
'''
Set a custom value
@param perc: Percentage value between 0 and 100
'''
if 0 <= perc <= 100:
duty = Servo.MIN_DUTY + (Servo.MAX_DUTY - Servo.MIN_DUTY) * perc / 100.0
self._pwm.setDuty(duty)
def setNeutral(self):
'''
Set value to neutral
'''
self._pwm.setDuty(Servo.NEUTRAL_DUTY)
def setMin(self):
'''
Set value to minimum
'''
self._pwm.setDuty(Servo.MIN_DUTY)
def setMax(self):
'''
Set value to maximum
'''
self._pwm.setDuty(Servo.MAX_DUTY)
