def __init__(self):
GPIO.setwarnings(False)
GPIO.setmode(GPIO.BCM)
self.GPIO_4 = 4
GPIO.setup(self.GPIO_4, GPIO.OUT)
GPIO.output(self.GPIO_4, False)
self.imu = IMU()
self.servo = Servo()
self.move_flag = 0x01
self.relax_flag = False
self.pid = Incremental_PID(0.500, 0.00, 0.0025)
self.flag = 0x00
self.timeout = 0
self.height = -25
self.body_point = [[137.1, 189.4, self.height], [225, 0, self.height],
[137.1, -189.4, self.height],
[-137.1, -189.4,
self.height], [-225, 0, self.height],
[-137.1, 189.4, self.height]]
self.calibration_leg_point = self.readFromTxt('point')
self.leg_point = [[140, 0, 0], [140, 0, 0], [140, 0, 0], [140, 0, 0],
[140, 0, 0], [140, 0, 0]]
self.calibration_angle = [[0, 0, 0], [0, 0, 0], [0, 0, 0], [0, 0, 0],
[0, 0, 0], [0, 0, 0]]
self.angle = [[90, 0, 0], [90, 0, 0], [90, 0, 0], [90, 0, 0],
[90, 0, 0], [90, 0, 0]]
self.order = ['', '', '', '', '', '']
self.calibration()
self.setLegAngle()
self.Thread_conditiona = threading.Thread(target=self.condition)
def __init__(self): self.motors = motors.Motors() self.IMU = IMU.IMU() self.comms = comms.Comms() self.name = self.comms.getHostname() self.camera = camera.Camera() self.nearbyRobots = set() self.knownTargets = set()
def _make_IMUs(self):
"""
generate IMU models
:return: None
"""
if "Devices" in self.data_dict:
sensors = self.data_dict["Devices"]
if "IMU" in sensors:
keys = self._filter_dict(sensors, 'IMU')
for key in keys:
self._IMUs[int(filter(str.isdigit, key))] = IMU.IMU(key, sensors[key])
else:
print "No IMUs"
else:
print "No Devices"
def __init__(self):
self.imu = IMU()
self.servo = Servo()
self.pid = Incremental_PID(0.5, 0.0, 0.0025)
self.speed = 8
self.height = 99
self.timeout = 0
self.move_flag = 0
self.move_count = 0
self.move_timeout = 0
self.order = ['', '', '', '', '']
self.point = [[0, 99, 10], [0, 99, 10], [0, 99, -10], [0, 99, -10]]
self.calibration_point = self.readFromTxt('point')
self.angle = [[90, 0, 0], [90, 0, 0], [90, 0, 0], [90, 0, 0]]
self.calibration_angle = [[0, 0, 0], [0, 0, 0], [0, 0, 0], [0, 0, 0]]
self.relax_flag = True
self.balance_flag = False
self.attitude_flag = False
self.Thread_conditiona = threading.Thread(target=self.condition)
self.calibration()
self.relax(True)
def __init__(self,
callback,
max_power,
goal,
debug=False,
calibration=None):
self.imu = IMU()
self.connected = self.imu.connect()
self.goalNumber = 1
self.totalGoal = goal
self.goal = self.totalGoal[self.goalNumber]
self.coordinates = self.totalGoal[0]
self.max_power = max_power / 100
self.debug = debug
self.t = None
self.running = False
self.cb = callback
self.goalAngle = None
self.boatAngle = None
self.boatNorthOffset = 0
self.calibration = calibration
def pid_controller(P=1, I=1, D=1):
T = 1 #Periodo
numlecturas = 10
#lecturas=[0]*numlecturas
#Inicialización
ui_prev = 0
e_prev = 0
e_acumulado = 0
ti = time.clock() #tiempo inicial
Referencia = 0
#Actuadores
motor1 = motor.motor(1)
motor2 = motor.motor(2)
#IMU
imu1 = IMU.IMU()
while True:
# Error between the desired and actual output
ta = time.clock()
sensor = 0
for i in range(numlecturas):
sensor += imu1.SetAcc[1]
i += 1
sensor /= numlecturas
if ta - ti >= T:
e_actual = sensor - Referencia
e_acumulado += e_actual
diferencia_e = e_actual - e_prev
control_PID = e_actual * P + e_acumulado * I + diferencia_e * D
# Adjust previous values
e_prev = e_actual
ti = time.clock()
from IMU import *
from picamera import PiCamera
from Helper import *
from scipy import ndimage
from scipy import misc
log = LogHelper()
log.logInfo('Data capturing started...')
log.logInfo('******************************************************')
GPIO.setmode(GPIO.BCM)
GPIO.setup(17,GPIO.IN)
imu = IMU()
def worker():
imu.read()
try:
command = ''
camera = PiCamera()
camera.rotation = 180
while (True):
#if(command == 'y'):
#command = ''
if(GPIO.input(17)):
thread = threading.Thread(target=worker)
import IMU
import MotorMovement
from PressureSensor import Pressure
imu = IMU.IMU(kp_x=0,
ki_x=0,
kd_x=0,
kp_y=1,
ki_y=0,
kd_y=.2,
kp_z=1,
ki_z=.00,
kd_z=.7,
kp_acc_x=0,
ki_acc_x=0,
kd_acc_x=0,
kp_acc_y=0,
ki_acc_y=0,
kd_acc_y=0)
print(IMU.sensor.euler, IMU.sensor.acceleration)
pressure = Pressure(2.5, 0, 1.5, setpoint=1000)
motors = MotorMovement.motor_generator()
motor_powers = [0, 0, 0, 0, 0, 0]
acc_powers = [0, 0, 0, 0, 0, 0]
imu_powers = [0, 0, 0, 0, 0, 0]
pressure_powers = [0, 0, 0, 0, 0, 0]
Also opens a web socket, to which it outputs the most recent position, acceleration, and velocity in HTML. To view while running, open
a browser tab on any device and enter <Raspbery Pi's wlan IP>:<web socket port>
"""
import IMU, Encoder
import sys, socket, struct, time
from twisted.internet.protocol import Protocol, Factory, ClientFactory
from twisted.protocols.basic import LineOnlyReceiver
from twisted.internet import task, reactor
from twisted.web.server import Site
from twisted.web.resource import Resource
from ast import literal_eval
R_Enc = Encoder.Encoder(19, 26)
L_Enc = Encoder.Encoder(6, 13)
IMU1 = IMU.IMU()
# list structure that web server samples from
# TODO (suggestion): pass into PID control algorithm
payload = ['dummy'] * 4
# hostname and port setup for Motive TCP connection
hostName = 'bach.ese.wustl.edu'
host = None
defaultTwistedServerPort = 53335
# use win32 reactor if applicable
if sys.platform == 'win32':
from twisted.internet import win32eventreactor
win32eventreactor.install()
# ----------------------------------------------------------------------------------------
# SENSOR INPUT: Get LIDAR readings
[distance, bearing] = LIDAR(H_Obs, State0) # lidarData[0] = distance, lidarData[1] = bearing
# SENSOR INPUT: Get Over_Camera readings
[S_Obs_measure, target_measure, position_measure] = Over_Camera(S_Obs,
Image_W,
Image_L,
Scaling,
Target,
State0)
# -----------------------------------------------------------------------------------------------
# SENSOR INPUT: IMU readings
[th_IMU, x_IMU, y_IMU, z_IMU, vx_IMU, vy_IMU, vz_IMU] = IMU(State0);
# -----------------------------------------------------------------------------------------------
# SENSOR FUSION: Here is where we decide what goes into the State update
# Convert LIDAR input to real world coordinates
for j in range(0, N_H_Obs,1):
H_Obs[j][0] = State0[0] + distance[j] * math.cos(bearing[j]) # lidarData[1] = bearing
H_Obs[j][1] = State0[1] + distance[j] * math.sin(bearing[j]) # lidarData[0] = distance
# convert soft obstacles to real world coordinates
S_Obs[0] = S_Obs_measure[:][0] * Scaling / Image_L # S_Obs_measure[:][0] = x_S_Obs_measure
S_Obs[1] = S_Obs_measure[:][1] * Scaling / Image_W # S_Obs_measure[:][1] = y_S_Obs_measure
from termcolor import colored
from time import sleep
import math
from IMU import *
from Omnibot import *
from PID import *
imu = IMU(1)
zeroAngle = math.radians(180)
twitch = Omnibot()
def getDriveAngle(x, y, zeroAngle):
fallingAngle = math.atan2(x, y)
if fallingAngle < 0:
fallingAngle = fallingAngle + 2 * math.pi
drivingAngle = fallingAngle + math.pi + zeroAngle
if drivingAngle > 2 * math.pi:
drivingAngle = drivingAngle - 2 * math.pi
return drivingAngle
def getDriveSpeed(x, y):
drivingSpeed = 200 * (x**2 + y**2)**(.5)
return drivingSpeed
pitch = []
roll = []
pitchPIDController = PIDController()
def test_IMU(self):
expected_pitch = [0 for i in xrange(31)]
expected_pitch.extend([
-0.0000129771407, -0.0285492133501, -0.0547640005259,
-0.0792589312808, -0.1010267293485, -0.1201317682940,
-0.1376801346591, -0.1538118936673, -0.1674959918280,
-0.1799662529240, -0.1912019027219, -0.2016772549969,
-0.2116178988554, -0.2199291980645
])
expected_pitch = iter(expected_pitch)
expected_roll = [0 for i in xrange(31)]
expected_roll.extend([
-0.0000671958995, -0.0102896959721, -0.0198513431794,
-0.0287402348280, -0.0370458244604, -0.0449268315667,
-0.0511151115435, -0.0561065533944, -0.0609793194461,
-0.0650030216116, -0.0694612927996, -0.0734142553262,
-0.0780162504125, -0.0812502508942
])
expected_roll = iter(expected_roll)
expected_yaw = [0 for i in xrange(31)]
expected_yaw.extend([
0.0000557418889, -0.0025098377494, -0.0048848958068,
-0.0072329167865, -0.0094494686731, -0.0115499931893,
-0.0137438247065, -0.0160766028034, -0.0184064167549,
-0.0207389660695, -0.0229567446802, -0.0252784687839,
-0.0274220741921, -0.0298249441309
])
expected_yaw = iter(expected_yaw)
expected_direction_x = [0 for i in xrange(31)]
expected_direction_x.extend([
-0.0000557410168, 0.0028035548168, 0.0059705240700,
0.0095051050806, 0.0131781286177, 0.0169201125758, 0.0207370672208,
0.0246408741289, 0.0285292559561, 0.0323182998844, 0.0360856467136,
0.0398956163053, 0.0436991560093, 0.0474203730763
])
expected_direction_x = iter(expected_direction_x)
expected_direction_y = [1 for i in xrange(31)]
expected_direction_y.extend([
0.9999999961888, 0.9999431700721, 0.9997857284320, 0.9995444218266,
0.9992339671884, 0.9988621596827, 0.9985032123083, 0.9981593033359,
0.9977852634508, 0.9974323396333, 0.9970229077851, 0.9966163932787,
0.9961346122551, 0.9957297449738
])
expected_direction_y = iter(expected_direction_y)
expected_direction_z = [0 for i in xrange(31)]
expected_direction_z.extend([
-0.0000671958994, -0.0102853180428, -0.0198202760414,
-0.0286460600840, -0.0368484987874, -0.0445880308451,
-0.0506093643475, -0.0554150870569, -0.0600886756959,
-0.0639081762408, -0.0681406408795, -0.0718617009557,
-0.0761985395798, -0.0792059526299
])
expected_direction_z = iter(expected_direction_z)
imu = IMU.IMU(((744, -499, -491), (1857, 530, 426)))
data_file = os.path.join(DATA_PATH, 'imu_test_data')
with open(data_file, 'r') as f:
data = f.readlines()
data = map(lambda a: map(float, a.split()), data)
for p_id, data_point in enumerate(data):
imu.calc(data_point)
real_pitch = imu.angles['pitch']
real_roll = imu.angles['roll']
real_yaw = imu.angles['yaw']
(real_direction_x, real_direction_y,
real_direction_z) = imu.get_y_direction()
# I know that you want to delete this commented code,
# but don't do it it is very useful when the algorytm
# is changed more than just refactored :)
# print '%.13f,' % real_pitch,
# if p_id % 3 == 0:
# print
assertAlmostEqual(real_pitch, next(expected_pitch))
assertAlmostEqual(real_roll, next(expected_roll))
assertAlmostEqual(real_yaw, next(expected_yaw))
assertAlmostEqual(real_direction_x, next(expected_direction_x))
assertAlmostEqual(real_direction_y, next(expected_direction_y))
assertAlmostEqual(real_direction_z, next(expected_direction_z))
