def __init__(self, conffile):
Observable.__init__(self)
cp = ConfigParser()
cp.read(conffile)
if not Globals.globSimulate:
wiringpi.wiringPiSetupGpio()
self.md = MotorDriver([
cp.getint("MotorDriver", "LEFT_L"),
cp.getint("MotorDriver", "LEFT_R"),
cp.getint("MotorDriver", "RIGHT_L"),
cp.getint("MotorDriver", "RIGHT_R"),
cp.getint("MotorDriver", "VERTICAL_L"),
cp.getint("MotorDriver", "VERTICAL_R"),
])
self.md.start()
self.ds = DistanceSensor(
cp.getint("DistanceSensor", "TRIGGER"),
cp.getint("DistanceSensor", "ECHO")
)
self.ds.start()
self.acl = AltitudeControlLoop(self.md, self.ds)
self.update("MotorDriver", self.md)
self.update("DistanceSensor", self.ds)
self.update("AltitudeControlLoop", self.acl)
self.md.subscribe(self.update)
self.ds.subscribe(self.update)
self.acl.subscribe(self.update)
self.setCamera(False)
class TankoService(Service):
def __init__(self, index):
self.start_pwm()
self.start_dc_motors()
value = [
dbus.Byte(0x5A),
dbus.Byte(0x00),
dbus.Byte(100),
dbus.Byte(0x5A),
dbus.Byte(0x5A),
dbus.Byte(0x5A),
dbus.Byte(0x5A),
dbus.Byte(0x5A),
dbus.Byte(100),
dbus.Byte(100)]
self.move_motors(value)
Service.__init__(self, index, SVC_UUID, True)
self.rxCharacteristic = RxCharacteristic(self)
self.txCharacteristic = TxCharacteristic(self)
self.add_characteristic(self.rxCharacteristic)
self.add_characteristic(self.txCharacteristic)
def start_pwm(self):
self.pwm = PCA9685(0x40, debug=False)
self.pwm.setPWMFreq(50)
def start_dc_motors(self):
self.motorDriver = MotorDriver()
def move_motors(self, value):
print("---")
self.value = value
# Move servo motors
for channel in range(0, 8):
i = value[channel]
self.pwm.setServoAngle(channel,i)
print("Channel %d: %d" % (channel, i))
# self.rxCharacteristic.send_update()
# Move DC motors
m1 = value[8]
m2 = value[9]
self.motorDriver.move(m1 - 100, m2 - 100)
from MotorDriver import MotorDriver as Motor
from time import sleep
motor = Motor(8, 10)
while (True):
motor.drive(50, 50)
elif class_label == 1 and not media_player.is_playing():
media_player.play()
motor_driver.__start_motor__()
print()
print("Setting everything up...")
start = time.time()
# Setting up main objects that control execution
audio_recorder = AudioRecorder()
audio_recorder.__start_audio__()
motor_driver = MotorDriver()
model_runner = ModelRunner()
media_player = vlc.MediaPlayer(LULLABY_FILE)
old_volume = media_player.audio_get_volume()
media_player.audio_set_volume(100)
end = time.time()
print("Start-up in {} seconds".format(end - start))
finish_time = datetime.datetime.now() + datetime.timedelta(seconds=30)
while datetime.datetime.now() < finish_time:
# Actually records the audio and runs through the model
get_and_classify_audio(audio_recorder, model_runner, media_player,
class HeliosController(Observable):
md = None
ds = None
acl = None
cam = None
status = {}
def __init__(self, conffile):
Observable.__init__(self)
cp = ConfigParser()
cp.read(conffile)
if not Globals.globSimulate:
wiringpi.wiringPiSetupGpio()
self.md = MotorDriver([
cp.getint("MotorDriver", "LEFT_L"),
cp.getint("MotorDriver", "LEFT_R"),
cp.getint("MotorDriver", "RIGHT_L"),
cp.getint("MotorDriver", "RIGHT_R"),
cp.getint("MotorDriver", "VERTICAL_L"),
cp.getint("MotorDriver", "VERTICAL_R"),
])
self.md.start()
self.ds = DistanceSensor(
cp.getint("DistanceSensor", "TRIGGER"),
cp.getint("DistanceSensor", "ECHO")
)
self.ds.start()
self.acl = AltitudeControlLoop(self.md, self.ds)
self.update("MotorDriver", self.md)
self.update("DistanceSensor", self.ds)
self.update("AltitudeControlLoop", self.acl)
self.md.subscribe(self.update)
self.ds.subscribe(self.update)
self.acl.subscribe(self.update)
self.setCamera(False)
#self.acl.debug = True
#self.md.debug = True
def shutdown(self):
self.md.shutdown()
self.md.join()
self.ds.shutdown()
self.ds.join()
self.setCamera(False)
def turnLeft(self):
self.md.turnLeft()
def turnRight(self):
self.md.turnRight()
def forward(self):
self.md.forward()
def backward(self):
self.md.backward()
def up(self):
self.md.up()
def down(self):
self.md.down()
def setSpeed(self, val):
self.md.setSpeed(val)
def setAuto(self, val):
self.acl.setAuto(val)
def setHeight(self, val):
self.acl.setHeight(val)
def setForceDescent(self, val):
self.acl.setForceDescent(val)
def setSingleSteerMode(self, val):
self.md.setSingleSteerMode(val)
def setCamera(self, val):
if self.cam == None:
if val:
print "HeliosController enabling camera"
self.cam = Camera()
self.cam.subscribe(self.updateImage)
self.cam.start()
else:
print "HeliosController tried to disable camera, but is not active"
else:
if val:
print "HeliosController tried to enable camera, but is already active"
else:
print "HeliosController disabling camera"
self.cam.shutdown()
self.cam.join()
self.cam = None
self.status["cameraImage"] = "data:image/jpeg;base64," + base64.b64encode(open("nocam.jpg","rb").read())
self.status["cameraActive"] = True if self.cam != None else False
self.emit("HeliosController", self)
def updateImage(self, id, cam):
self.status["cameraImage"] = "data:image/jpeg;base64," + base64.b64encode(cam.getImage())
def getImage(self):
if self.cam != None:
return self.cam.getImage()
return open("nocam.jpg","rb").read()
def update(self, etype, src):
if etype == "MotorDriver":
self.status["statLeft"] = src.statusLeft()
self.status["statRight"] = src.statusRight()
self.status["statVert"] = src.statusVert()
self.status["pwmValue"] = src.getPWM()
self.status["singleSteeringMode"] = src.getSingleSteerMode()
if etype == "DistanceSensor":
self.status["currentAltitude"] = src.getCurrentDistance()
self.status["altitudeHistory"] = src.getDistanceHistory()
if etype == "AltitudeControlLoop":
self.status["requestedAltitude"] = src.getHeight()
self.status["maintainAltitude"] = src.getAuto()
self.status["forceDescent"] = src.getForceDescent()
self.emit("HeliosController", self)
def getStatus(self):
return self.status
'''Initialization of i2c, shares, queues, and objects used across multiple tasks'''
i2c = I2C(1, freq=200000) #Uses bus 3 because of the pins it is connected to
TOF1Share = task_share.Share('l')
TOF2Share = task_share.Share('l')
q0 = task_share.Queue('I',
68,
thread_protect=False,
overwrite=False,
name="Queue_0")
IRShare = task_share.Share('i')
shareIMU = task_share.Share('f')
shareLine = task_share.Share('i')
encoderR = EncoderDriver('PB6', 'PB7', 4, direction='clockwise')
encoderL = EncoderDriver('PC6', 'PC7', 8, direction='counterclockwise')
controllerR = ClosedLoopDriver(0, 0, .2, 100)
motorR = MotorDriver()
controllerL = ClosedLoopDriver(0, 0, .2, 20)
motorL = MotorDriver('PC1', 'PA0', 'PA1', 5, 100)
if __name__ == "__main__":
print('\033[2JTesting scheduler in cotask.py\n')
task1 = cotask.Task(MotorControlTask,
name='Task1 MC',
priority=6,
period=50,
profile=True,
trace=False)
task2 = cotask.Task(LineSensorTask,
name='Task2 LS',
priority=5,
# try:
# distance = sensor.get_average_distance()
# except BadDistanceException:
# pass
qualifier, distance = sensor.get_average_distance()
if qualifier == 1:
if distance < 20:
drive.stop()
time.sleep(0.01)
drive.reverse(duty_cycle - 2)
time.sleep(0.25)
drive.left(duty_cycle - 2)
time.sleep(0.25)
drive.forward(duty_cycle)
else:
drive.forward()
else:
pass
if __name__ == "__main__":
try:
ultrasonic = UltrasonicRanger()
led = LED()
robot_drive = MotorDriver()
loop(ultrasonic, robot_drive, led)
except KeyboardInterrupt:
ultrasonic.destroy()
from HaptogramService import HaptogramService
from I2CVibrationDevice import I2CVestDevice
from MotorDriver import MotorDriver
import json
import time
class TactileBoardListener:
def __init__(self, message_bus, haptogram_service):
self.hs = haptogram_service
message_bus.add_listener("suitceyes/tactile-board/play",
self._handle_message)
def _handle_message(self, data):
payload = json.loads(data.payload)
self.hs.enqueue(payload)
return
if __name__ == "__main__":
with I2CVestDevice(0x40) as vest_device, MqttMessageService() as mb:
motor_driver = MotorDriver(vest_device)
vpp = VibrationPatternPlayer(motor_driver)
hs = HaptogramService(vpp, 2.0, 0.1)
hs.start()
listener = TactileBoardListener(mb, hs)
while True:
time.sleep(1)
hs.stop()
# 1.create socket object:socket=socket.socket(family,type)
socket_tcp = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
print("TCP server listen @ %s:%d!" % (HOST_IP, HOST_PORT))
host_addr = (HOST_IP, HOST_PORT)
# 2.bind socket to addr:socket.bind(address)
socket_tcp.bind(host_addr)
# 3.listen connection request:socket.listen(backlog)
socket_tcp.listen(3)
# 5.handle
context = zmq.Context() #init tcp trans to send opencv catched camera frame.
footage_socket = context.socket(zmq.PUB) # zmq的广播模式
footage_socket.bind("tcp://*:5555")
# control 2 motor flags
Motor = MotorDriver()
# control 2 Servo flags
Ser = Servo()
#a PID controler.
RPID = PID(0.006, 0.0001, 0.005)
#Globale variables
l_speed = 100
r_speed = 100
global_message = ""
auto_tracer = False # a controlable flag.
land_mode = False
global_color = ""
is_saving = False
i2c_bus = board.I2C()
from HCSR04 import HCSR04
from MotorDriver import MotorDriver
class State(Enum) :
Search = 0
Forward = 1
if __name__ == "__main__":
triggerPin = 18
echoPin = 23
state = State.Search
if input("Running pre-flight tests. Proceed? [Y/n] ") == "n" : exit()
hcsr04 = HCSR04(triggerPin, echoPin)
motorDriver = MotorDriver()
print("Testing sensor...")
for i in range(0, 19) :
print ("Range: " + str(hcsr04.getRangeInCentimeters()))
time.sleep(0.5)
if input("Success? [Y/n] ") == "n" : exit()
print ("Testing motor driver...")
motorDriver.forward(0.1)
if input("Moving forward...success? [Y/n] ") == "n" : exit()
motorDriver.reverse(0.1)
from MotorDriver import MotorDriver as Motor from time import sleep motor = Motor(8,10) while(True): motor.drive(50,50)
from MotorDriver import MotorDriver
import time
import board
from adafruit_ina219 import ADCResolution, BusVoltageRange, INA219
Motor = MotorDriver()
i2c_bus = board.I2C()
k = 0
ina1 = INA219(i2c_bus, addr=0x40)
ina2 = INA219(i2c_bus, addr=0x41)
ina3 = INA219(i2c_bus, addr=0x42)
ina4 = INA219(i2c_bus, addr=0x43)
print("ina219 test")
ina1.bus_adc_resolution = ADCResolution.ADCRES_12BIT_32S
ina1.shunt_adc_resolution = ADCResolution.ADCRES_12BIT_32S
ina1.bus_voltage_range = BusVoltageRange.RANGE_16V
ina2.bus_adc_resolution = ADCResolution.ADCRES_12BIT_32S
ina2.shunt_adc_resolution = ADCResolution.ADCRES_12BIT_32S
ina2.bus_voltage_range = BusVoltageRange.RANGE_16V
ina3.bus_adc_resolution = ADCResolution.ADCRES_12BIT_32S
ina3.shunt_adc_resolution = ADCResolution.ADCRES_12BIT_32S
ina3.bus_voltage_range = BusVoltageRange.RANGE_16V
ina4.bus_adc_resolution = ADCResolution.ADCRES_12BIT_32S
ina4.shunt_adc_resolution = ADCResolution.ADCRES_12BIT_32S
def getForceDescent(self):
return self.forceDescent
def getHeight(self):
return self.height
def update(self, etype, src):
if self.auto and etype == "DistanceSensor":
alt = src.getCurrentDistance()
diff = alt - self.height
if self.debug:
print "Alt: %f, Height: %f, Diff: %f" % (alt, self.height, diff)
if self.forceDescent and diff > 5:
return self.motorDriver.down()
if diff < -5:
return self.motorDriver.up()
self.motorDriver.stopVertical()
if __name__ == '__main__':
md = MotorDriver([1,2,3,4,5,6])
ds = DistanceSensor(7,8)
acl = AltitudeControlLoop(md, ds)
acl.debug = True
ds.lastValue = 100
md.start()
ds.start()
acl.keepThisHeight()
def start_dc_motors(self):
self.motorDriver = MotorDriver()
def __init__(self):
"""SmartMTk class constructor
"""
# Init motors
self.motors = MotorDriver(22, 27, 17, 24, 23, 18, 25)
class SmartMTk:
"""This class manage an RC monstrer truck powered by a RaspberryPI
"""
def __init__(self):
"""SmartMTk class constructor
"""
# Init motors
self.motors = MotorDriver(22, 27, 17, 24, 23, 18, 25)
def forward(self):
"""Move forward the monster truck
"""
self.motors.a_forward(100)
def backward(self):
"""Move backward the monster truck
"""
self.motors.a_backward(100)
def shortBrake(self):
"""Short brake
"""
self.motors.a_shortBrake()
def turnRight(self):
"""Turn the wheels to the right
"""
self.motors.b_forward(100)
def turnLeft(self):
"""Move forward the monster truck
"""
self.motors.b_backward(100)
def straight(self):
"""Go straight
"""
self.motors.b_stop()