#!/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()
