def __init__(self, controller):
self.arduinoController = controller
self.omoplate = servo.Servo(self.arduinoController)
self.shoulder = servo.Servo(self.arduinoController)
self.rotate = servo.Servo(self.arduinoController)
self.bicep = servo.Servo(self.arduinoController)
self.isAttached = False
def main():
# The GPIO9 and GPIO12 is meant.
# Don't use GPIO13, this bug is happening then https://github.com/espressif/esptool/issues/394
servo_y = servoLib.Servo(machine.Pin(9), 50, 1000, 2000, 180)
servo_x = servoLib.Servo(machine.Pin(12), 50, 1000, 2000, 180)
while True:
servo_y.write_angle(0)
servo_x.write_angle(0)
time.sleep(2)
servo_y.write_angle(180)
servo_x.write_angle(180)
time.sleep(2)
def __init__(self):
self.armLength = constants.ARM_LENGTHS[0] + constants.ARM_LENGTHS[
1] + constants.MALLET_RADIUS
self.surface = pygame.Surface((self.armLength * 2, self.armLength * 2))
self.surface.set_colorkey((0, 0, 0))
self.upperArmLength = constants.ARM_LENGTHS[0]
self.lowerArmLength = constants.ARM_LENGTHS[1]
self.basePos = (0, 0)
self.lowerArm = servo.Servo(0)
self.upperArm = servo.Servo(1)
self.mousePos = (0, 0)
async def test_telemetry_hello(
monkeypatch, optimizer: servo.configuration.Optimizer) -> None:
expected = f'"telemetry": {{"servox.version": "{servo.__version__}", "servox.platform": "{platform.platform()}", "servox.namespace": "test-namespace"}}'
# Simulate running as a k8s pod
monkeypatch.setenv("POD_NAMESPACE", "test-namespace")
# NOTE: Can't use servo_runner fixture; its init happens prior to setting of env var above
servo_runner = servo.runner.ServoRunner(
servo.Servo(
config=servo.BaseServoConfiguration(name="archibald",
optimizer=optimizer),
connectors=[], # Init empty servo
))
request = respx.post(
"https://api.opsani.com/accounts/dev.opsani.com/applications/servox/servo"
).mock(return_value=httpx.Response(
200, text=f'{{"status": "{servo.api.OptimizerStatuses.ok}"}}'))
await servo_runner._post_event(
servo.api.Events.hello,
dict(agent=servo.api.user_agent(),
telemetry=servo_runner.servo.telemetry.values),
)
assert request.called
assert expected in request.calls.last.request.content.decode()
async def test_diagnostics_reset(
monkeypatch, optimizer: servo.configuration.Optimizer) -> None:
monkeypatch.setenv("POD_NAMESPACE", "test-namespace")
servo_runner = servo.runner.ServoRunner(
servo.Servo(
config=servo.BaseServoConfiguration(name="archibald",
optimizer=optimizer),
connectors=[], # Init empty servo
))
put = respx.put(
f"https://api.opsani.com/accounts/dev.opsani.com/applications/servox/{servo.telemetry.DIAGNOSTICS_CHECK_ENDPOINT}"
).mock(return_value=httpx.Response(200, text=f'{{"status": "ok"}}'))
reset_state = servo.telemetry.DiagnosticStates.withhold
response = await servo.telemetry.DiagnosticsHandler(
servo_runner.servo)._diagnostics_api(
method="PUT",
endpoint=servo.telemetry.DIAGNOSTICS_CHECK_ENDPOINT,
output_model=servo.api.Status,
json=reset_state,
)
assert put.called
assert response.status == servo.api.OptimizerStatuses.ok
def main(max_time, altitude_dif, delay_sample):
formatted_time = datetime.datetime.utcnow().isoformat()
print('[{}][main] Starting main'.format(formatted_time))
bmp280_0 = bmp280.BMP280(delay_sample - 0.01)
dht11_0 = dht11.DHT11(pin=4)
mpu9250_0 = mpu9250.MPU9250()
servo_0 = servo.Servo(0)
deployer_0 = parachute_deployer.Deployer(bmp280_0, servo_0, altitude_dif,
delay_sample)
logger_0 = data_logger.Logger(bmp280_0, dht11_0, mpu9250_0)
camera_0 = camera.Camera()
deployer_thread = threading.Thread(target=deployer_0.main)
logging_thread = threading.Thread(target=logger_0.log)
deployer_thread.start()
logging_thread.start()
camera_0.start_recording()
finisher_input_thread = threading.Thread(target=finisher_input,
args=(deployer_0, logger_0,
camera_0),
daemon=True)
finisher_time_thread = threading.Thread(target=finisher_time,
args=(deployer_0, logger_0,
camera_0, max_time),
daemon=True)
finisher_input_thread.start()
finisher_time_thread.start()
def __init__(self, pins, positionTask):
self.position = positionTask
#self.sensorOffset = array.array('f', [ -45 , -45 , 45 , 45 ] ) # Angle of each sensor wrt servo [deg]
self.sensorOffset = array.array(
'f', [45]) # Angle of each sensor wrt servo [deg]
self.servo = servo.Servo(1, offset=0)
self.angles = array.array(
'f', [-90, 90, 15]) # start angle, max angle, inc angle
#self.angles = array.array('f', [0, 0, 15] ) # start angle, max angle, inc angle
self.servoWait = 200 # ms of wait time for servo to reach new angle
# Create rangeFinder driver objects
self.rangeFinders = [
#rangeFinder.RangeFinder( pins["Rangefinder 4"] , [0, 0, 0] ), # Top Left
#rangeFinder.RangeFinder( pins["Rangefinder 3"] , [0, 55, -2]), # Bottom Left
#rangeFinder.RangeFinder( pins["Rangefinder 2"] , [0, 0, 0] ), # Top Right
rangeFinder.RangeFinder(pins["Rangefinder 3"],
[7, 50.0, -1.97]) # Bottom Right
]
self.servo.setAngle(self.angles[0])
self.runTime = pyb.millis() + self.servoWait
self.bluetooth = pyb.UART(2, 9600)
self.bluetooth.init(9600, bits=8, parity=None, stop=1)
return
async def test_diagnostics_request_404(
monkeypatch, optimizer: servo.configuration.Optimizer) -> None:
# Simulate running as a k8s pod
monkeypatch.setenv("POD_NAMESPACE", "test-namespace")
# NOTE: Can't use servo_runner fixture; its init happens prior to setting of env var above
servo_runner = servo.runner.ServoRunner(
servo.Servo(
config=servo.BaseServoConfiguration(name="archibald",
optimizer=optimizer),
connectors=[], # Init empty servo
))
request = respx.get(
f"https://api.opsani.com/accounts/dev.opsani.com/applications/servox/{servo.telemetry.DIAGNOSTICS_CHECK_ENDPOINT}"
).mock(return_value=httpx.Response(404, text=f'{{"data": "WITHHOLD"}}'))
request = await servo.telemetry.DiagnosticsHandler(
servo_runner.servo)._diagnostics_api(
method="GET",
endpoint=servo.telemetry.DIAGNOSTICS_CHECK_ENDPOINT,
output_model=servo.telemetry.DiagnosticStates,
)
assert isinstance(request, servo.telemetry.DiagnosticStates)
assert request == servo.telemetry.DiagnosticStates.withhold
def servo_example():
# Pin 18
servo_1 = servo.Servo(0)
time.sleep(1)
servo_1.change_pos(0.5)
time.sleep(1)
servo_1.stop()
GPIO.cleanup()
def __init__(self, servoPin, minimum, maximum, position):
self.servoPin = machine.Pin(servoPin)
self.minimum = minimum
self.maximum = maximum
self.position = position
self.servo = servo.Servo(self.servoPin, freq=50)
self.servo.writeAngle(self.position)
def test_set_angle(self):
print('** test_set_angle **')
s = servo.Servo()
a = s.angle
print('before angle:', a)
s.set_angle(100)
b = s.angle
print('after angle:', b)
result = (b == a)
self.assertFalse(result == True)
def __init__(self, filename):
stream = file(filename, 'r')
self.data = yaml.load(stream)
self.realClaw = {}
for servoData in self.data['real_robot']:
self.realClaw[servoData['port']] = servo.Servo(servoData)
self.joint = []
for m in self.data['robot_arm']:
self.joint.append(
dhmatrix.DHMatrix(m['theta'], m['d'], m['r'], m['alpha'],
m['transform']['scale'],
m['transform']['offset'],
self.realClaw[m['bind_port']]))
self.port = self.data['backend_server']['port']
self.realClawRotate = servo.Servo(self.data['real_claw_rotate'])
self.realClaw = servo.Servo(self.data['real_claw'])
def __init__(self):
self._interrupted = False
self._lock = threading.RLock()
self._cmdThread = None
self.motor = Motor.Motor()
self.sonic = Ultrasonic.Ultrasonic()
self.adc = ADC.Adc()
self.servo = servo.Servo()
self.stop()
self.pointSonic()
def __init__(self, controller):
# Parse the YAML file to get the actual servo descriptions
servo_desc_fname = "marie_servo_descriptions.yaml"
with open(servo_desc_fname, 'r') as f:
self.servo_descriptions = yaml.load(f, Loader=yaml.FullLoader)
facial_expressions_fname = "expressions_gestures.yaml"
with open(facial_expressions_fname, 'r') as f:
exp_gestures = yaml.load(f, Loader=yaml.FullLoader)
self.expressions = exp_gestures['expressions']
self.servos = {}
for name in self.servo_descriptions.keys():
self.servos[name] = servo.Servo(name, controller)
def paid():
if not request.json:
abort(400)
content = request.json
if content['security_id'] == SECURITY_ID:
serv = servo.Servo()
serv.open()
return jsonify({'state': 'open'}), 201
else:
abort(400)
def __init__(self, controller):
self.arduinoController = controller
self.thumb = servo.Servo(controller)
self.index = servo.Servo(controller)
self.majeure = servo.Servo(controller)
self.ringfinger = servo.Servo(controller)
self.pinky = servo.Servo(controller)
self.wrist = servo.Servo(controller)
def __init__(self, debug = False):
# data for horizontal (0) servo
self.hor_cur = 90
self._hor_min = 50
self._hor_max = 130
# data for vertical (1) servo
self.ver_cur = 90
self._ver_min = 80
self._ver_max = 120
# handle for servor driver
self._pwm = servo.Servo()
self.debug = debug
def main():
profile_list = [
AnimalProfile("0782B18622", "Jim Kirk", 0, 0, session_save_path),
AnimalProfile("0782B182D6", "Yuri Gagarin", 0, 0, session_save_path),
AnimalProfile("0782B17DE9", "Elon Musk", 0, 0, session_save_path),
AnimalProfile("0782B18A1E", "Buzz Aldrin", 0, 0, session_save_path),
AnimalProfile("5643564457", "Captain Picard", 0, 0, session_save_path)
]
# Initializing servo, camera and RFID reader and session controller.
servo_1 = servo.Servo(servo_PWM_BCM_pin_number)
camera_1 = camera.Camera(fourcc, camera_index, camera_fps, camera_res)
RFID_1 = RFID_Reader(serial_interface_path, baudrate, RFID_proximity_BCM_pin_number)
session_controller = SessionController(profile_list, serial, servo_1, camera_1, RFID_1)
# Main loop until I implement a GUI or something
while True:
RFID_code = session_controller.RFID_reader.listenForRFID()
profile = session_controller.searchForProfile(RFID_code)
session_controller.startSession(profile)
#!/usr/bin/env python
from importlib import import_module
import os
from flask import Flask, render_template, Response, request, session, escape, redirect, url_for
import servo
from camera_pi import Camera
app = Flask(__name__)
my_servo = servo.Servo()
@app.route('/')
def index():
"""Video streaming home page."""
# return render_template('index.html')
if "user" in session and escape(session["user"]) == "stan":
return render_template('index.html')
else:
session["user"] = "******"
return render_template("login.html")
@app.route("/login", methods=["POST"])
def login():
user = request.form["user"]
password = request.form["password"]
print(user, password)
if user == "Stan" and password == "112358":
session["user"] = "******"
return redirect("/")
def test_servo_create(self):
s = servo.Servo()
result = s.pin
self.assertTrue(result == 1)
xmax = round(center[0] + ((TRACK_LENGTH_MM/2) * pix_per_mm) + PIXEL_PADDING)
xmin = round(center[0] - ((TRACK_LENGTH_MM/2) * pix_per_mm) - PIXEL_PADDING)
ymax = round(center[1] + (ARUCO_TO_CENTER_MM + (TRACK_WIDTH_MM/2)) * pix_per_mm) + 10
ymin = round(center[1] + (ARUCO_TO_CENTER_MM - (TRACK_WIDTH_MM/2)) * pix_per_mm) - 10
# Adjust bounds if they exceed original image size
xmax = min(xmax, VID_WIDTH)
xmin = max(xmin, 0)
ymax = min(ymax, VID_HEIGHT)
ymin = max(ymin, 0)
return xmin, xmax, ymin, ymax
# Setup servo range
servo_1 = servo.Servo(SERVO_PWM_STEPS, inverted=True, n90_val=39300, p90_val=35500)
ser = None
cap = None
try:
# Set up and Open Serial Port
ser = serial.Serial(SERIAL_PORT, SERIAL_BAUD, timeout=1)
ser.close()
ser.open()
# Set up and open webcam
cap = cv2.VideoCapture(0)
cap.set(3, VID_WIDTH)
cap.set(4, VID_HEIGHT)
def __init__(self):
super(Autopilot, self).__init__()
# setup all processes to exit on any signal
self.childpids = []
def cleanup(signal_number, frame=None):
print('got signal', signal_number, 'cleaning up')
while self.childpids:
pid = self.childpids.pop()
#print('kill child', pid)
os.kill(pid, signal.SIGTERM) # get backtrace
sys.stdout.flush()
if signal_number != 'atexit':
raise KeyboardInterrupt # to get backtrace on all processes
# unfortunately we occasionally get this signal,
# some sort of timing issue where python doesn't realize the pipe
# is broken yet, so doesn't raise an exception
def printpipewarning(signal_number, frame):
print('got SIGPIPE, ignoring')
import signal
for s in range(1, 16):
if s == 13:
signal.signal(s, printpipewarning)
elif s != 9:
signal.signal(s, cleanup)
# self.server = SignalKServer()
self.server = SignalKPipeServer()
self.boatimu = BoatIMU(self.server)
self.sensors = Sensors(self.server)
self.servo = servo.Servo(self.server, self.sensors)
self.version = self.Register(Value, 'version',
'pypilot' + ' ' + strversion)
self.heading_command = self.Register(HeadingProperty,
'heading_command', 0)
self.enabled = self.Register(BooleanProperty, 'enabled', False)
self.lastenabled = False
self.lost_mode = self.Register(BooleanValue, 'lost_mode', False)
self.mode = self.Register(ModeProperty, 'mode')
self.mode.ap = self
self.lastmode = False
self.last_heading = False
self.last_heading_off = self.boatimu.heading_off.value
self.pilots = []
for pilot_type in pilots.default:
self.pilots.append(pilot_type(self))
self.pilot = self.Register(EnumProperty,
'pilot',
'basic',
['simple', 'basic', 'learning', 'wind'],
persistent=True)
timestamp = self.server.TimeStamp('ap')
self.heading = self.Register(SensorValue,
'heading',
timestamp,
directional=True)
self.heading_error = self.Register(SensorValue, 'heading_error',
timestamp)
self.heading_error_int = self.Register(SensorValue,
'heading_error_int', timestamp)
self.heading_error_int_time = time.time()
self.tack = tacking.Tack(self)
self.gps_compass_offset = HeadingOffset()
self.gps_speed = self.Register(SensorValue, 'gps_speed', timestamp)
self.wind_compass_offset = HeadingOffset()
self.true_wind_compass_offset = HeadingOffset()
self.wind_direction = self.Register(SensorValue,
'wind_direction',
timestamp,
directional=True)
self.wind_speed = self.Register(SensorValue, 'wind_speed', timestamp)
self.runtime = self.Register(TimeValue, 'runtime') #, persistent=True)
device = '/dev/watchdog0'
self.watchdog_device = False
try:
self.watchdog_device = open(device, 'w')
except:
print('warning: failed to open special file', device,
'for writing')
print(' cannot stroke the watchdog')
if os.system('sudo chrt -pf 1 %d 2>&1 > /dev/null' % os.getpid()):
print('warning, failed to make autopilot process realtime')
self.starttime = time.time()
self.times = 4 * [0]
self.childpids = [
self.boatimu.imu_process.pid, self.boatimu.auto_cal.process.pid,
self.server.process.pid, self.sensors.nmea.process.pid,
self.sensors.gps.process.pid
]
signal.signal(signal.SIGCHLD, cleanup)
import atexit
atexit.register(lambda: cleanup('atexit'))
self.lastdata = False
self.lasttime = time.time()
def main():
"""Main method creates a TJ bot and starts it along with the console_input.
main method
"""
F = open("convo_commands.txt", "w")
thick_line = "================================================================================\n"
thin_line = "--------------------------------------------------------------------------------\n"
dash_line = "- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \n"
F.write(thick_line)
F.write("Commands for MusicManager\n")
F.write(thick_line)
mm = music.MusicManager()
for m in dir(mm):
details = eval("pydoc.render_doc(mm." + m + ")")
details = fix_line(details)
out = "music." + details
F.write(out)
ss = music.Song("./resources/output.wav")
for s in dir(ss):
details = eval("pydoc.render_doc(ss." + s + ")")
details = fix_line(details)
out = "music." + details
F.write(out)
F.write(thick_line)
F.write("Commands for LedManager\n")
F.write(thick_line)
lm = led.LedManager()
for m in dir(lm):
details = eval("pydoc.render_doc(lm." + m + ")")
details = fix_line(details)
out = "led." + details
F.write(out)
lm = led.NeoPixel()
for m in dir(lm):
details = eval("pydoc.render_doc(lm." + m + ")")
details = fix_line(details)
out = "led." + details
F.write(out)
F.write(thick_line)
F.write("Commands for ServoManager\n")
F.write(thick_line)
lm = servo.ServoManager()
for m in dir(lm):
details = eval("pydoc.render_doc(lm." + m + ")")
details = fix_line(details)
out = "servo." + details
F.write(out)
se = servo.Servo()
for m in dir(se):
details = eval("pydoc.render_doc(se." + m + ")")
details = fix_line(details)
out = "servo." + details
F.write(out)
import servo, time
s = servo.Servo(1)
while True:
try:
print("0")
s.set(0)
time.sleep(2)
print("90")
s.set(90)
time.sleep(2)
except KeyboardInterrupt:
s.set(0)
break
def main():
try:
stt = streaming.StreamingSTT(
# replace with speech to text credentials username
'9d28bb42-fd0f-4126-ac66-2d0882fbe7f8',
# replace with speech to text credentials password
'Gixj8klOaqK4')
except:
fatalFailure()
try:
tts = textToSpeech.TextToSpeech(
# replace with text to speech credentials username
'6fa627fb-384e-4586-9a72-185b70c1f09a',
# replace with text to speech credentials password
'zQYWSqHEA7dm')
except:
fatalFailure()
try:
convo = conversation.Conversation(
# replace with conversation credentials username
'bdce62cf-d7f7-4cbc-8b0b-dee6950d1c01',
# replace with conversation credentials password
'K5Cmn1aJ3He8',
# replace with workspace ID.
'0c3cdb7b-3258-457e-a3b7-1b1236fea9c4')
except:
fatalFailure()
# replace with robot name
name = 'Bonnie'
servo_obj = servo.Servo()
servoP = servoProcess.ServoProcess(servo_obj)
led_obj = led.Led()
ledP = ledProcess.LedProcess(led_obj)
music_obj = music.Music("/home/pi/tj-python/resources/music.wav")
musicP = musicProcess.MusicProcess(music_obj)
time.sleep(.5)
ledP.red()
time.sleep(.5)
ledP.green()
time.sleep(.5)
ledP.blue()
time.sleep(.5)
ledP.rainbowCycle(.0001, 99999999999999)
# tts.speak("Hello, just a moment while I boot up")
servoP.wave(3)
"""l = led.Led()
le = ledProcess.LedProcess(l)
print('sleeping')
time.sleep(3)
print('done sleeping, Lets Strobe')
le.strobe()
#le.customColor(255,0,0)
print('sleeping')
time.sleep(3)
print('kill in 2')
time.sleep(2)
le.customColor(255,0,0)
#le.stop()
print('dead') """
tts.speak('Hello my name is ' + name + ' I am the CBU admissions bot')
while True:
ledP.blue()
try:
phrase = stt.get_phrase()
except:
ledP.red()
ledP.orange()
if (name in phrase) or ('bunny' in phrase) or ('body' in phrase) or (
'Bani' in phrase):
response = convo.sendMessage(phrase)
ledP.green()
#response = response.upper()
if '~' in response:
print('Command Found')
if '~RED' in response:
ledP.red()
response = response.replace('~RED', '', 1)
if '~ORANGE' in response:
ledP.orange()
response = response.replace('~ORANGE', '', 1)
if '~YELLOW' in response:
ledP.yellow()
response = response.replace('~YELLOW', '', 1)
if '~GREEN' in response:
ledP.green()
response = response.replace('~GREEN', '', 1)
if '~BLUE' in response:
print('Its Blue')
ledP.blue()
response = response.replace('~BLUE', '', 1)
if '~PURPLE' in response:
ledP.purple()
response = response.replace('~PURPLE', '', 1)
if '~PINK' in response:
ledP.pink()
response = response.replace('~PINK', '', 1)
if '~WHITE' in response:
ledP.white()
response = response.replace('~WHITE', '', 1)
if '~RAINBOW' in response:
ledP.rainbow()
response = response.replace('~RAINBOW', '', 1)
if '~RAINBOWCYCLE' in response:
ledP.rainbowCycle()
response = response.replace('~RAINBOWCYCLE', '', 1)
if '~MUSICPLAY' in response:
musicP.play()
response = response.replace('~MUSICPLAY', '', 1)
if '~MUSICSTOP' in response:
musicP.stop()
response = response.replace('~MUSICSTOP', '', 1)
if '~LEDOFF' in response:
ledP.off()
response = response.replace('~LEDOFF', '', 1)
if '~ARMSTOP' in response:
servoP.stop()
response = response.replace('~ARMSTOP', '', 1)
if '~ARMUP' in response:
servoP.armUp()
response = response.replace('~ARMUP', '', 1)
if '~ARMDOWN' in response:
servoP.armDown()
response = response.replace('~ARMDOWN', '', 1)
if '~DANCE' in response:
servoP.wave(10)
ledP.rainbowCycle(1, 50)
response = response.replace('~DANCE', '', 1)
if '~ARMANGLE' in response:
response = response.replace('~ARMANGLE', '', 1)
param = int(response.split("~", 1)[0])
response = response.split("~", 1)[1]
servoP.angle(param)
if '~ARMWAVECOUNT' in response:
response = response.replace('~ARMWAVECOUNTARMANGLE', '', 1)
param = int(response.split("~", 1)[0])
response = response.split("~", 1)[1]
servoP.wave(param)
if '~ARMWAVE' in response:
servoP.wave(2)
response = response.replace('~ARMWAVE', '', 1)
if response == '':
response = 'awkward silence'
ledP.pink()
tts.speak(response)
def __init__(self):
super(Autopilot, self).__init__()
self.watchdog_device = False
self.server = pypilotServer()
self.client = pypilotClient(self.server)
self.boatimu = BoatIMU(self.client)
self.sensors = Sensors(self.client)
self.servo = servo.Servo(self.client, self.sensors)
self.version = self.register(Value, 'version', 'pypilot' + ' ' + strversion)
self.timestamp = self.client.register(SensorValue('timestamp', 0))
self.starttime = time.monotonic()
self.mode = self.register(ModeProperty, 'mode')
self.preferred_mode = self.register(Value, 'preferred_mode', 'compass')
self.lastmode = False
self.mode.ap = self
self.heading_command = self.register(HeadingProperty, 'heading_command', self.mode)
self.enabled = self.register(BooleanProperty, 'enabled', False)
self.lastenabled = False
self.last_heading = False
self.last_heading_off = self.boatimu.heading_off.value
self.pilots = {}
for pilot_type in pilots.default:
try:
pilot = pilot_type(self)
self.pilots[pilot.name] = pilot
except Exception as e:
print('failed to load pilot', pilot_type, e)
pilot_names = list(self.pilots)
print('Loaded Pilots:', pilot_names)
self.pilot = self.register(EnumProperty, 'pilot', 'basic', pilot_names, persistent=True)
self.heading = self.register(SensorValue, 'heading', directional=True)
self.heading_error = self.register(SensorValue, 'heading_error')
self.heading_error_int = self.register(SensorValue, 'heading_error_int')
self.heading_error_int_time = time.monotonic()
self.tack = tacking.Tack(self)
self.gps_compass_offset = HeadingOffset()
self.gps_speed = 0
self.wind_compass_offset = HeadingOffset()
self.true_wind_compass_offset = HeadingOffset()
self.wind_direction = self.register(SensorValue, 'wind_direction', directional=True)
self.wind_speed = 0
self.runtime = self.register(TimeValue, 'runtime') #, persistent=True)
self.timings = self.register(SensorValue, 'timings', False)
device = '/dev/watchdog0'
try:
self.watchdog_device = open(device, 'w')
except:
print('warning: failed to open special file', device, 'for writing')
print(' cannot stroke the watchdog')
self.server.poll() # setup process before we switch main process to realtime
if os.system('sudo chrt -pf 1 %d 2>&1 > /dev/null' % os.getpid()):
print('warning, failed to make autopilot process realtime')
self.lasttime = time.monotonic()
# setup all processes to exit on any signal
self.childprocesses = [self.boatimu.imu, self.boatimu.auto_cal,
self.sensors.nmea, self.sensors.gpsd,
self.sensors.signalk, self.server]
def cleanup(signal_number, frame=None):
#print('got signal', signal_number, 'cleaning up')
if signal_number == signal.SIGCHLD:
pid = os.waitpid(-1, os.WNOHANG)
#print('sigchld waitpid', pid)
if signal_number != 'atexit': # don't get this signal again
signal.signal(signal_number, signal.SIG_IGN)
while self.childprocesses:
process = self.childprocesses.pop().process
if process:
pid = process.pid
#print('kill', pid, process)
try:
os.kill(pid, signal.SIGTERM) # get backtrace
except Exception as e:
pass
#print('kill failed', e)
sys.stdout.flush()
if signal_number != 'atexit':
raise KeyboardInterrupt # to get backtrace on all processes
# unfortunately we occasionally get this signal,
# some sort of timing issue where python doesn't realize the pipe
# is broken yet, so doesn't raise an exception
def printpipewarning(signal_number, frame):
print('got SIGPIPE, ignoring')
import signal
for s in range(1, 16):
if s == 13:
signal.signal(s, printpipewarning)
elif s != 9:
signal.signal(s, cleanup)
signal.signal(signal.SIGCHLD, cleanup)
import atexit
atexit.register(lambda : cleanup('atexit'))
def __init__(self, number):
self.servo = servo.Servo(number)
# Instantiate Temperature sensor
#Address of TMP007 Temperature Sensor
temp007Address = 64
tempSensor = temp007.Temp007(i2c, temp007Address)
util.msg('Temp007 instantiated, name: tempSensor')
# Instantiate Proximity sensor
#Address of VCNL4010 Proximity Sensor
proxAddress = 19
proxSensor = vcnl4010.Vcnl4010(i2c, proxAddress)
proxSensor.setup()
util.msg('Vcnl instantiated, name: proxSensor')
# Instantiate 2 servos
top = servo.Servo(machine.Pin(12), 50, 350, 2400, 180)
bottom = servo.Servo(machine.Pin(0), 50, 350, 2400, 180)
util.msg('2 servos instantiated, name: top; bottom')
# Instantiate servo controller
servoCtrl = servoController.ServoController(top, bottom)
util.msg('Servos controller instantiated, name: servoCtrl')
# Connecting to the Wifi
networkUtil.wifiConnect('MotoG3', 'embedded') #('EEERover', 'exhibition')
# #Grab the time from the MQTT broker and set the RTC
# timeSet = networkUtil.Publisher('192.168.0.10', defaultTopic = 'esys/time')
# timeSet.subscribe()
# Publisher for Embedded system class demo
def __init__(self):
# Initialise the PCA9685 using the default address (0x40).
#self.pwm = Adafruit_PCA9685.PCA9685()
# Set frequency to 60hz, good for servos.
#self.pwm.set_pwm_freq(60)
self.throttleServo = servo.Servo(0, throttle_servo_min,
throttle_servo_max, false)
self.aileronServo = servo.Servo(1, aileron_servo_min,
aileron_servo_max, false)
self.elevatorServo = servo.Servo(2, elevator_servo_min,
elevator_servo_max, true)
self.rudderServo = servo.Servo(0, rudder_servo_min, rudder_servo_max,
false)
IMU.detectIMU()
IMU.writeACC(LSM9DS1_CTRL_REG7_XL, 0b11100001)
self.gyroXangle = 0.0
self.gyroYangle = 0.0
self.gyroZangle = 0.0
self.CFangleX = 0.0
self.CFangleY = 0.0
self.CFangleZ = 0.0 # Uneeded in new?
self.CFangleXFiltered = 0.0
self.CFangleYFiltered = 0.0
self.kalmanX = 0.0
self.kalmanY = 0.0
self.oldXMagRawValue = 0
self.oldYMagRawValue = 0
self.oldZMagRawValue = 0
self.oldXAccRawValue = 0
self.oldYAccRawValue = 0
self.oldZAccRawValue = 0
# Setup the tables for the median filter. Fill them all with '1' so we don't get a divide by zero error
self.acc_medianTable1X = [1] * ACC_MEDIANTABLESIZE
self.acc_medianTable1Y = [1] * ACC_MEDIANTABLESIZE
self.acc_medianTable1Z = [1] * ACC_MEDIANTABLESIZE
self.acc_medianTable2X = [1] * ACC_MEDIANTABLESIZE
self.acc_medianTable2Y = [1] * ACC_MEDIANTABLESIZE
self.acc_medianTable2Z = [1] * ACC_MEDIANTABLESIZE
self.mag_medianTable1X = [1] * MAG_MEDIANTABLESIZE
self.mag_medianTable1Y = [1] * MAG_MEDIANTABLESIZE
self.mag_medianTable1Z = [1] * MAG_MEDIANTABLESIZE
self.mag_medianTable2X = [1] * MAG_MEDIANTABLESIZE
self.mag_medianTable2Y = [1] * MAG_MEDIANTABLESIZE
self.mag_medianTable2Z = [1] * MAG_MEDIANTABLESIZE
# Kalman filter variables
self.y_bias = 0.0
self.x_bias = 0.0
self.XP_00 = 0.0
self.XP_01 = 0.0
self.XP_10 = 0.0
self.XP_11 = 0.0
self.YP_00 = 0.0
self.YP_01 = 0.0
self.YP_10 = 0.0
self.YP_11 = 0.0
self.KFangleX = 0.0
self.KFangleY = 0.0
self.pressure = 0.0
self.altitude = 0.0
# Moving average filter settings for Roll, Pitch, Yaw readings
self.combined_filter_length = 1
self.CFangleX_filter_length = self.combined_filter_length
self.CFangleY_filter_length = self.combined_filter_length
self.CFangleZ_filter_length = self.combined_filter_length
self.tiltHeading_filter_length = self.combined_filter_length
self.CFangleX_filtered = 0.0
self.CFangleX_reading = [0.0] * self.CFangleX_filter_length
self.CFangleY_filtered = 0.0
self.CFangleY_reading = [0.0] * self.CFangleY_filter_length
self.CFangleZ_filtered = 0.0
self.CFangleZ_reading = [0.0] * self.CFangleZ_filter_length
self.tiltHeading_filtered = 0.0
self.tiltHeading_reading = [0.0] * self.tiltHeading_filter_length
self.time_a = datetime.datetime.now()
#######################
import configparser
import servo
# Configuration importation
###########################
config = configparser.ConfigParser()
config.read("config.ini")
SERVO_NAME_LIST = config["SERVO"]["SERVO_LIST"].split(", ")
# Main code
###########
# Gui declaration
gui = servo.Gui()
# Servo declaration
servo_list = []
for x in SERVO_NAME_LIST:
servo_list.append(servo.Servo(x, gui.root, SERVO_NAME_LIST.index(x)))
# GUI servo objects declaration
gui.buildServoObjects(servo_list)
# GUI Grid
gui.guiGrid()
# GUI startup
gui.start()
