""" initial time stamp """
timeSTMP0.timeSTMP()
""" Open .dat file to record values """
f = open('writeTest.dat', 'w')
f.write(
str('t') + '\t' + str('rollEr') + '\t' + str('pitchEr') + '\t' +
str('ADC_Ch1') + '\t' + str('ADC_Ch2') + '\t' + str('pitchPOT') +
'\t' + str('heavePOT') + '\t' + str('q') + '\t' + str('mot1_pos') +
'\t' + str('mot2_pos') + '\n') #Indices
""" The Main Loop """
while (True):
""" Measure current time and update 't' """
timeSTMP1.timeSTMP() # Current CPU time
t = timeSTMP1.STMP - timeSTMP0.STMP # Current CPU time - Initial time stamp
""" Measure an Euler angles """
EulerAng = sensor.EBIIMURead(ser)
""" Read ADC values from the potentiometers """
ADC_Ch1 = ADC.read("AIN1") # [ADC Value]
ADC_Ch2 = ADC.read("AIN2") # [ADC Value]
""" convert ADC value into physical size """
pitchPOT = -307 * ADC_Ch1 + 142.2 # [deg]
heavePOT = -720.53 * ADC_Ch2 + 969.47 # [mm]
""" Update the error matrices (2 x 2) """
rollEr[0] = rollEr[1]
rollEr[1] = [t, EulerAng[0]]
pitchEr[0] = pitchEr[1]
pitchEr[1] = [t, EulerAng[1]]
""" Derivative control """
ctrlRoll.dCtrl(rollEr[0, 0], rollEr[1, 0], rollEr[0, 1], rollEr[1, 1])
ctrlPitch.dCtrl(pitchEr[0, 0], pitchEr[1, 0], pitchEr[0, 1],
pitchEr[1, 1])
""" Python Modules """
import serial
import time
""" pyBBIO """
import Adafruit_BBIO.UART as UART
""" Developed Module """
import sensor
""" Use UART1 """
UART.setup("UART1")
""" Set Port and Baud-rate"""
ser = serial.Serial(port="/dev/ttyO1", baudrate=115200)
ser.close() # Close the UART1 if it is opened
ser.open()
if ser.isOpen():
while (True):
print(sensor.EBIIMURead(ser)) # Print roll,pitch and yaw
ser.close()