#!/usr/bin/env python3.4 """ Main TRANSDEC Mission """ import time from control.controller import RelativeController from auxiliary.logging import mkLog, DEBUG, STANDARD, CRITICAL log = mkLog("MISSION") from mission.framework import * def run(): controller = RelativeController() time.sleep(5) (p, r, initialHeading) = controller.sparton.read() for i in range(10): controller.step(0, 0, -5, 0, 0, initialHeading) time.sleep(0.1) while True: controller.step(50, 0, -5, 0, 0, initialHeading) if __name__ == "__main__": run()
''' Basic PID 6-DOF Controller ''' from auxiliary.logging import mkLog, STANDARD, DEBUG, CRITICAL log = mkLog('CONTROLLER') from control.pid import PIDLoop import control.config as config from sensors.dvl import DVL from sensors.sparton import Sparton import numpy as np class RelativeController: def __init__(self): self.pidX = PIDLoop(kP = config.pid.relative.kP_X, kI = config.pid.relative.kI_X, kD = config.pid.relative.kD_X) self.pidY = PIDLoop(kP = config.pid.relative.kP_Y, kI = config.pid.relative.kI_Y, kD = config.pid.relative.kD_Y) self.pidZ = PIDLoop(kP = config.pid.relative.kP_Z, kI = config.pid.relative.kI_Z, kD = config.pid.relative.kD_Z) self.pidR = PIDLoop(kP = config.pid.relative.kP_R, kI = config.pid.relative.kI_R, kD = config.pid.relative.kD_R) self.pidP = PIDLoop(kP = config.pid.relative.kP_P, kI = config.pid.relative.kI_P, kD = config.pid.relative.kD_P) self.pidQ = PIDLoop(kP = config.pid.relative.kP_Y, kI = config.pid.relative.kI_Y, kD = config.pid.relative.kD_Y) self.sparton = Sparton("/dev/ttyUSB0") self.dvl = DVL()