class PanTilt(object):
def __init__(self, config=PanTiltConfig()):
try:
self.config = config
if (self.config.i2c_mode == 0):
self.sb = open('/dev/servoblaster', 'w')
else:
self.servo16 = PWM(0x40)
except (IOError):
print "*** ERROR ***"
print "Unable to communicate to the servo"
exit()
# Pulse width is specified in 0.01 millisec values
# ie: a pulse value of 150 represents 150*.01 = 1.5 msec, which is center
def pwm(self, channel, pulse):
if (self.config.i2c_mode == 0):
self.sb.write(str(channel) + '=' + str(int(pulse)) + '\n')
self.sb.flush()
else:
print "pwm(chan=", channel, ", pulse=", pulse
pulseLength = 1000000 # 1,000,000 us per second
pulseLength /= 60 # 60 Hz
pulseLength /= 4096 # 12 bits of resolution
pulse *= 10 # convert value to usec
pulse /= pulseLength # calculate channel pulse length
print "servo16(chan=", channel, ", pulse=", pulse
self.servo16.setPWM(channel, 0, int(pulse))
def go(self, pan_angle, tilt_angle, incline_angle):
self.pwm(self.config.tilt_pin, map(tilt_angle, 0, 100, self.config.tilt_down_limit, self.config.tilt_up_limit))
self.pwm(self.config.pan_pin, map(pan_angle, 0, 100, self.config.pan_left_limit, self.config.pan_right_limit))
self.pwm(self.config.incline_pin, map(incline_angle, 0, 100, self.config.incline_left_limit, self.config.incline_right_limit))
def main(argv):
minValue = 0 # Min pulse length out of 4096
maxValue = 4095 # Max pulse length out of 4096
frequency = 1200 # Frequency
color = [0,0,0, 0,0,0, 0,0,0];
try:
opts, args = getopt.getopt(argv,"hr:g:b:f:a:b:c:",["red=","green=","blue=", "frequency="])
except getopt.GetoptError:
print 'setrgb.py -r <red> -g <green> -b <blue>'
sys.exit(2)
for opt, arg in opts:
if opt == '-h':
print 'setrgb.py -r <red> -g <green> -b <blue>'
sys.exit()
elif opt in ("-f", "--frequency"):
frequency = int(arg);
elif opt in ("-a", "--channela"):
color[0] = level(int(arg[:2], 16), minValue, maxValue)
color[2] = level(int(arg[2:4], 16), minValue, maxValue)
color[1] = level(int(arg[4:6], 16), minValue, maxValue)
elif opt in ("-b", "--channelb"):
color[3] = level(int(arg[:2], 16), minValue, maxValue)
color[5] = level(int(arg[2:4], 16), minValue, maxValue)
color[4] = level(int(arg[4:6], 16), minValue, maxValue)
elif opt in ("-c", "--channelc"):
color[6] = level(int(arg[:2], 16), minValue, maxValue)
color[8] = level(int(arg[2:4], 16), minValue, maxValue)
color[7] = level(int(arg[4:6], 16), minValue, maxValue)
elif opt in ("-r", "--red"):
color[0] = level(arg, minValue, maxValue)
color[3] = level(arg, minValue, maxValue)
color[6] = level(arg, minValue, maxValue)
elif opt in ("-g", "--green"):
color[2] = level(arg, minValue, maxValue)
color[5] = level(arg, minValue, maxValue)
color[8] = level(arg, minValue, maxValue)
elif opt in ("-b", "--blue"):
color[1] = level(arg, minValue, maxValue)
color[4] = level(arg, minValue, maxValue)
color[7] = level(arg, minValue, maxValue)
pwm = PWM(0x41, debug=False)
pwm.setPWMFreq(frequency) # Set frequency to 1000 Hz
i = 0;
while i < 9:
print "Setting channel %i to value %i" % (i, color[i]);
pwm.setPWM(i, 0, color[i])
i = i + 1;
def do_POST(self):
# Initialise the PWM device (I2C address of 0x40)
pwm = PWM(0x40, debug=True)
pwm.setPWMFreq(60)
if self.path == '/command':
form = cgi.FieldStorage(fp=self.rfile, headers=self.headers, environ={'REQUEST_METHOD':'POST'})
room = form['room'].value
name = form['name'].value
lcd.lcd_display_string("Open Blinds", 1)
location = float(form['location'].value) #% Open
print time.strftime("[%Y/%m/%d %H:%M:%S] ", time.localtime()) + "Web Command Received."
blinds = self.get_blind_data()
for blind in blinds:
if (blind.get('room') == room or blind.get('name') == name):
servoOpenLocation = float(blind.get('OpenLocation'))
servoCloseLocation = float(blind.get('CloseLocation'))
PWMLocation = int(blind.get('PWM'))
location = 100 - location
servoLocation = int(location * math.fabs(servoOpenLocation - servoCloseLocation) + servoOpenLocation)
pwm.setPWM(PWMLocation, 0, servoLocation)
time.sleep(0.50)
pwm.setPWM(PWMLocation, 0, 000)
#--- Added...may not be needed.
self.send_response(200)
self.send_header('Content-type','text-html')
self.end_headers()
self.wfile.write('Command Received and Processed')
#---
return
return self.do_GET()
class Receiver:
def __init__(self):
self.bus=smbus.SMBus(0)
self.pwm = PWM(0x40, debug=True, bus=self.bus)
self.pwm.setPWMFreq(50)
self.sensor=ColourSensor(self.bus)
def setServoPulse(self, pulse):
self.pwm.setPWM(0, 0, int(pulse * 4096 / 20))
def readSensor(self):
value=self.sensor.read()
print "R%03X G%03X B%03X" % (value['red'], value['green'], value['blue'])
if(value['blue'] <= 0x130) and (value['red'] <= 0x130):
return "Black"
elif(value['blue'] >= 0x155) and (value['red'] >= 0x155):
return "White"
else:
return "None"
def dropMarble(self):
self.setServoPulse(1.9)
sleep(1)
self.setServoPulse(1.6)
sleep(1)
class Motor:
FRONT_LEFT = 5
FRONT_RIGHT = 4
BACK_LEFT = 3
BACK_RIGHT = 6
def __init__(self, pin_n):
#drive Output Enable in PCA low
pin = GPIO.Pin(27)
pin.write(0)
PCA9685_DEFAULT_ADDRESS = 0x40
frequency = 50
self.NAVIO_RCOUTPUT = pin_n
SERVO_MIN_ms = 1.250 # mS
SERVO_MAX_ms = 1.750 # mS
#convert mS to 0-4096 scale:
self.SERVO_MIN = math.trunc((SERVO_MIN_ms * 4096.0) / (1000.0 / frequency) - 1)
self.SERVO_MAX = math.trunc((SERVO_MAX_ms * 4096.0) / (1000.0 / frequency) - 1)
self.pwm = PWM(0x40, debug=False)
# Set frequency to 60 Hz
self.pwm.setPWMFreq(frequency)
def setSpeed(self, speed):
speed = speed if speed > self.SERVO_MIN else self.SERVO_MIN
speed = speed if speed < self.SERVO_MAX else self.SERVO_MAX
#print "Max: {}, Min: {} speed: {}".format(self.SERVO_MAX, self.SERVO_MIN, speed)
self.pwm.setPWM(self.NAVIO_RCOUTPUT, 0, speed)
def run(self):
pwm = PWM(0x40, debug=True)
pwm.setPWMFreq(60) # Set frequency to 60 Hz
while True:
for i in range (0,4):
pwm.setPWM(2*i, 0, motor[i])
time.sleep(0.01)
def setAngle(axe, angle_h): angle = angleToPulse(angle_h) global PWM if axe == True: PWM.setPWM(HORIZONTAL, 0, angle) else: PWM.setPWM(VERTICAL, 0, angle)
class AdafruitPWMOutput(Output):
def __init__(self, address = 0x40, bus = 0):
self.pwm = PWM(address, False, bus)
self.pwm.setPWMFreq(1600)
for i in range(16):
self.pwm.setPWM(i, 0, 0)
super(AdafruitPWMOutput, self).__init__()
def _setChannelValue(self, channel, value):
self.pwm.setPWM(channel, 0, value)
class Servo(object):
"""
Setting the channels, address, startingPulse and frequency to create an servo.
Using the Adafruit PWM servo driver to set the min and max Pulse of the servo.
"""
def __init__(self, address, channel, startingPulse, inverse=False, offset=0):
# Constructor
self.pulse = startingPulse
self.channel = channel
self.address = address
self.freq = 60
self.maxPulse = 600
self.minPulse = 150
self.inverse = inverse
self.offset = offset
try:
self.pwm = PWM(address)
self.pwm.setPWMFreq(self.freq)
self.setPulse(startingPulse)
except:
pass
def setPulse(self, pulse):
pulse + self.offset
# Setting the area of pulses between 150 and 600. If the input is bigger then 600 pulses
# it will be set to the max 600 pulses. If pulses are lower then 150 it will be set tot 150 pulses
if pulse > self.maxPulse:
pulse = self.maxPulse
elif pulse < self.minPulse:
pulse = self.minPulse
if self.inverse == True:
pulse = pulse - 150
#calculate difference
difference = 225 - pulse
pulse = 375 + difference
try:
self.pwm.setPWM(self.channel, 0, pulse) # 0 means: zero pulse away from input pulse.
except:
#print "moved servo " + str(self.address) + " " + str(self.channel) + " to: " + str(pulse)
pass
self.pulse = pulse # setting the input pulse
def getPulse(self):
if self.inverse == True:
pulse = self.pulse - 150
#calculate difference
difference = 225 - pulse
pulse = 375 + difference
return pulse - self.offset
return self.pulse
class PanTiltDriver:
def __init__(self, customaddress=0x40, panchannel=0, tiltchannel=2):
self.Address = customaddress
self.PanChannel = panchannel
self.TiltChannel = tiltchannel
self.PanMin = 104
self.PanMax = 560
self.TiltMin = 104
self.TiltMax = 307
self.PWM = PWM(self.Address)
self.PWM.setPWMFreq(50)
def panTo(self, degrees):
if degrees < 0:
degrees = 0
if degrees > 180:
degrees = 180
ratio = degrees / float(180)
panTicks = self.PanMin + (ratio * (self.PanMax - self.PanMin))
self.PWM.setPWM(self.PanChannel, 0, int(panTicks))
def tiltTo(self, degrees):
"""
Tilts to a specified angle. Not less then 0° and no more than 90.0°
:param degrees: The amount to tilt, expressed in degrees
"""
if degrees < 0:
degrees = 0
if degrees > 90:
degrees = 90
ratio = degrees / float(90)
tiltTicks = self.TiltMin + (ratio * (self.TiltMax - self.TiltMin))
self.PWM.setPWM(self.TiltChannel, 0, int(tiltTicks))
def panLeft(self):
"""Pan completely to the left"""
self.panTo(0)
def panCenter(self):
"""Pan to center position"""
self.panTo(90)
def panRight(self):
"""Pan completely to the right"""
self.panTo(180)
def tiltDown(self):
"""Tilt completely down"""
self.tiltTo(90)
def tiltUp(self):
"""Tilt to horizontal position"""
self.tiltTo(0.0)
class servoHandler: def __init__(self): self.frequency = 50 self.lastAddress = -1 def setServo(self, address, channel, pos): if address != self.lastAddress: self.pwm = PWM(address) # set board address # self.pwm = PWM(address, debug=True) # set board address self.pwm.setPWMFreq(self.frequency) # just to be sure, set frequency self.pwm.setPWM(channel, 0, pos) # Set the servo position time.sleep(0.3) # wait for a bit to allow for positioning
class Servos(object): def __init__(self, i2cAddress, xAxisChannel, yAxisChannel, pwmFreqHz): self.pwm = PWM(i2cAddress, debug=True) self.pwm.setPWMFreq(pwmFreqHz) self.xaxis = xAxisChannel self.yaxis = yAxisChannel def setXAxis(self, value): self.pwm.setPWM(self.xaxis, 0, value) def setYAxis(self, value): self.pwm.setPWM(self.yaxis, 0, value)
class PanTiltDriver:
def __init__(self, customaddress=0x40, panchannel=0, tiltchannel=3):
self.Address = customaddress
self.PanChannel = panchannel
self.TiltChannel = tiltchannel
self.PanMin = 104
self.PanMax = 560
self.TiltMin = 0
self.TiltMax = 0
self.PWM = PWM(self.Address)
self.PWM.setPWMFreq(50)
def panTo(self, degrees):
if degrees < 0:
degrees = 0
if degrees > 180:
degrees = 180
ratio = degrees / float(180)
panTicks = self.PanMin + (ratio * (self.PanMax - self.PanMin))
self.PWM.setPWM(self.PanChannel, 0, int(panTicks))
def tiltTo(self, degrees):
if degrees < 0:
degrees = 0
if degrees > 90:
degrees = 90
ratio = degrees / float(90)
tiltTicks = self.TiltMin + (ratio * (self.TiltMax - self.TiltMin))
self.PWM.setPWM(self.PanChannel, 0, int(tiltTicks))
def panLeft(self):
"""Pan completely to the left"""
self.panTo(0)
def panCenter(self):
"""Pan to center position"""
self.panTo(90)
def panRight(self):
"""Pan completely to the right"""
self.panTo(180)
def tiltDown(self):
"""Tilt completely down"""
self.tiltTo(90)
def tiltUp(self):
"""Tilt to horizontal position"""
self.tiltTo(0)
class ServoMovementController(object):
do_shutdown = False
def __init__(self, controller_addr, servo_id, usec_max=2500, usec_min=500):
self._pwm = PWM(controller_addr, debug=True)
self._pwm.setPWMFreq(60)
self.servo_id = servo_id
self._angle = 0
self._usec_max = usec_max
self._usec_min = usec_min
self.t = threading.Thread(target=self.run, args=())
self.t.start()
def run(self):
while not self.do_shutdown or not movement_queue.empty():
if self.do_shutdown and not movement_queue.empty():
print('Received shutdown command. Waiting for all movements to complete')
movement = movement_queue.get()
self.do_it(movement)
movement_queue.task_done()
print('All movements complete. Shutting down')
def do_it(self, m):
while self._angle != m['target_angle']:
if m['target_angle'] > self._angle:
self._angle += float(m['velocity']/10.0)
if self._angle > m['target_angle']:
self._angle = m['target_angle']
if m['target_angle'] < self._angle:
self._angle -= float(m['velocity']/10.0)
if self._angle < m['target_angle']:
self._angle = m['target_angle']
self._pwm.setPWM(self.servo_id, 0, int(self.angle_to_usec(self._angle)))
time.sleep(1.0/m['velocity'])
def angle_to_usec(self, angle):
angleRange = (100 - 0)
usecRange = ( self._usec_max - self._usec_min )
angle = (((angle - 0) * usecRange) / angleRange) + self._usec_min
return angle/4
def get_cur_angle(self):
return self._angle
def shutdown(self):
self.do_shutdown = True
class Adafruit_MotorHAT:
FORWARD = 1
BACKWARD = 2
BRAKE = 3
RELEASE = 4
LED_ON = 1
LED_OFF = 4
def __init__(self, addr = 0x60, freq = 1600):
self._i2caddr = addr # default addr on HAT
self._frequency = freq # default @1600Hz PWM freq
self.motors = [ Adafruit_DCMotor(self, m) for m in range(2) ] # modified, becausee of 2 LEDs
# for my own two servo motors
self.servoMotors = [ Jangho_Servo(self, m) for m in range(2) ]
# for my own two LEDs
self.leds = [ Jangho_LED(self, m) for m in range(2) ]
self._pwm = PWM(addr, debug=False)
self._pwm.setPWMFreq(self._frequency)
def setPin(self, pin, value):
if (pin < 0) or (pin > 15):
raise NameError('PWM pin must be between 0 and 15 inclusive')
if (value != 0) and (value != 1):
raise NameError('Pin value must be 0 or 1!')
if (value == 0):
self._pwm.setPWM(pin, 0, 4096)
if (value == 1):
self._pwm.setPWM(pin, 4096, 0)
def getLED(self, num):
if (num < 0) or (num >= 2):
raise NameError('MotorHAT LED must be between 0 and 1 inclusive')
else:
return self.leds[num]
def getServoMotor(self, num):
if (num < 0) or (num >= 2):
raise NameError('MotorHAT Servos must be between 0 and 1 inclusive')
else:
return self.servoMotors[num]
def getDCMotor(self, num):
if (num < 0) or (num >= 2):
raise NameError('MotorHAT Motor must be between 0 and 1 inclusive')
return self.motors[num]
class dw_MotorCONTROL:
FORWARD = 1
BACKWARD = 2
BRAKE = 3
RELEASE = 4
SINGLE = 1
DOUBLE = 2
INTERLEAVE = 3
MICROSTEP = 4
ININ = 0
PHASE = 1
def __init__(self, addr = 0x60, freq = 1600):
self._i2caddr = addr # default addr on HAT
self._frequency = freq # default @1600Hz PWM freq
# self.steppers = [ Adafruit_StepperMotor(self, 1), Adafruit_StepperMotor(self, 2) ]
self._pwm = PWM(addr, debug=False)
self._pwm.setPWMFreq(self._frequency)
# Just gonna default to high for now
GPIO.setmode(GPIO.BCM)
GPIO.setwarnings(False)
GPIO.setup(17, GPIO.OUT)
GPIO.output(17, GPIO.HIGH)
self.motors = [ dw_DCMotor(self, m) for m in range(6) ]
def setPin(self, pin, value):
if (pin < 0) or (pin > 15):
raise NameError('PWM pin must be between 0 and 15 inclusive')
if (value != 0) and (value != 1):
raise NameError('Pin value must be 0 or 1!')
if (value == 0):
self._pwm.setPWM(pin, 0, 4096)
if (value == 1):
self._pwm.setPWM(pin, 4096, 0)
def getMotor(self, num):
if (num < 1) or (num > 6):
raise NameError('MotorHAT Motor must be between 1 and 6 inclusive')
return self.motors[num-1]
def allOff(self):
for y in range(5):
self.motors[y].run(dw_MotorCONTROL.RELEASE, 0)
class pca9685:
def __init__(self, name):
self.name = name
self.type = 'pca9685_pwm'
self.channel_config = []
self.i2c_address = None
self.dmx_channel_start = None
self.dmx_channel_end = None
self.handler = self.default_handler
self.servo_min = 150 # Min pulse length out of 4096
self.servo_max = 600 # Max pulse length out of 4096
self.pwm = PWM(0x40, debug=True)
self.pwm.setPWMFreq(60) # Set frequency to 60 Hz
def __str__(self):
ret = "\nName: %s\n" % self.name
ret += "Type: %s\n" % self.type
ret += "I2C Address: %s\n" % self.i2c_address
ret += "DMX Channel Start: %d\n" % self.dmx_channel_start
ret += "DMX Channel End: %d\n" % self.dmx_channel_end
for channel_num, channel_type in enumerate(self.channel_config):
ret += "PWM Channel %d: " % channel_num
ret += "Type: %s \n" % channel_type
return ret
def default_handler(self,data):
print name, " Using default handler"
print "Data:", data
def default_handler(self,data):
print self.name, " Using hardware handler"
for channel_num, channel_type in enumerate(self.channel_config):
if channel_type == 'Dimmer':
if controller.verbose:
print "Channel:", channel_num, "Channel Type: ", channel_type, "Value: ", data[self.dmx_channel_start+channel_num]
self.pwm.setPWM(channel_num, 0, data[self.dmx_channel_start+channel_num]*PCA9685_MAX/DMX_MAX)
if channel_type == 'Servo':
if controller.verbose:
print "Channel:", channel_num, "Channel Type: ", channel_type, "Value: ", data[self.dmx_channel_start+channel_num]
# position is in 0-255 range
# 0degree position = 150; max degree position = 450
servo_max_delta = self.servo_max - self.servo_min
value = self.servo_min + data[self.dmx_channel_start+channel_num] * (servo_max_delta / DMX_MAX)
if controller.verbose:
print "Channel:", channel_num, "Channel Type: ", channel_type, "Value: ", value
self.pwm.setPWM(channel_num, 0, value)
def moveServo(direction):
#This moves a servo up or down.
# Initialise the PWM device using the default address
pwm = PWM(0x40)
# Note if you'd like more debug output you can instead run:
#pwm = PWM(0x40, debug=True)
servoMin = 150 # Min pulse length out of 4096
servoMax = 600 # Max pulse length out of 4096
pwm.setPWMFreq(60) # Set frequency to 60 Hz
if (direction=="up"):
pwm.setPWM(0, 0, servoMax)
else:
pwm.setPWM(0, 0, servoMin)
class Servo:
""" Wrapper interface to a Servo."""
driver_address = 0x40
pwm_freq = 60
# The servo driver takes a 4byte number which divides the 1/F time into
# 4096 quantiles. The start of the signal and the end of the signal are
# expressed as the indices of these quantiles.
driver_resolution = 4096
def __init__(self, driver_channel, init_pos=0, move_delay=0.01):
"""
driver_channel: Provide the channel number (on the driver) of the Servo that this object represents.
init_pos: A number between 0 and 180 that specifies the initial angle
move_delay: parameter in the constructor
"""
self.move_delay = move_delay
if (init_pos < 0 or init_pos > 180):
raise ValueError("Initial position invalid:" + str(init_pos))
self.driver_channel = driver_channel
self.pwm = PWM(Servo.driver_address)
self.pwm.setPWMFreq(Servo.pwm_freq)
self.angle_to_quantile = get_pulse_lengths(Servo.pwm_freq, Servo.driver_resolution)
self.set_angle(init_pos)
def set_angle(self, angle):
if (angle < 0 or angle > 180):
raise ValueError("Destination position invalid:" + str(angle))
self.curr_pos = angle
self.pwm.setPWM(self.driver_channel, 0, self.angle_to_quantile[angle])
# Ensure that dest_angle is an int. Type check does not happen here.
def move_to(self, dest_angle):
if (dest_angle< 0 or dest_angle > 180):
raise ValueError("Destination position invalid:" + str(dest_angle))
direction = 1
if (dest_angle < self.curr_pos):
direction = -1
for angle in range(self.curr_pos, dest_angle, direction):
self.set_angle(angle)
time.sleep(self.move_delay)
self.set_angle(dest_angle)
def get_current_pos(self):
return self.curr_pos
class AdafruitPwmControlledMotors:
def __init__(self, debug_flag):
self.pwmController = PWM(ADAFRUIT_PWM_ADRESS, debug = debug_flag)
self.pwmController.setPWMFreq(ADAFRUIT_PWM_FREQUENCY)
# These values must be chosen so that the ESC reacts to them.
# TODO: maybe this has to change with pwm frequency??
self.servoMin = 150 # min pulse length out of 4096
self.servoMax = 600 # max pulse length out of 4096
# channel: pin on PCA9685 where this motor is connected
# motorSpeed: desired motor speed in [0.0, 1.0]
def _servo_pulse(self, channel, motor_speed):
assert motor_speed >= 0.0
assert motor_speed <= 1.0
# TODO: check formula!
# pulse length in us (1.000.000 per second) per bit
# pulseLength = 1000000 * self.pwmFrequency / 4096
# pwmPulse = pulse * 1000 / pulseLength
pwm_pulse = motor_speed * (self.servoMax - self.servoMin) + self.servoMin
# setPWM(self, channel, on, off)
# on: The tick (between 0..4095) when the signal should transition from low to high
# off: The tick (between 0..4095) when the signal should transition from high to low
# 4096 because of 12 bit resolution
# we always start with ON at time 0
self.pwmController.setPWM(channel, 0, pwm_pulse)
# motorSpeeds: [0.0, 1.0]^4
def setSpeed(self, motor_speeds):
assert len(motor_speeds) == 4
for i in range(0,4):
assert 0.0 <= motor_speeds[i] <= 1.0
self._servo_pulse(PIN_MOTOR_1, motor_speeds[0])
self._servo_pulse(PIN_MOTOR_2, motor_speeds[1])
self._servo_pulse(PIN_MOTOR_3, motor_speeds[2])
self._servo_pulse(PIN_MOTOR_4, motor_speeds[3])
class ServoDriver :
def __init__(self):
# PWM settings
FREQ = 60.0 # Set frequency to 60 Hz
self.pwm = PWM(0x40, debug=False)
self.pwm.setPWMFreq(FREQ)
pulseLength = 1000000 # 1,000,000 us per second
pulseLength /= FREQ # 60 Hz
pulseLength /= 4096 # 12 bits of resolution
self.pulseFactor = 1000 / pulseLength # millisecond multiplier
# Tested Servo range
self.pulsecenter = 1.6
self.pulserange = 0.8 #per adafruit 0.5 = 1.0...1.5...2.0
def move(self, servo, position):
if -1 < position < 1: #movement range -1...0...1
pulse = self.pulsecenter + (position*self.pulserange) # w/in range from middle
pulse *= self.pulseFactor # ~120-600 at 60hz
self.pwm.setPWM(servo, 0, int(pulse))
class Motor(): servoMin = 150 # Min pulse length out of 4096 servoMax = 600 # Max pulse length out of 4096 servoInit = 185 servoMotorStart = 220 pwm = None debug = False def __init__(self, debug=True): self.debug = debug # Initialise the PWM device using the default address self.pwm = PWM(0x40, debug=debug) self.pwm.setPWMFreq(60) # Set frequency to 60 Hz self.reset() def init(self): if self.debug: print "Init...", self.pwm.setPWM(0, 0, self.servoInit) time.sleep(3) if self.debug: print "Done" def reset(self): if self.debug: print "Reset servo (minimum) %d" % self.servoMin self.pwm.setPWM(0, 0, self.servoMin) def set_speed(self, percent): if percent > 1 or percent < 0: if self.debug: print "Invalid value, must be between 0 and 1" servo_pos = int((self.servoMax - self.servoMotorStart) * percent + self.servoMotorStart) #print "Set to %.2f%% (%d)" % (percent, servo_pos) self.pwm.setPWM(0, 0, servo_pos) return servo_pos
class Adafruit_MotorHAT:
FORWARD = 1
BACKWARD = 2
BRAKE = 3
RELEASE = 4
BIPOLAR_BACKWARD = 5
SINGLE = 1
DOUBLE = 2
INTERLEAVE = 3
MICROSTEP = 4
def __init__(self, addr = 0x60, freq = 1600):
self._i2caddr = addr # default addr on HAT
self._frequency = freq # default @1600Hz PWM freq
self.motors = [ Adafruit_DCMotor(self, m) for m in range(4) ]
self.steppers = [ Adafruit_StepperMotor(self, 1), Adafruit_StepperMotor(self, 2) ]
self._pwm = PWM(addr, debug=False)
self._pwm.setPWMFreq(self._frequency)
def setPin(self, pin, value):
if (pin < 0) or (pin > 15):
raise NameError('PWM pin must be between 0 and 15 inclusive')
if (value != 0) and (value != 1):
raise NameError('Pin value must be 0 or 1!')
if (value == 0):
self._pwm.setPWM(pin, 0, 4096)
if (value == 1):
self._pwm.setPWM(pin, 4096, 0)
def getStepper(self, steps, num):
if (num < 1) or (num > 2):
raise NameError('MotorHAT Stepper must be between 1 and 2 inclusive')
return self.steppers[num-1]
def getMotor(self, num):
if (num < 1) or (num > 4):
raise NameError('MotorHAT Motor must be between 1 and 4 inclusive')
return self.motors[num-1]
class Servo(object):
def __init__(self, channel, address, maxAngle=40.0, minAngle=40.0, freq=60):
self.channel = channel
self.address = address
self.freq = freq
self.maxAngle = maxAngle
self.minAngle = minAngle
self.maxmillisec = 2.0
self.minmillisec = 1.0
self.m = (self.minmillisec - self.maxmillisec)/(self.maxAngle - self.minAngle)
self.b = self.maxmillisec - (self.m * self.minAngle)
self.pwm = PWM()
self.pwm.setPWMFreq(self.freq)
self.cycle = 1.0/float(self.freq)
self.timepertick = self.cycle / 4096
def setAngle(self, angle):
self.angle = angle
print self.angle
if self.angle > self.maxAngle:
self.angle = self.maxAngle
elif self.angle < self.minAngle:
self.angle = self.minAngle
self.millisec = self.angle * self.m + self.b
print self.millisec
if self.millisec > self.maxmillisec:
self.millisec = self.maxmillisec
elif self.millisec < self.minmillisec:
self.millisec = self.minmillisec
self.tickOn = (self.millisec/1000.0)/self.timepertick
print self.tickOn
self.pwm.setPWM(self.channel, 0, int(self.tickOn))
def getAngle(self):
return self.angle
class Transmitter:
def __init__(self):
self.bus=smbus.SMBus(1)
self.pwm = PWM(0x40, debug=True, bus=self.bus)
self.pwm.setPWMFreq(50)
self.setServoPulse(1.5)
def setServoPulse(self, pulse):
self.pwm.setPWM(0, 0, int(pulse * 4096 /20))
def send_left(self):
self.setServoPulse(1.1)
sleep(2)
self.setServoPulse(1.5)
sleep(2)
def send_right(self):
self.setServoPulse(1.8)
sleep(2)
self.setServoPulse(1.5)
sleep(2)
class ServoDriver: ## initializes the driver for a server def __init__(self, address = 0x40, channel = 0): self.data = [] self.server_channel = 0 self.freq = 60 self.pwm = PWM(0x40) self.servoMin = 150 self.servoMax = 600 self.pwm.setPWMFreq(self.freq) self.angle = 90 #centered in the beginning self.move_t_bin = 100 #milliseconds ## sets the server to the given angle with 90 being centered def set_angle(self, angle): self.angle = angle self.pwm.setPWM(self.server_channel, 0, self.__angle_to_pulse(self.angle)) ## takes care of using the server's settings to transform the values def __angle_to_pulse(self, angle): assert(angle >= 0 and angle <= 180) anglePercent = angle / 180.0 return int(self.servoMin + (self.servoMax - self.servoMin) * anglePercent) ## smooth turning to a given angle given the angular-velocity def smooth_move(self, angle, angular_v): constrain(angle, 0 , 180) constrain(angular_v, 0, 100) delta_angle = abs(self.angle - angle) t = delta_angle / angular_v * 1000 #milliseconds def __move_delta_angle_t(self, delta_angle, t): steps = t / self.move_t_bin stepSize = delta_angle / steps for step in range(0, steps): self.angle += stepSize self.set_angle(angle) # if necessary reduce processing time by working on the pulse directly time.sleep(self.move_t_bin)
class RC(object):
@staticmethod
def map(value, fromLow, fromHigh, toLow, toHigh):
fromRange = fromHigh - fromLow;
toRange = toHigh - toLow;
return (((value - fromLow) * toRange) / fromRange) + toLow;
def __init__(self, servo_channel, motor_channel):
self.servo_channel = servo_channel;
self.motor_channel = motor_channel;
self.servoMin = 275; # 1ms pulse
self.servoMax = 550; # 2ms pulse
self.speedMin = 275; # 1ms pulse
self.speedMax = 550; # 2ms pulse
self.pwm = PWM(0x40);
self.pwm.setPWMFreq(60);
######################################################################
# Angle is in degrees.
# -50 degrees corresponds to full left for grasshopper
# 50 degrees corresponds to full right for grasshopper
#
def setServoAngle(self, angle):
pulse = self.map(angle, -90, 90, self.servoMin, self.servoMax);
self.pwm.setPWM(self.servo_channel, 0, pulse);
#####################################################################
# Speed is a relative number between -2048 and 2048
# 0 - stop
# 2048 - full speed forward
# -2048 - full spped backwards
#
# depending on surface, low numbers may not move RC car at all
#
def setSpeed(self, speed):
pulse = self.map(speed * -1, -2048, 2048, self.speedMin, self.speedMax);
self.pwm.setPWM(self.motor_channel, 0, pulse);
class UglyRobot(object):
def __init__(self):
self.pwm = PWM(0x40)
self.pwm.setPWMFreq(60)
def setPowerToServo(self, servo, power):
zone = SERVO_ZONES[servo]
if abs(power) < 0.0001:
pwmValue = 0
elif power > 0:
pwmValue = zone[1] - power * (zone[1] - zone[0])
elif power < 0:
pwmValue = zone[2] - power * (zone[3] - zone[2])
self.pwm.setPWM(servo, 0, int(pwmValue))
def setVelocity(self, x, y):
leftPower = (y + x - x * y * (sgn(x) + sgn(y)) / 2)
rightPower = (y - x - x * y * (sgn(x) - sgn(y)) / 2)
leftPower *= leftPower
rightPower *= rightPower
self.setPowerToServo(0, leftPower)
self.setPowerToServo(1, -rightPower)
class AdafruitDriveController(DriveController):
""" Provides drive and steering control abstraction from eg PWM servo or ESC devices. """
# TODO might need tuning or configuring
#servoMin = 150 # Min pulse length out of 4096
#servoMax = 600 # Max pulse length out of 4096
# 'standard' analog servo freq
ic_pwm_freq = 60
def __init__(self, i2c_addr=0x40, i2c_bus=None, prop_channel=0, servo_channel=1, debug=False):
self.debug = debug
self.prop_channel = prop_channel
self.servo_channel = servo_channel
# Initialise the PWM device
from Adafruit_PWM_Servo_Driver import PWM
self._pwm = PWM(i2c_addr, i2c_bus=i2c_bus, debug=debug)
self._pwm.setPWMFreq(self.ic_pwm_freq)
# Set initial positions to centre
self.set_servo_pulse(self.prop_channel, 1.5)
self.set_servo_pulse(self.servo_channel, 1.5)
def set_servo_pulse(self, channel, pulse):
""" 0.001 is ~1ms pulse so standard servo would be in range 1ms <- 1.5ms -> 2/0ms """
pulseLength = 1000000 # 1,000,000 us per second
pulseLength /= self.ic_pwm_freq # 60 Hz
if (self.debug):
logging.debug("DRIVE:\t%d us per period", pulseLength)
pulseLength /= 4096 # 12 bits of resolution
if (self.debug):
logging.debug("DRIVE:\t%d us per bit", pulseLength)
pulse *= 1000
pulse /= pulseLength
if (self.debug):
logging.debug("DRIVE:\t%d pulse sent", pulse)
self._pwm.setPWM(channel, 0, int(pulse))
class Motor(): servoMin = 150 # Min pulse length out of 4096 servoMax = 600 # Max pulse length out of 4096 servoInit = 185 servoMotorStart = 220 speed_percent = 0 asked_speed_percent = 0 position = 0 pwm = None channel = None debug = False def __init__(self, channel, debug=True): self.channel = channel self.debug = debug # Initialise the PWM device using the default address self.pwm = PWM(0x40, debug=debug) self.pwm.setPWMFreq(60) # Set frequency to 60 Hz self.reset() def init(self): if self.debug: print "Init...", self.pwm.setPWM(self.channel, 0, self.servoInit) time.sleep(3) if self.debug: print "Done" return self def reset(self): if self.debug: print "Reset servo (minimum) %d" % self.servoMin self.pwm.setPWM(self.channel, 0, self.servoMin) def set_speed(self, percent, min=0, max=1): self.asked_speed_percent = percent if percent > 1 or percent < 0: if self.debug: print "Invalid value, must be between 0 and 1" if percent > max: percent = max if percent < min: percent = min servo_pos = int((self.servoMax - self.servoMotorStart) * percent + self.servoMotorStart) #print "Set to %.2f%% (%d)" % (percent, servo_pos) self.pwm.setPWM(self.channel, 0, servo_pos) self.position = servo_pos self.speed_percent = percent return servo_pos def adjust_speed(self, diff_speed): self.speed_percent = self.speed_percent + diff_speed self.set_speed(self.speed_percent/100.0, 0.1, 0.10)
class MuleBot:
""" Class MuleBot
This class accepts driving commands from the keyboard and it also has
a target mode where it drives to the target."""
WHEEL_RADIUS = 2
# Apparently, in the robot world, the distrance between the two motor
# driven wheels is called wheel base length.
WHEEL_BASE_LENGTH = 20
SECONDS_PER_MINUTE = 60
MAX_RPM = 12
RADIANS_IN_CIRCLE = 2.0
MAX_Radians_PM = RADIANS_IN_CIRCLE * MAX_RPM
# MAX_RPS = Maximum Radians Per Second
MAX_RPS = MAX_Radians_PM / SECONDS_PER_MINUTE
INCHES_PER_METER = 39.3701
CIRCUM_IN = RADIANS_IN_CIRCLE * math.pi * WHEEL_RADIUS
CIRCUM_M = CIRCUM_IN / INCHES_PER_METER
dcMotorPWMDurationLeft = 0
dcMotorPWMDurationRight = 0
def __init__(self):
"""
__init__ initializes class variables.
"""
global GPIO
# running is used to control thread execution.
self._running = True
# Keep MuleBot parallel to the wall at this distance.
self.distanceToWall = 0
self.pwmEnablePin = 23 # Broadcom pin 23 was 16
self.motor1DirectionPin = 24 # Broadcom pin 24 was 20
self.motor2DirectionPin = 25 # Broadcom pin 25 was 21
self.motorForward = GPIO.HIGH
self.motorReverse = GPIO.LOW
self.dcMotorLeftMotor = 0
self.dcMotorRightMotor = 1
self.laserDetectLeftPin = 6
self.laserDetectRightPin = 5
self.motorMaxRPM = 12.0
self.motorMaxRadiansPM = 2 * self.motorMaxRPM
# Pin Setup:
GPIO.setwarnings(False)
GPIO.setmode(GPIO.BCM) # Broadcom pin-numbering scheme
GPIO.setup(self.pwmEnablePin, GPIO.OUT)
GPIO.setup(self.motor1DirectionPin, GPIO.OUT)
GPIO.setup(self.motor2DirectionPin, GPIO.OUT)
GPIO.output(self.pwmEnablePin, GPIO.LOW)
# This is interupts setups. They get used with the
# test() method.
#GPIO.setup(laserDetectLeftPin, GPIO.IN, pull_up_down=GPIO.PUD_UP)
#GPIO.setup(laserDetectRightPin, GPIO.IN, pull_up_down=GPIO.PUD_UP)
#GPIO.add_event_detect(laserDetectLeftPin, GPIO.FALLING, callback=myInt)
#GPIO.add_event_detect(laserDetectRightPin, GPIO.FALLING, callback=myInt)
# Initialise the PWM device using the default address
self.pwm = PWM(0x40)
# Note if you'd like more debug output you can instead run:
#pwm = PWM(0x40, debug=True)
#count = 1
self.pwm.setPWMFreq(1000) # Set frequency to 1000 Hz
self.tgt_min_range = 15
def terminate(self):
"""terminate is for stopping the thread."""
self._running = False
def mps_to_rps(self, mps):
"""
mps_to_rps transforms meters per second to radians per second.
@type: float
@param: mps (meters per second)
@rtype: float
@param: rps (radians per second)
"""
rps = mps * 2 / MuleBot.CIRCUM_M
return rps
def rps_to_mps(self, rps):
"""
rps_to_mps transforms radians per second to meters per second.
If rps = 2.0, then mps should equal MuleBot.CIRCUM_M because there are
2.0 radians in a circle.
@type: float
@param: rps (radians per second)
@rtype: float
@param: mps (meters per second)
"""
mps = rps * MuleBot.CIRCUM_M / 2
return mps
def rps_to_rpm(self, rps):
"""
rps_to_rpm transforms radians per second to RPM.
@type: float
@param: rps (radians per second)
@rtype: float
@param: rpm
"""
rpm = rps * MuleBot.SECONDS_PER_MINUTE / MuleBot.RADIANS_IN_CIRCLE
return rpm
def rpm_to_rps(self, rpm):
"""
rpm_to_rps transforms RPM to radians per second.
@type: float
@param: rpm
@rtype: float
@param: rps
"""
rps = rpm / MuleBot.SECONDS_PER_MINUTE * MuleBot.RADIANS_IN_CIRCLE
return rps
def rpm_to_mps(self, rpm):
"""
rpm_to_mps transforms RPM to meters per second.
@type: float
@param: rpm
@rtype: float
@param: mps
"""
mps = rpm / 60 * MuleBot.CIRCUM_M
return mps
def v(self):
"""v returns the velocity in meters per second."""
# TODO This translation formula works, but needs simplified.
# PWM duration can go from 0 to 4095 with 4095 representing max rpm
# print("MuleBot.v MuleBot.dcMotorPWMDurationLeft:", MuleBot.dcMotorPWMDurationLeft)
speed_percentage = float(MuleBot.dcMotorPWMDurationLeft) / 4095.0
# print("speed_percentage: ", speed_percentage)
rpm = speed_percentage * self.motorMaxRPM
# print("rpm: ", rpm)
secondsPerMinute = 60
revs_per_second = rpm / secondsPerMinute
# print("--revs_per_second", revs_per_second)
inches_per_rev = 2.0 * math.pi * MuleBot.WHEEL_RADIUS
INCHES_PER_METER = 39.3701
meters_per_rev = inches_per_rev / INCHES_PER_METER
# print("--meters_per_rev", meters_per_rev)
meters_per_second = meters_per_rev * revs_per_second
# print("--meters_per_second: ", meters_per_second)
return meters_per_second
# I don't think setServoPulse is ever called.
# Is the pulse parameter ever used?
#servoMin = 4096 / 12 # Min pulse length out of 4096
#servoMax = 4095 # Max pulse length out of 4096
def setServoPulse(channel, pulse):
"""setServoPulse"""
pulseLength = 1000000 # 1,000,000 us per second
pulseLength /= 60 # 60 Hz
print("%d us per period" % pulseLength)
pulseLength /= 4096 # 12 bits of resolution
print("%d us per bit" % pulseLength)
pulse *= 1000
pulse /= pulseLength
self.pwm.setPWM(channel, 0, pulse)
def set_wheel_drive_rates(self, v_l, v_r):
#TODO Update this to handle nevative velocities.
""" set_wheel_drive_rates set the drive rates of the wheels to the
specified velocities (rads/s). The velocities are converted to RPM.
@type v_l: float
@param v_l: velocity left wheel (rads/s)
@type v_r: float
@param v_r: velocity right wheel (rads/s)
"""
# convert to rpm
# print(">>v_l: ", v_l)
# print(">>v_r: ", v_r)
rpm_l = self.rps_to_rpm(v_l)
rpm_r = self.rps_to_rpm(v_r)
# print(">>rpm_l: ", rpm_l)
# print(">>rpm_r: ", rpm_r)
self.motorSpeed(rpm_l, rpm_r)
return rpm_l, rpm_r
def forward(self, inches):
revolutions = inches / MuleBot.CIRCUM_IN
rpm = MuleBot.MAX_RPM
minutes = revolutions / rpm
seconds = minutes * MuleBot.SECONDS_PER_MINUTE
v = self.rpm_to_rps(rpm)
self.set_wheel_drive_rates(v, v)
time.sleep(seconds)
self.stop()
def stop(self):
v_l = 0
v_r = 0
self.set_wheel_drive_rates(v_l, v_r)
def u_turn(self, direction, diameter_in):
"""u_turn performs an 180 turn either to the 'left' or right based
on the diameter of the turn in inches.
@type: string
@param: direction
@type: float
@type: diameter_in
"""
# pdb.set_trace()
# Calculate radius of turn for the inside wheel.
r_in = diameter_in / 2
# Outside radius is 20 inches from inside radius.
r_out = r_in + MuleBot.WHEEL_BASE_LENGTH
# Outside travel distance
travel = r_out * 3.14159
travel_revolutions = travel / MuleBot.CIRCUM_IN
r_ratio = r_out / r_in
#r_ratio_half = r_ratio / 2
speed_multiplier = MuleBot.MAX_RPM / r_ratio
outside_rpm = r_ratio * speed_multiplier
inside_rpm = speed_multiplier
#
# minutes at outside_rpm
minutes = travel_revolutions / outside_rpm
seconds = minutes * MuleBot.SECONDS_PER_MINUTE
# Something isn't quite perfect.
if direction == 'left':
if diameter_in < 25:
seconds -= 1
else:
seconds -= 2
else:
if diameter_in < 25:
seconds += 1
else:
seconds += 2
if direction == 'left':
v_l = self.rpm_to_rps(inside_rpm)
v_r = self.rpm_to_rps(outside_rpm)
else:
v_r = self.rpm_to_rps(inside_rpm)
v_l = self.rpm_to_rps(outside_rpm)
#print("2inside: rpm: ", inside_rpm)
#print("2outside: rpm: ", outside_rpm)
#print("2.1: v_l: ", v_l)
#print("2.1: v_r: ", v_r)
# Set wheel drive rates.
self.set_wheel_drive_rates(v_l, v_r)
# Sleep during the turn.
time.sleep(seconds)
# Stop
self.stop()
# Move forward 24 inches.
self.forward(24)
def u_turn_supervisor(self, command):
__doc__ = """u_turn_supervisor parses u_turn commands and then
calls u_turn.
Examples: 'ul10' makes a left-handed u_turn with a 10" diameter.
'ur40' makes a right-handed u_turn with a 40" diameter.
@type: string
@param: command"""
# strip the initial 'u'
command = command[1:]
if len(command) > 0:
if command[0] == 'l':
direction = 'left'
else:
direction = 'right'
# strip the direction
command = command[1:]
if len(command) > 0:
diameter = int(command)
self.u_turn(direction, diameter)
def _uni_to_diff(self, v, omega):
"""
_uni_to_diff The is a "unicycle model". It performs a unicycle to
"differential drive model" mathematical translation.
NB: The input/output variable are in DIFFERENT units! This is because
the units of R are (meters/radian) does the conversion.
This came from the 'Sobot Rimulator' by Nick McCrea.
@type v: float
@param v: velocity (m/s)
@type omega: float
@param omega: angular velocity (rads/s)
@rtype: float
@return: v_l velocity left wheel (rads/s)
@rtype: float
@return: v_r velocity right wheel (rads/s)
"""
# print("--MuleBot._uni_to_diff({:.3f}, {:.3f})".format(v, omega))
loggerMB.debug("--MuleBot._uni_to_diff({:.3f}, {:.3f})".format(
v, omega))
# v = translation velocity (m/s)
# omega = angular velocity (rad/s)
# For some reason, it is necessary to multiply the angle by -1.
# TODO: Probably have to put this back in.
omega *= -1.0
inches_per_meter = 39.3701
circumference_in = 2.0 * math.pi * MuleBot.WHEEL_RADIUS
circumference_m = circumference_in / inches_per_meter
radians_per_circumference = 2.0
# R = roll?(meters/radian)
R = circumference_m / radians_per_circumference
# Get info in inches
Lin = MuleBot.WHEEL_BASE_LENGTH
# Convert inches to meters
Lm = Lin / inches_per_meter
# All measurements are now metric.
v_l = ((2.0 * v) - (omega * Lm)) / (2.0 * R)
v_r = ((2.0 * v) + (omega * Lm)) / (2.0 * R)
loggerMB.debug(
"--MuleBot._uni_to_diff v_l, v_r: {:.3f}, {:.3f}".format(v_l, v_r))
rpm_l = self.rps_to_rpm(v_l)
rpm_r = self.rps_to_rpm(v_r)
# print("--MuleBot._uni_to_diff rpm_l, rpm_r: {:.3f}, {:.3f}".format(rpm_l, rpm_r))
loggerMB.debug(
"--MuleBot._uni_to_diff rpm_l, rpm_r: {:.3f}, {:.3f}".format(
rpm_l, rpm_r))
return v_l, v_r
def motorDirection(self, motorPin, direction):
"""
motorDirection sets the direction of a single motor.
Keyword arguments:
motorPin -- Integer representing the direction pin for a specific motor.
direction -- Single bit representing fowards or backwards.
Usage:
self.motorDirection(self.motor1DirectionPin, self.motorReverse)
"""
# print "motorPin: ", motorPin
# print "direction: ", direction
GPIO.output(motorPin, direction)
def motorsDirection(self, direction):
"""
motorsDirection sets the direction of both motors to the same direction.
Keyword arguments:
direction -- single character
"""
print(direction)
if direction == 'r' or direction == 'R':
self.motorDirection(self.motor1DirectionPin, self.motorReverse)
self.motorDirection(self.motor2DirectionPin, self.motorReverse)
print("Direction reverse")
else:
self.motorDirection(self.motor1DirectionPin, self.motorForward)
self.motorDirection(self.motor2DirectionPin, self.motorForward)
print("Direction forward")
def dcMotorLeftTurn(self, duration):
"""dcMotorLeftTurn"""
print("From dcMotorLeftTurn: ", MuleBot.dcMotorPWMDurationLeft)
tempPWMDurationLeft = int(MuleBot.dcMotorPWMDurationLeft * 70 /
100) # 98
self.pwm.setPWM(self.dcMotorLeftMotor, 0, tempPWMDurationLeft)
# Duration of the turn
time.sleep(duration)
# Go straight
self.pwm.setPWM(self.dcMotorLeftMotor, 0,
MuleBot.dcMotorPWMDurationLeft)
def dcMotorRightTurn(self, duration):
"""dcMotorRightTurn"""
tempPWMDurationRight = int(MuleBot.dcMotorPWMDurationRight * 70 / 100)
self.pwm.setPWM(self.dcMotorRightMotor, 0, tempPWMDurationRight)
# Duration of the turn
time.sleep(duration)
# Go straight
self.pwm.setPWM(self.dcMotorRightMotor, 0,
MuleBot.dcMotorPWMDurationRight)
def constrainSpeed(self, speedRPM):
"""constrainSpeed ensures 0 <= speedRPM <= max.
@type speedRPM: float
@param speedRPM: wheel speedRPM (rpm)
@rtype: float
@return: constrained wheel speed (rpm)
"""
if speedRPM > self.motorMaxRPM:
speedRPM = self.motorMaxRPM
if speedRPM < 0.0:
speedRPM = 0.0
# print ( "motorSpeed RPM adjusted: ", speedRPM )
return speedRPM
def motors__Direction(self, speed_l, speed_r):
"""motorDirection sets the direction of the motors based on the sign of
the speed.
@type: float
@param: speed_l
@type: float
@param: speed_r
"""
if speed_l >= 0:
self.motorDirection(self.motor1DirectionPin, self.motorForward)
else:
self.motorDirection(self.motor1DirectionPin, self.motorReverse)
if speed_r >= 0:
self.motorDirection(self.motor2DirectionPin, self.motorForward)
else:
self.motorDirection(self.motor2DirectionPin, self.motorReverse)
def motorSpeed(self, speedRPM_l, speedRPM_r):
"""motorSpeed sets the speed of the motors to the supplied rpm. This has
been updated to handle negative speeds.
@type: float
@param: speedRPM_l (rpm)
@type: float
@param: speedRPM_r (rpm)
"""
self.motors__Direction(speedRPM_l, speedRPM_r)
speedRPM_l = abs(speedRPM_l)
speedRPM_r = abs(speedRPM_r)
speedRPM_l = self.constrainSpeed(speedRPM_l)
speedRPM_r = self.constrainSpeed(speedRPM_r)
# Left motor
pwmDuration = 4095.0 * speedRPM_l / self.motorMaxRPM
# print("MuleBot.motorSpeed Duration left float: ", pwmDuration)
pwmDuration = int(pwmDuration)
# print("MuleBot.motorSpeed Duration left int: ", pwmDuration)
startOfPulse = 0
self.pwm.setPWM(self.dcMotorLeftMotor, startOfPulse, pwmDuration)
MuleBot.dcMotorPWMDurationLeft = pwmDuration
# Right motor
#Adjust for right motor being faster
pwmDuration = 4095.0 * speedRPM_r / self.motorMaxRPM
pwmDuration = pwmDuration * 9727 / 10000 # 98.519113 percent
pwmDuration = int(pwmDuration)
# print("MuleBot.motorSpeed Duration right int: ", pwmDuration)
startOfPulse = 0
self.pwm.setPWM(self.dcMotorRightMotor, startOfPulse, pwmDuration)
MuleBot.dcMotorPWMDurationRight = pwmDuration
def init(self):
"""init"""
junk = 0
# This is all interupt stuff for calibrating the speed
# of the wheels.
#self.interruptLeftCount = -2
#self.interruptRightCount = -2
#self.startTimeLeft = 0
#self.startTimeRight = 0
#self.lastTimeLeft = 0
#self.lastTimeRight = 0
def run1(self, _q1, _q2, _qWallDistance):
"""run1 is used to navigate the MuleBot to
a desired distance from the wall.
This method is a thread.
_q1 is the current distance to the wall.
_qWallDistance is used occasionally to establish
the desire distance.
_q2 is used to send steering directions to the run2 thread."""
timeInRightTurn = 0
timeInLeftTurn = 0
while self._running:
#name = threading.currentThread().getName()
#print "Consumer thread 1: ", name
# This method is the only consumer of _qWallDistance.
# Therefore checking if the queue is empty works.
# In a multi-consumer environment, check empty()
# can cause a race condition.
if _qWallDistance.empty():
pass
else:
self.distanceToWall = _qWallDistance.get()
_qWallDistance.task_done()
currentDistance = _q1.get()
print("Current distance: ", currentDistance)
qSize = _q1.qsize()
if qSize > 1:
print("***** Distance Queue Size: ", qSize, " *****")
# Are we navigating?
navigating = (self.distanceToWall > 0)
if navigating:
print("Desired distance: ", self.distanceToWall)
accuracy = 0.5
# Navigate
if currentDistance < self.distanceToWall - accuracy:
print("Turn right >>>")
timeInRightTurn += 1
_q2.put('s1')
elif currentDistance > self.distanceToWall + accuracy:
print("Turn left <<<")
timeInLeftTurn += 1
_q2.put('p1')
else:
if (timeInRightTurn > 0):
for i in range(timeInRightTurn):
_q2.put('p1')
# Reset the time
timeInRightTurn = 0
if (timeInLeftTurn > 0):
for i in range(timeInLeftTurn):
_q2.put('s1')
# Reset the time
timeInLeftTurn = 0
print("On path.")
# end if
_q1.task_done()
def lidarNav_queue_check(self, q_lidar_nav, tgt_range, tgt_width):
target_range = tgt_range
target_width = tgt_width
# The leading 'n' has been stripped of in the run2 thread.
if not q_lidar_nav.empty():
command = q_lidar_nav.get()
command = command.lower()
first_char = 0
if command[first_char] == 'r':
target_range = float(command[1:])
if command[first_char] == 'w':
target_width = float(command[1:])
return target_range, target_width
def lidarNav_should_i_stay_or_should_i_go(self, tgt_range, angle):
"""lidarNav_should_i_stay_or_should_i_go will stay/stop if MuleBot is
too close to the target. Otherwise, it will go/continue.
@type tgt_range: float
@param : (inches)
@type angle: float
@param : (degrees)
@rtype: float
@return: target_range (inches)
@rtype: float
@return: angle (radians)
"""
# Stop if we are too close to the target
if tgt_range < self.tgt_min_range:
v_l = 0
v_r = 0
self.set_wheel_drive_rates(v_l, v_r)
# setting the range to zero will stop navigating.
target_range = 0
angle_rad = None
else:
# Use the updated range for the next run.
target_range = tgt_range
# Turn based on the angle to target.
# Positive angles are left.
# Negative angles are right.
# Convert from degrees to radians.
angle_rad = math.radians(angle)
return target_range, angle_rad
def velocity_check(self, v_l, v_r):
"""velocity_check slows down the velocities of the two wheels to stay
between +-MAX_RPS.
@type: float
@param: v_l (radians per second)
@type: float
@param: v_r (radians per second)
@rtype: float
@param: v_l_prime (radians per second)
@rtype: float
@param: v_r_prime (radians per second)
@rtype: float
@param: turn_duration (seconds)
"""
if v_l == v_r:
turn_duration = 0
return v_l, v_r, turn_duration
# Assumption: The robot is going forward or is stationary when it is
# hunting for the target, i.e., v >= 0.
# Assumption: It is not known which direction the robot needs to turn.
# Fact: The return values of the _uni_to_diff method are symetrical
# about v.
# Conclusion: Therefore it is only necessary to consider the larger of
# v_l and v_r to determine the turn duration due to the RPM
# limit of the motors.
vel_max = max(v_l, v_r)
if vel_max < MuleBot.MAX_RPS:
turn_duration = 1
return v_l, v_r, turn_duration
# pdb.set_trace()
turn_duration = vel_max / MuleBot.MAX_RPS
v = self.v()
# If v_? > v, v - v_? is negative. If you subract a negative, then
# it becomes an addition. So, it ends up on the correct side of
# velocity.
v_l_prime = v - (v - v_l) / turn_duration
v_r_prime = v - (v - v_r) / turn_duration
return v_l_prime, v_r_prime, turn_duration
def lidarNav_turn(self, angle_rad):
"""lidarNav_turn performs a turn based on an angle.
@type: float
@param: angle_rad
The return values are used for testing.
@rtype: float
@param: v_l (radians per second)
@rtype: float
@param: v_r (radians per second)
@rtype: float
@param: turn_duration (seconds)
"""
# print("--MuleBot.lidarNav_turn({:.4f}(rad))".format(angle_rad))
# pdb.set_trace()
# What is our current velocity (m/s)
v = self.v()
rpm = self.rps_to_rpm(self.mps_to_rps(v))
# print("1: rpm: ", rpm)
# Navigate per the angle.
omega = angle_rad
# v_l and v_r are in radians per second
v_l, v_r = self._uni_to_diff(v, omega)
rpm = self.rps_to_rpm(v_l)
# print("2l: rpm: ", rpm)
rpm = self.rps_to_rpm(v_r)
# print("2r: rpm: ", rpm)
# pdb.set_trace()
v_l, v_r, turn_duration = self.velocity_check(v_l, v_r)
rpm = self.rps_to_rpm(v_l)
# print("3l: rpm: ", rpm)
rpm = self.rps_to_rpm(v_r)
# print("3r: rpm: ", rpm)
self.set_wheel_drive_rates(v_l, v_r)
# Sleep during the turn
time.sleep(turn_duration)
# Drive straight
omega = 0 # zero is no turn
# Something is wrong with _uni_to_diff.
v_l, v_r = self._uni_to_diff(v, omega)
# Override v_l and v_r
v_l = self.mps_to_rps(v)
v_r = self.mps_to_rps(v)
rpm = self.rps_to_rpm(v_l)
# print("4l: rpm: ", rpm)
rpm = self.rps_to_rpm(v_r)
# print("4r: rpm: ", rpm)
self.set_wheel_drive_rates(v_l, v_r)
return v_l, v_r, turn_duration
def lidarNav(self, _q2, q_lidar_nav, q_water_pump):
"""lidarNav is used to navigate the MuleBot to
an object.
This method is a thread.
_q2 is used to send steering directions to the run2 thread.
q_lidar_nav receives target range and width information."""
# Create the RangeBot instance.
servo_channel = 3
range_bot = RangeBot(servo_channel)
UPDATE_PERIOD = .2
MINIMUM_MANUEVER_RANGE = 28
target_range = 0
target_width = 0
navigating = False
was_navigating = False
loggerMB.info('lidarNav before while loop.')
while self._running:
v = self.v()
rpm = self.rps_to_rpm(self.mps_to_rps(v))
# print("---1: rpm: ", rpm)
loggerMB.info('lidarNav before lidarNav_queue_check.')
target_range, target_width = \
self.lidarNav_queue_check(q_lidar_nav, target_range, target_width)
loggerMB.info('lidarNav after lidarNav_queue_check.')
v = self.v()
rpm = self.rps_to_rpm(self.mps_to_rps(v))
# print("---2: rpm: ", rpm)
# Are we navigating?
navigating = target_range > 0 and target_width > 0
loggerMB.debug('lidarNav navigating = {}.'.format(navigating))
if navigating and not was_navigating:
q_water_pump.put('won')
was_navigating = True
if navigating:
# v = self.v()
# print("aMuleBot.lidarNav: v (m/s): ", v)
# print("bMuleBot.lidarNav: target_range: ", target_range)
# print("cMuleBot.lidarNav: target_width: ", target_width)
loggerMB.debug('lidarNav before execute_hunt.')
loggerMB.debug(
'lidarNav before execute_hunt. range: {}, width: {}'.
format(target_range, target_width))
angle, tgt_range, hits = \
range_bot.execute_hunt(target_range, target_width)
loggerMB.debug('lidarNav after execute_hunt.')
# v = self.v()
# print("dMuleBot.lidarNav: v (m/s): ", v)
# print("eMuleBot.lidarNav: angle (deg): ", angle)
# print("fMuleBot.lidarNav: tgt_range (inches): ", tgt_range)
if True:
loggerMB.debug(
'lidarNav before lidarNav_should_i_stay_or_should_i_go.'
)
target_range, angle_rad = \
self.lidarNav_should_i_stay_or_should_i_go(tgt_range, angle)
loggerMB.debug(
'lidarNav after lidarNav_should_i_stay_or_should_i_go.'
)
# v = self.v()
# print("gMuleBot.lidarNav: v (m/s): ", v)
# print("hMuleBot.lidarNav: target_range: ", target_range)
# print("iMuleBot.lidarNav: angle_rad: ", angle_rad)
# input("Press [Enter] to continue.")
if target_range == 0:
q_water_pump.put('woff')
navigating = False
was_navigating = False
# Is a turn required?
if target_range > 0 and not (angle_rad == 0):
# Only make turns if target > inches away.
if target_range > MINIMUM_MANUEVER_RANGE:
# A turn is required.
loggerMB.debug('lidarNav before lidarNav_turn.')
self.lidarNav_turn(angle_rad)
# end target range > 0
# end if navigating
time.sleep(UPDATE_PERIOD)
loggerMB.info('lidarNav after while loop.')
def intFromStr(self, _string, _index):
"""intFromStr extract an integer from a string."""
list = re.findall(r'\d+', _string)
return int(list[_index])
def run2(self, _q2, _qWallDistance, q_lidar_nav, q_water_pump):
""" run2 is a thread
It is processing commands from the keyboard
_q2 is a command queue
_qWallDistance is the ideal distance from the wall
q_lidar_nav is target range and width pairs"""
while self._running:
# name = threading.currentThread().getName()
# print ("Consumer thread 2: ", name)
qCommand = _q2.get()
# print ("Here is the command... ", qCommand)
# print
qSize = _q2.qsize()
if qSize > 1:
print("***** Command Queue Size: ", qSize, " *****")
# Change the command to lowercase
qCommand = qCommand.lower()
cmd = qCommand
command = cmd[0]
if command == 'h':
pass
elif command == 'p':
index = 0
count = self.intFromStr(cmd, index)
print("Left Turn, ", count, " seconds")
self.dcMotorLeftTurn(count)
elif command == 's':
index = 0
count = self.intFromStr(cmd, index)
print("Right Turn, ", count, " seconds")
self.dcMotorRightTurn(count)
elif command == 't':
self.test()
elif command == 'u':
# U-turn command
self.u_turn_supervisor(cmd)
elif command == 'w':
q_water_pump.put(cmd)
# n is for navigating using Lidar.
elif command == 'n':
if len(cmd) >= 3:
if cmd[1] == 'r':
# get range to target
target_range = cmd[2:]
target_range = int(target_range)
# print("Target range: ", target_range)
q_lidar_nav.put('r' + str(target_range))
if cmd[1] == 'w':
# get width of target
target_width = cmd[2:]
target_width = int(target_width)
# print("Target width: ", target_width)
q_lidar_nav.put('w' + str(target_width))
# end if
elif command == 'z':
self.setMotorsDirection('f')
# Get speeds (m/m)
speeds = cmd[1:]
comma_index = speeds.find(',')
vmm_l = speeds[0:comma_index]
vmm_r = speeds[comma_index + 1:]
print("vmm_l: ", vmm_l)
print("vmm_r: ", vmm_r)
# Convert from meters per minute to meters per second
v_l = float(vmm_l) / 60.0
v_r = float(vmm_r) / 60.0
print("v_l: ", v_l)
print("v_r: ", v_r)
self.set_wheel_drive_rates(v_l, v_r)
elif command == 'f' or command == 'r':
direction = command
print(direction)
self.setMotorsDirection(direction)
index = 0
speed = float(cmd[1:])
print("Speed: ", speed)
self.motorSpeed(speed, speed)
elif command == 'd':
index = 0
inches = self.intFromStr(cmd, index)
_qWallDistance.put(inches)
else:
print("Invalid input: ", command)
print("Please try again.")
#time.sleep(4)
_q2.task_done()
# End while
self.shutdown()
time.sleep(2)
def laserNav(self, _qCommands):
""" Name: laserNav
Date: January 2018
Arguments: self
Purpose: laserNav
"""
lastCommandChangeTime = None
lastCommand = None
while self._running:
if not (lastCommandChangeTime == None):
if not (lastCommand == None):
# There is a time and command.
# Check if at least 30 seconds have passed since
# last state change.
TIME_TO_WAIT = 30 # Seconds
currentTime = time.time() # Seconds
sufficientWaitTime = ((currentTime - lastCommandChangeTime)
> TIME_TO_WAIT)
if sufficientWaitTime:
_qCommands.put(lastCommand)
lastCommandChangeTime = currentTime
files = os.listdir("/home/pi/p/MuleBot2/")
for file in files:
# Looking for files ending in ".loc"
if file.endswith(".loc"):
print(file)
command = "rm " + file
os.system(command)
# Determine which of the six states that the
# Laser Detector is in and steer accordingly.
if file == "LO_FL.loc":
_qCommands.put("S6")
lastCommand = "S6"
elif file == "LO_L.loc":
_qCommands.put("S3")
lastCommand = "S3"
elif file == "LO_C.loc":
_qCommands.put("S1")
lastCommand = "S1"
elif file == "RO_C.loc":
_qCommands.put("P1")
lastCommand = "P1"
elif file == "RO_R.loc":
_qCommands.put("P3")
lastCommand = "P3"
elif file == "RO_FR.loc":
_qCommands.put("P6")
lastCommand = "P6"
else:
# This should never happen.
print(file, " is an invalid state name")
# Ignoring the check for an invalid state, their
# there should have been a command issued.
lastCommandChangeTime = time.time()
time.sleep(0.5)
def setMotorsDirection(self, _direction):
"""setMotorsDirection sets both motors to the same direction. """
if _direction == 'f' or _direction == 'F':
self.motorDirection(self.motor1DirectionPin, self.motorForward)
self.motorDirection(self.motor2DirectionPin, self.motorForward)
elif _direction == 'r' or _direction == 'R':
self.motorDirection(self.motor1DirectionPin, self.motorReverse)
self.motorDirection(self.motor2DirectionPin, self.motorReverse)
else:
print("ERROR: setMotorsDirection bad parameter: " + direction)
def shutdown(self):
"""shutdown """
count = 0
self.pwm.setPWM(0, 0, count)
self.pwm.setPWM(1, 0, count)
### How to use the Enable Pin???
#TODO: Put this back in.
GPIO.output(self.pwmEnablePin, GPIO.HIGH)
GPIO.cleanup()
print
print("Good Bye!")
print(Angle_B)
for i in range(CurrentAngle, Angle_B, -1)
i += (200/45)
pwm.setPWM(pinElbow, 0, i)
#delay(5)
if not isnan(Angle_C):
# CurrentAngle = WristTilt.read()
if Angle_C > CurrentAngle:
print(CurrentAngle)
print(" < ")
print(Angle_C)
for i in range(CurrentAngle, Angle_C)
pwm.setPWM(pinWrist, 0, i)
print("New angle: ", i)
def reset_servos():
"""
Pins
"""
# delay(2000);
# pinFinger = 0
def setServoPulse(channel, pulse):
pulseLength = 1000000 # 1,000,000 us per second
pulseLength /= 60 # 60 Hz
print "%d us per period" % pulseLength
pulseLength /= 4096 # 12 bits of resolution
print "%d us per bit" % pulseLength
pulse *= 1000
pulse /= pulseLength
pwm.setPWM(channel, 0, pulse)
pwm.setPWMFreq(60) # Set frequency to 60 Hz
def tap():
pwm.setPWM(2, 0, 300)
time.sleep(.5)
pwm.setPWM(2, 0, 500)
time.sleep(.5)
pwm.setPWM(2, 0, 300)
time.sleep(.5)
pwm.setPWM(1, 0, 375)
time.sleep(.5)
pwm.setPWM(2, 0, 300)
while y != "no":
print ">>>> M value is: ", m
m = raw_input(" Now what? >> ")
if m != "stop" and int(m) > 149 and int(m) < 601:
pwm.setPWM(1, 0, int(m))
tap()
elif int(m) < 150:
from Adafruit_PWM_Servo_Driver import PWM # enabling servo driver pwm = PWM(0x40) # initialise the PWM device using the default address pwm.setPWMFreq(60) # set frequency to 60 Hz pwm.setPWM(0, 0, 430) # head up
#--------------------------------------------------------------- # Servo Drive setup #--------------------------------------------------------------- #pwm channels ail = 0 ele = 1 rud = 2 ttl = 3 pan = 4 tlt = 5 ttl_min = 260 #pwm setup pwm = PWM() pwm.setPWMFreq(43) pwm.setPWM(ail, 1, 275) #Aileron, RollL=200, RollR=400, Center=275 pwm.setPWM(ele, 1, 270) #Elevator, 400 pitch down, 200 pitch up pwm.setPWM(rud, 1, 290) #Rudder, YawL=400, YawR=200, Center=290 pwm.setPWM(ttl, 1, ttl_min) #Throttle pwm.setPWM(pan, 1, 290) #Pan, PanL=400, PanR=200, Center=290 pwm.setPWM(tlt, 1, 250) #Tilt, TiltD=400, TileU=200, Center=250 #--------------------------------------------------------------- # GPIO Aknowledge/Request #--------------------------------------------------------------- # Pin definitions cc_akn_pin = 27 cc_req_pin = 22 auto_akn_pin = 4 auto_req_pin = 17
from Adafruit_PWM_Servo_Driver import PWM import time pwm = PWM(0x40) pwm.setPWM(1, 0, 450) pwm.setPWM(8, 0, 150) time.sleep(3) pwm.setAllPWM(0, 300) #pwm.setPWM(1,0,300) #pwm.setPWM(8,0,300)
servoMaxxx = 760 y = "yes" m = 1 def setServoPulse(channel, pulse): pulseLength = 1000000 # 1,000,000 us per second pulseLength /= 60 # 60 Hz print "%d us per period" % pulseLength pulseLength /= 4096 # 12 bits of resolution print "%d us per bit" % pulseLength pulse *= 1000 pulse /= pulseLength pwm.setPWM(channel, 0, pulse) pwm.setPWMFreq(60) # Set frequency to 60 Hz for i in range(15, 60): i = i * 10 pwm.setPWM(1, 0, int(i)) time.sleep(.5) print ".......", m m = m + 1 pwm.setPWM(1, 0, servoMin) time.sleep(2) pwm.setPWM(1, 0, servoMax) time.sleep(2) pwm.setPWM(1, 0, servoMin) time.sleep(2)
def setServoPulse(channel, pulse):
pulseLength = 1000000 # 1,000,000 us per second
pulseLength /= 60 # 60 Hz
print "%d us per period" % pulseLength
pulseLength /= 4096 # 12 bits of resolution
print "%d us per bit" % pulseLength
pulse *= 1000
pulse /= pulseLength
pwm.setPWM(channel, 0, pulse)
pwm.setPWMFreq(60) # Set frequency to 60 Hz
#while (True):
# Change speed of continuous servo on channel O
#pwm.setPWM(0, 0, servoMin)
#time.sleep(1)
#i = 150
#while i < 600:
# pwm.setPWM(0, 0, i)
# time.sleep(1)
# i = i + 25
# print i
#pwm.setPWM(0,0,0)
pwm.setPWM(0, 0, 250)
time.sleep(0.5)
pwm.setPWM(0, 0, 000)
pulseLength = 1000000 # 1,000,000 us per second
pulseLength /= 60 # 60 Hz
print "%d us per period" % pulseLength
pulseLength /= 4096 # 12 bits of resolution
print "%d us per bit" % pulseLength
pulse *= 1000
pulse /= pulseLength
pwm.setPWM(channel, 0, pulse)
pwm.setPWMFreq(60) # Set frequency to 60 Hz
while y != "no":
# oldcomment Change speed of continuous servo on channel O
m = raw_input("Now what? >>")
if m != "stop":
pwm.setPWM(1, 0, int(m))
# elif m == "0":
# pwm.setPWM(1, 0, servoMinn)
# elif m == "3":
# pwm.setPWM(1, 0, servoMax)
# elif m == "2":
# pwm.setPWM(1, 0, 375)
# elif m == "4":
# pwm.setPWM(1, 0, servoMaxx)
# elif m == "5":
# pwm.setPWM(1, 0, servoMaxxx)
else:
y = "no"
print "murder she wrote"
# ===========================================================================
# Example Code
# ===========================================================================
# Initialise the PWM device using the default address
pwm = PWM(0x40)
# Note if you'd like more debug output you can instead run:
# pwm = PWM(0x40, debug=True)
servoMin = 100 # Min pulse length out of 4096
servoMax = 800 # Max pulse length out of 4096
def setServoPulse(channel, pulse):
"""Change pulse of servo."""
pulseLength = 1000000 # 1,000,000 us per second
pulseLength /= 60 # 60 Hz
print "%d us per period" % pulseLength
pulseLength /= 4096 # 12 bits of resolution
print "%d us per bit" % pulseLength
pulse *= 1000
pulse /= pulseLength
pwm.setPWM(channel, 0, pulse)
while True:
pwm.setPWMFreq(60) # Set frequency to 60 Hz
pwm.setPWM(5, 5, servoMin)
time.sleep(1)
pwm.setPWM(5, 5, servoMax)
time.sleep(1)
import sys
sys.path.append("..")
from Navio import GPIO
#drive Output Enable in PCA low
pin = GPIO.Pin(27)
pin.write(0)
PCA9685_DEFAULT_ADDRESS = 0x40
frequency = 50
NAVIO_RCOUTPUT_1 = 3
SERVO_MIN_ms = 1.250 # mS
SERVO_MAX_ms = 1.750 # mS
#convert mS to 0-4096 scale:
SERVO_MIN = math.trunc((SERVO_MIN_ms * 4096.0) / (1000.0 / frequency) - 1)
SERVO_MAX = math.trunc((SERVO_MAX_ms * 4096.0) / (1000.0 / frequency) - 1)
pwm = PWM(0x40, debug=True)
# Set frequency to 60 Hz
pwm.setPWMFreq(frequency)
while (True):
pwm.setPWM(NAVIO_RCOUTPUT_1, 0, SERVO_MIN)
time.sleep(1)
pwm.setPWM(NAVIO_RCOUTPUT_1, 0, SERVO_MAX)
time.sleep(1)
import time # =========================================================================== # Example Code # =========================================================================== # Initialise the PWM device using the default address pwm = PWM(0x40) # Note if you'd like more debug output you can instead run: #pwm = PWM(0x40, debug=True) servoMin = 150 # Min pulse length out of 4096 servoMax = 600 # Max pulse length out of 4096 def setServoPulse(channel, pulse): pulseLength = 1000000 # 1,000,000 us per second pulseLength /= 60 # 60 Hz print "%d us per period" % pulseLength pulseLength /= 4096 # 12 bits of resolution print "%d us per bit" % pulseLength pulse *= 1000 pulse /= pulseLength pwm.setPWM(channel, 0, pulse) pwm.setPWMFreq(60) # Set frequency to 60 Hz while (True): # Change speed of continuous servo on channel O pwm.setPWM(0, 0, servoMin) time.sleep(1) pwm.setPWM(0, 0, servoMax)
class AdafruitMotorHAT(object):
FORWARD = 1
BACKWARD = 2
RELEASE = 3
def __init__(self, addr=0x60, freq=1600, i2c=None, i2c_bus=None):
"""
Initialize a Motor Hat
:param addr:
Motor Hat address
:type addr:
hex
:param freq:
pwm frequency
:type freq:
int
:param i2c:
the i2c device
:param i2c_bus:
the i2c bus to utilize
"""
self._frequency = freq
self.motors = [AdafruitDCMotor(self, m) for m in range(4)]
self.steppers = [
AdafruitStepperMotor(self, 1),
AdafruitStepperMotor(self, 2)
]
self._pwm = PWM(addr, debug=False, i2c=i2c, i2c_bus=i2c_bus)
self._pwm.setPWMFreq(self._frequency)
def set_pin(self, pin, value):
"""
Set a pin output state
:param pin:
pin number
:type pin:
int
:param value:
pin state 0, or 1
:type value:
int
:return:
None
"""
if pin not in range(16):
raise ValueError('PWM pin must be between 0 and 15 inclusive')
if value not in [0, 1]:
raise ValueError('Pin value must be 0 or 1!')
if value == 0:
self._pwm.setPWM(pin, 0, 4096)
if value == 1:
self._pwm.setPWM(pin, 4096, 0)
def get_stepper(self, num):
"""
Get a stepper instance
:param num:
the stepper to get 1, or 2
:type num:
int
:return:
:class:`AdafruitStepperMotor'
"""
if num not in [1, 2]:
raise ValueError(
'MotorHAT Stepper must be between 1 and 2 inclusive')
return self.steppers[num - 1]
def get_motor(self, num):
"""
Get a motor instance
:param num:
the stepper to get 1-4
:type num:
int
:return:
:class:`AdafruitDCMotor'
"""
if num not in [1, 2, 3, 4]:
raise ValueError(
'MotorHAT Motor must be between 1 and 4 inclusive')
return self.motors[num - 1]
#!/usr/bin/python
from Adafruit_PWM_Servo_Driver import PWM
import time
pwm = PWM(0x40, debug=True)
cycle = 50
pwm.setPWMFreq(cycle)
pulseLength = 1000000 / cycle
tick = pulseLength / 4096 #12 bit
while (True):
value = raw_input('Enter the micro second position: ')
pwm.setPWM(0, 0, int(value) / tick)
time.sleep(1)
def setServoPulse(channel, pulse):
pulseLength = 1000000 # 1,000,000 us per second
pulseLength /= 60 # 60 Hz
print "%d us per period" % pulseLength
pulseLength /= 4096 # 12 bits of resolution
print "%d us per bit" % pulseLength
pulse *= 1000
pulse /= pulseLength
pwm.setPWM(channel, 0, pulse)
pwm.setPWMFreq(60) # Set frequency to 60 Hz
while (True):
# Change speed of continuous servo on channel O
pwm.setPWM(0, 0, servoMin)
pwm.setPWM(1, 0, servoMin)
pwm.setPWM(2, 0, servoMin)
pwm.setPWM(3, 0, servoMin)
pwm.setPWM(4, 0, servoMin)
pwm.setPWM(5, 0, servoMin)
pwm.setPWM(6, 0, servoMin)
pwm.setPWM(7, 0, servoMin)
pwm.setPWM(8, 0, servoMin)
pwm.setPWM(9, 0, servoMin)
pwm.setPWM(10, 0, servoMin)
pwm.setPWM(11, 0, servoMin)
pwm.setPWM(12, 0, servoMin)
pwm.setPWM(13, 0, servoMin)
pwm.setPWM(14, 0, servoMin)
pwm.setPWM(15, 0, servoMin)
class Motor:
#phase_shift = total_steps / 3
def __init__(self, EN1, EN2, EN3, IN1, IN2, IN3):
self.pwm = PWM(0x40)
self.pwm.setPWMFreq(1000)
self.EN1 = LED(EN1)
self.EN2 = LED(EN2)
self.EN3 = LED(EN3)
self.IN1 = IN1
self.IN2 = IN2
self.IN3 = IN3
self.pwm_values = []
self.current_step = 0
self.total_steps = 7200
self.speed = 0
self.createPWVM()
#self.createPWM()
def motorOff(self):
self.EN1.off()
self.EN2.off()
self.EN3.off()
self.pwm.setPWM(self.IN1, 0, 0)
self.pwm.setPWM(self.IN2, 0, 0)
self.pwm.setPWM(self.IN3, 0, 0)
def motorSpeed(self, speed):
self.speed = speed
def motorStep(self):
self.current_step = (self.current_step + self.speed) % self.total_steps
current_stepA = self.current_step
current_stepB = (current_stepA +
self.total_steps / 3) % self.total_steps
current_stepC = (current_stepA +
self.total_steps / 3 * 2) % self.total_steps
self.EN1.on()
self.EN2.on()
self.EN3.on()
self.pwm.setPWM(self.IN1, 0, self.pwm_values[current_stepA])
self.pwm.setPWM(self.IN2, 0, self.pwm_values[current_stepB])
self.pwm.setPWM(self.IN3, 0, self.pwm_values[current_stepC])
def createPWVM(self):
max_amplitude = 4721
divisor = 1
for step in range(0, self.total_steps):
degree = step * 360.0 / self.total_steps
phaseA = math.sin(
degree * math.pi / 180.0) * max_amplitude / divisor
phaseB = math.sin(
(degree - 120) * math.pi / 180.0) * max_amplitude / divisor
phaseC = math.sin(
(degree + 120) * math.pi / 180.0) * max_amplitude / divisor
voff = (min(phaseA, phaseB, phaseC) +
max(phaseA, phaseB, phaseC)) / 2
self.pwm_values.append((int((phaseA - voff) / 2)) +
(2047 / divisor))
def createPWM(self):
max_amplitude = 4095
divisor = 1
for step in range(0, self.total_steps):
degree = step * 360.0 / self.total_steps
phaseA = math.sin(
degree * math.pi / 180.0) * max_amplitude / divisor
self.pwm_values.append(int(phaseA) + (2047 / divisor))
# random_second_number = random.randint(0, 1000)
# random_third_number = random.randint(0, 1000)
# pwm.setPWM(1, 0, random_number) # changed 0 to 13 to 1
# pwm.setPWM(2, 0, random_second_number) # changed 0 to 14 to 2
# pwm.setPWM(3, 0, random_third_number) # changed 0 to 15 to 3
#
# time.sleep(0.2) #waits a fifth second
# pwm.setPWM(1, 0, 0) # changed 0 to 13 to 1
# pwm.setPWM(2, 0, 0) # changed 0 to 14 to 2
# pwm.setPWM(3, 0, 0) # changed 0 to 15 to 3
pwm = PWM(0x40)
for x in range(1, 2): #perform the while loop 1 times
new_number = 1
while new_number < 17:
pwm.setPWM(new_number, 0, 500)
new_number = new_number + 1
time.sleep(
0.05
) # waits a twentieth second. change frequency: p.ChangeFrequency(new frequency in Hz)
pwm = PWM(0x41)
for x in range(1, 2): #perform the while loop 1 times
new_number_2 = 1
while new_number_2 < 17:
pwm.setPWM(new_number_2, 0, 500)
new_number_2 = new_number_2 + 1
time.sleep(
0.05
) # waits a twentieth second. change frequency: p.ChangeFrequency(new frequency in Hz)
pwm = PWM(0x42)
for x in range(1, 2): #perform the while loop 1 times
# =========================================================================== # Example Code # =========================================================================== # Initialise the PWM device using the default address pwm = PWM(0x40) # Note if you'd like more debug output you can instead run: #pwm = PWM(0x40, debug=True) servoMin = 150 # Min pulse length out of 4096 servoMax = 600 # Max pulse length out of 4096 def setServoPulse(channel, pulse): pulseLength = 1000000 # 1,000,000 us per second pulseLength /= 60 # 60 Hz print "%d us per period" % pulseLength pulseLength /= 4096 # 12 bits of resolution print "%d us per bit" % pulseLength pulse *= 1000 pulse /= pulseLength pwm.setPWM(channel, 0, pulse) pwm.setPWMFreq(50) # Set frequency to 60 Hz pwm.setPWM(0, 0,305) pwm.setPWM(1, 0,235) pwm.setPWM(2, 0,520) pwm.setPWM(3, 0,305) pwm.setPWM(4, 0,305) pwm.setPWM(5, 0,340)
class RGB_LED(object):
"""Object communicating to the LEDs.
Low level class that creates the PWM messages that are sent to the
microcontroller. It contains offset addresses relatives to the
address of the various LEDs.
Each LED on a Duckiebot or a watchtower is indexed by a number:
+------------------+------------------------------------------+
| Index | Position (rel. to direction of movement) |
+==================+==========================================+
| 0 | Front left |
+------------------+------------------------------------------+
| 1 | Rear left |
+------------------+------------------------------------------+
| 2 | Top / Front middle |
+------------------+------------------------------------------+
| 3 | Rear right |
+------------------+------------------------------------------+
| 4 | Front right |
+------------------+------------------------------------------+
Setting the color of a single LED is done by setting the brightness of the
red, green, and blue channels to a value between 0 and 255. The communication
with the hardware controller is abstracted through the :obj:`setRGB` method. By
using it, you can set directly set the desired color to any LED.
"""
# Class-specific constants
OFFSET_RED = 0 #: Offset address for the red color
OFFSET_GREEN = 1 #: Offset address for the green color
OFFSET_BLUE = 2 #: Offset address for the blue color
def __init__(self, debug=False):
self.pwm = PWM(address=0x40, debug=debug)
for i in range(15):
# Sets fully off all the pins
self.pwm.setPWM(i, 0, 4095)
def setLEDBrightness(self, led, offset, brightness):
"""Sets value for brightness for one color on one LED.
Calls the function pwm.setPWM to set the PWM signal according to
the input brightness.
Typically shouldn't be used directly. Use :obj:`setRGB` instead.
Args:
led (:obj:`int`): Index of specific LED (from the table above)
offset (:obj:`int`): Offset for color
brightness (:obj:`int8`): Intensity of brightness (between 0 and 255)
"""
self.pwm.setPWM(3 * led + offset, brightness << 4, 4095)
def setRGB(self, led, color):
"""Sets value for brightness for all channels of one LED
Converts the input color brightness from [0,1] to [0,255] for all
channels, then calls self.setLEDBrightness with the right offset
corresponding to the color channel in the PWM signal and the color
value as int8.
Args:
led (:obj:`int`): Index of specific LED (from the table above)
color (:obj:`list` of :obj:`float`): Brightness for the three RGB channels, in interval [0,1]
"""
self.setLEDBrightness(led, self.OFFSET_RED, int(color[0] * 255))
self.setLEDBrightness(led, self.OFFSET_GREEN, int(color[1] * 255))
self.setLEDBrightness(led, self.OFFSET_BLUE, int(color[2] * 255))
def __del__(self):
"""Destructur method.
Turns off all the LEDs and deletes the PWM object.
"""
for i in range(15):
# Sets fully off all channels of all the LEDs (3 channles * 5 LEDs)
self.pwm.setPWM(i, 0, 4095)
del self.pwm
3500,
3600,
3700,
3800,
3900,
4000,
4095
]
# PCA9685-PWM-Board
pwm = PWM(0x40)
pwm.setPWMFreq(400)
PWM_CHANNELS = 16
channels = []
for i in range(PWM_CHANNELS):
pwm.setPWM(i, 0, 0)
channels.append(0)
led = LED_RED
def gpio_callback(gpio_id, val):
print("gpio %s: %s" % (gpio_id, val))
if gpio_id == 10:
if val:
pwm.setPWM(1, 0, 0)
else:
pwm.setPWM(1, 0, 4000)
def socket_callback(socket, val):
def main():
pwm = PWM(0x40)
pwm.setPWMFreq(100)
vs = PiVideoStream(resolution=(640, 480)).start()
time.sleep(1.0)
frame = vs.read()
prvs = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
hsv = np.zeros_like(frame)
hsv[..., 1] = 255
mode = 0
speed = 0
steer = 0
while True:
frame = vs.read()
#frame = rotate_image(frame)
next = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
if mode == 3:
flow = cv2.calcOpticalFlowFarneback(prvs, next, 0.5, 3, 15, 3, 5,
1.2, 0)
mag, ang = cv2.cartToPolar(flow[..., 0], flow[..., 1])
hsv[..., 0] = ang * 180 / 3.14 / 2
hsv[..., 2] = cv2.normalize(mag, None, 0, 255, cv2.NORM_MINMAX)
bgr = cv2.cvtColor(hsv, cv2.COLOR_HSV2BGR)
cv2.putText(frame, "Speed: {}, Steer: {}".format(speed, steer),
(10, 20), cv2.FONT_HERSHEY_PLAIN, 1.0, (255, 0, 0))
if mode == 1:
frame = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
if mode == 2:
frame = cv2.Canny(frame, 20, 100)
if mode == 3:
cv2.imshow("Frame", bgr)
else:
cv2.imshow("Frame", frame)
key = cv2.waitKey(1) & 0xFF
if key == ord("q"):
break
if key == ord("f"):
mode = 3
if key == ord("e"):
mode = 2
if key == ord("g"):
mode = 1
if key == ord("c"):
mode = 0
if key == ord("l"):
pwm.setPWM(0, 0, 500)
if key == ord("r"):
pwm.setPWM(0, 0, 300)
if key == 81: # left
if steer > -1:
steer = steer - 0.1
if key == 83: # right
if steer < 1:
steer = steer + 0.1
if key == 82: # up
if speed < 1:
speed = speed + 0.1
if key == 84: # down
if steer > -1:
speed = speed - 0.1
if key == ord("s"):
speed = 0
steer = 0
pwm.setPWM(0, 0, 500 + int(speed * 100))
pwm.setPWM(2, 0, 670 - int(steer * 100))
prvs = next
cv2.destroyAllWindows()
vs.stop()
elif a < -40.0:
a = -40.0
print a
ms = a * m + b
if ms > 2.0:
ms = 2.0
elif ms < 1.0:
ms = 1.0
s = ms / 1000
print s
ticks = int(s / timepertick)
print ticks
return ticks
pwm.setPWM(0, 0, AngleToPulse(0))
time.sleep(1)
pwm.setPWM(1, 0, AngleToPulse(0))
time.sleep(1)
pwm.setPWM(2, 0, AngleToPulse(0))
time.sleep(1)
while (True):
# Change speed of continuous servo on channel O
a = float(raw_input("Angle -40 to 40: "))
pwm.setPWM(2, 0, AngleToPulse(a))
#print 1
#time.sleep(1)
#setServoPulse(0, 50)
#print 50
#time.sleep(1)
print "d% us per bit" % pulseLength
pulse *= 1000
pulse /= pulseLength
pwm.setPWM(channel, 0, pulse)
for frame in camera.capture_continuous(rawCapture,
format='bgr',
use_video_port=True):
image = frame.array
pwm.setPWMFreq(60)
print "---------"
#change speed of cont servo on channel 0
pwm.setPWM(LEFT_RIGHT, 0, MAX_LEFT)
pwm.setPWM(UP_DOWN, 0, 550)
time.sleep(2)
pwm.setPWM(LEFT_RIGHT, 0, 541)
pwm.setPWM(UP_DOWN, 0, 550)
time.sleep(2)
pwm.setPWM(LEFT_RIGHT, 0, 503)
pwm.setPWM(UP_DOWN, 0, 550)
time.sleep(2)
pwm.setPWM(LEFT_RIGHT, 0, 465)
pwm.setPWM(UP_DOWN, 0, 550)
cv2.imshow("Frame", image)
from Adafruit_PWM_Servo_Driver import PWM
import time
pwm1 = PWM(0x40)
pwm2 = PWM(0x41)
pwm1.setPWMFreq(60)
pwm2.setPWMFreq(60)
mid = 300
#end = 590
for channel in range(0, 6):
pwm2.setPWM(channel, 0, mid)
time.sleep(0.5)
for channel in range(8, 14):
pwm2.setPWM(channel, 0, mid)
time.sleep(0.5)
for channel in range(0, 6):
pwm1.setPWM(channel, 0, mid)
time.sleep(0.5)
class CarControl(object):
def __init__(self):
self._pwm = PWM(0x40) # I2C address
self._pwm.setPWMFreq(SERVO_REFRESH_FREQUENCY)
self.motor_stop_and_center()
self.reset_steering()
return
def more_gas(self):
global gas_pedal
if gas_pedal <= MOTOR_MAX_THROTTLE_PERCENTAGE:
gas_pedal += MOTOR_STEP_SIZE_PERCENT
else:
gas_pedal = MOTOR_MAX_THROTTLE_PERCENTAGE
self.throttle = gas_pedal
return
def reverse(self):
global gas_pedal
if gas_pedal >= MOTOR_MAX_REVERSE_PERCENTAGE:
gas_pedal -= MOTOR_STEP_SIZE_PERCENT
else:
gas_pedal = MOTOR_MAX_REVERSE_PERCENTAGE
self.throttle = gas_pedal
return
def active_braking(self):
global gas_pedal
# ACTIVE BRAKE OPPOSITE DIRECTION
if gas_pedal > 0:
self.throttle = -MOTOR_BRAKING_FORCE
elif gas_pedal < 0:
self.throttle = MOTOR_BRAKING_FORCE
time.sleep(MOTOR_BRAKING_DELAY)
self.throttle = 0
gas_pedal = 0
return
def sensor_braking(self):
global gas_pedal
# ACTIVE BREAK OPPOSITE DIRECTION
self.throttle = -MOTOR_BRAKING_FORCE
time.sleep(MOTOR_BRAKING_DELAY)
self.throttle = 0
gas_pedal = 0
return
def _get_motor_pwm_value(self):
if self.throttle == 0:
return MOTOR_STOP_PWM_VALUE
elif self.throttle < 0:
coeff = -self.throttle / 100.0
min_value = MOTOR_DEADBAND_START_VALUE
max_value = MOTOR_MIN_PWM_VALUE
else:
coeff = self.throttle / 100.0
min_value = MOTOR_DEADBAND_STOP_VALUE
max_value = MOTOR_MAX_PWM_VALUE
assert 0.0 <= coeff <= 1.0
diff = max_value - min_value
# Interpolate between the min and max values
return int(round(min_value + coeff * diff, 0))
@property
def throttle(self):
return self._throttle
@throttle.setter
def throttle(self, value):
assert -100 <= value <= 100
self._throttle = value
pwm_value = self._get_motor_pwm_value()
print('Setting motor PWM to', pwm_value)
self._pwm.setPWM(MOTOR_PWM_CHANNEL, 0, pwm_value)
return
def motor_stop_and_center(self):
global currentSteeringAngle
self.throttle = 0
currentSteeringAngle = steerCenter
return
def stop_and_reset(self):
self.throttle = 0
self.active_breaking()
self.motor_stop_and_center()
self.reset_steering()
return
def reset_steering(self):
global currentSteeringAngle
self._pwm.setPWM(STEERING_PWM_CHANNEL, 0, steerCenter)
currentSteeringAngle = steerCenter
return
def steer_right(self):
global currentSteeringAngle
if currentSteeringAngle <= (steerRight - STEERING_ANGLE_STEP):
currentSteeringAngle += STEERING_ANGLE_STEP
else:
currentSteeringAngle = steerRight
self._pwm.setPWM(STEERING_PWM_CHANNEL, 0, currentSteeringAngle)
return
def steer_left(self):
global currentSteeringAngle
if currentSteeringAngle >= (steerLeft + STEERING_ANGLE_STEP):
currentSteeringAngle -= STEERING_ANGLE_STEP
else:
currentSteeringAngle = steerLeft
self._pwm.setPWM(STEERING_PWM_CHANNEL, 0, currentSteeringAngle)
return
# ===========================================================================
# Hatalogico PWM Blanking Test for LEDs - powered by Adafruit's Libraries
# -------------------------------------------------
# Date: 24/3/2015
# Stolen By: John Lumley
#
# BIG THANKS TO ADAFRUIT INDUSTRIES FOR MAKING THIS POSSIBLE AND EASY
# ===========================================================================
import os, sys
# DETERMINE CURRENT PATH
scriptPath = os.path.realpath(os.path.dirname(sys.argv[0]))
os.chdir(scriptPath)
# APPEND FOLDER OF REQUIRED LIBRARY
sys.path.append("Adafruit/Adafruit_PWM_Servo_Driver")
# FINALLY LOAD THE LIBRARY
from Adafruit_PWM_Servo_Driver import PWM
# SETUP THE PWM DRIVER (I2C ADDRESS IS 70 BY DEFAULT ON HATALOGICO)
# EACH CHANNEL RANGES FROM 0 (off) TO 4095 (on)
pwm = PWM(0x70)
# SET FREQUENCY
pwm.setPWMFreq(120)
# LOOP THROUGH THE 16 PINS - note: range goes up to but does not include the 2nd parameter
for pwmPin in range(0, 16):
# TURN IT OFF COMPLETELY
pwm.setPWM(pwmPin, 0, 0)
class Adafruit_MotorShield:
LOW = 0
HIGHT = 1
def __init__(self, address=0x60, debug=False):
self.address = address
self.pwm = PWM(address, debug)
self.steppers = [
Adafruit_StepperMotor(debug),
Adafruit_StepperMotor(debug)
]
self.dcmotors = [
Adafruit_DCMotor(debug),
Adafruit_DCMotor(debug),
Adafruit_DCMotor(debug),
Adafruit_DCMotor(debug)
]
def begin(self, freq=1600):
self.pwm.begin()
self.freq = freq
self.pwm.setPWMFreq(freq)
# This is the maximum PWM frequency
for i in range(0, 16):
self.pwm.setPWM(i, 0, 0)
def setPWM(self, pin, value):
if value > 4095:
self.pwm.setPWM(pin, 4096, 0)
else:
self.pwm.setPWM(pin, 0, value)
def setPin(self, pin, value):
if value == self.LOW:
self.pwm.setPWM(pin, 0, 0)
else:
self.pwm.setPWM(pin, 4096, 0)
def getMotor(self, num):
if num > 4:
return
num -= 1
if (self.dcmotors[num].motornum is None):
# not init'd yet!
self.dcmotors[num].motornum = num
self.dcmotors[num].MC = self
if (num == 0):
pwm = 8
in2 = 9
in1 = 10
elif (num == 1):
pwm = 13
in2 = 12
in1 = 11
elif (num == 2):
pwm = 2
in2 = 3
in1 = 4
elif (num == 3):
pwm = 7
in2 = 6
in1 = 5
self.dcmotors[num].PWMpin = pwm
self.dcmotors[num].IN1pin = in1
self.dcmotors[num].IN2pin = in2
return self.dcmotors[num]
def getStepper(self, steps, num):
if num > 2:
return
num -= 1
if self.steppers[num].steppernum is None:
# not init'd yet!
self.steppers[num].steppernum = num
self.steppers[num].revsteps = steps
self.steppers[num].MC = self
if num == 0:
pwma = 8
ain2 = 9
ain1 = 10
pwmb = 13
bin2 = 12
bin1 = 11
elif num == 1:
pwma = 2
ain2 = 3
ain1 = 4
pwmb = 7
bin2 = 6
bin1 = 5
self.steppers[num].PWMApin = pwma
self.steppers[num].PWMBpin = pwmb
self.steppers[num].AIN1pin = ain1
self.steppers[num].AIN2pin = ain2
self.steppers[num].BIN1pin = bin1
self.steppers[num].BIN2pin = bin2
return self.steppers[num]
import time # =========================================================================== # Example Code # =========================================================================== # Initialise the PWM device using the default address pwm = PWM(0x40) # Note if you'd like more debug output you can instead run: #pwm = PWM(0x40, debug=True) servoMin = 104 # Min pulse length out of 4096 servoMid = 270 servoMax = 380 # Max pulse length out of 4096 pwm.setPWMFreq(50) # Set frequency to 50 Hz while (True): # Change speed of continuous servo on channel O pwm.setPWM(3, 0, servoMin) time.sleep(1) pwm.setPWM(3, 0, servoMid) time.sleep(1) pwm.setPWM(3, 0, servoMax) time.sleep(1) pwm.setPWM(3, 0, servoMid) time.sleep(1)
class RoboCarModel1(RoboCarBase.RoboCarBase):
model = "RobotCar Model 1"
version = 0.1
def stop(self):
self.mh.getMotor(1).run(Adafruit_MotorHAT.RELEASE)
self.mh.getMotor(3).run(Adafruit_MotorHAT.RELEASE)
def __init__(self, i2c_addr):
super(RoboCarModel1, self).__init__()
#logging.basicConfig(stream=sys.stderr, level=logging.DEBUG)
self.i2c_addr = i2c_addr
self.mh = Adafruit_MotorHAT(addr=i2c_addr)
self.pwm = PWM(0x60)
self.servoMin = 150 # Min pulse length out of 4096
self.servoMax = 600 # Max pulse length out of 4096
self.pwm.setPWMFreq(60)
self.motorLeft = self.mh.getMotor(3)
self.motorRight = self.mh.getMotor(1)
atexit.register(self.stop)
self.isRunningForward = 0
self.isRunningReverse = 0
self.isRunningLeft = 0
self.isRunningRight = 0
def setCamAngle(self, angle):
self.camAngle = angle
newAngle = self.map(self.camAngle, 0, 180, self.servoMin,
self.servoMax)
self.pwm.setPWM(0, 0, newAngle)
def setMotors(self, motorLeftSpeed, motorRightSpeed, motorLeftDir,
motorRightDir):
# set the speed to start, from 0 (off) to 255 (max speed)
newLeftSpeed = self.map(motorLeftSpeed, 0, 10, 0, 255)
newRightSpeed = self.map(motorRightSpeed, 0, 10, 0, 255)
self.motorLeft.setSpeed(newLeftSpeed)
self.motorRight.setSpeed(newRightSpeed)
self.motorLeft.run(motorLeftDir)
self.motorRight.run(motorRightDir)
def setMotorForward(self):
self.isRunningForward = 1
self.isRunningReverse = 0
self.setMotors(self.motorSpeed, self.motorSpeed,
Adafruit_MotorHAT.FORWARD, Adafruit_MotorHAT.FORWARD)
def setMotorReverse(self):
self.isRunningForward = 0
self.isRunningReverse = 1
self.setMotors(self.motorSpeed, self.motorSpeed,
Adafruit_MotorHAT.BACKWARD, Adafruit_MotorHAT.BACKWARD)
def setMotorForwardReverseStop(self):
self.isRunningForward = 0
self.isRunningReverse = 0
if (self.isRunningLeft):
self.setMotorLeft()
elif (self.isRunningRight):
self.setMotorRight()
else:
self.stop()
def setMotorLeftRightStop(self):
self.isRunningLeft = 0
self.isRunningRight = 0
if (self.isRunningForward):
self.setMotorForward()
elif (self.isRunningReverse):
self.setMotorReverse()
else:
self.stop()
def setMotorLeft(self):
self.isRunningLeft = 1
self.isRunningRight = 0
self.setMotors(self.motorTurnSpeed, self.motorTurnSpeed,
Adafruit_MotorHAT.BACKWARD, Adafruit_MotorHAT.FORWARD)
def setMotorRight(self):
self.isRunningLeft = 0
self.isRunningRight = 1
self.setMotors(self.motorTurnSpeed, self.motorTurnSpeed,
Adafruit_MotorHAT.FORWARD, Adafruit_MotorHAT.BACKWARD)
from Adafruit_PWM_Servo_Driver import PWM
import time
pwm = PWM(0x70)
pwm.setPWMFreq(50)
while (True):
pwm.setPWM(0, 0, 120)
time.sleep(0.3)
pwm.setPWM(0, 0, 290)
time.sleep(0.3)
pwm.setPWM(0, 0, 480)
time.sleep(0.3)
