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))