def __init__(self): print("Motor thread is created") self.uavServerTX = UAVServerTX() self.uavServerRX = UAVServerRX() self.imu = IMU()
def __init__(self, core_file_name): super(Listener, self).__init__() self.core_file_name = core_file_name self.setup_ui() self.setup_timers() port = SERIAL_PORT self.serial = serial.Serial(port, BAUDE_RATE, timeout=0) self.incoming_data = [] self.imu = IMU(PLATFORM_SPECIFIC_QUOTIENTS['stm']) self.stroke = Stroke() self.selector = Selector(self.core_file_name) self.acceleration_filter = AperiodicFilter(ACCELERATION_TIME_CONST) self.stroke.widget = self.display self.stroke.on_done = self.get_stroke self.previous_time = None self.data_buffer = '' self.init_selector()
class IMUMessage: def __init__(self): self.odom_pub = rospy.Publisher(PUB_TOPIC, Odometry, queue_size=50) # self.odom_broadcaster = tf.TransformBroadcaster() self.imu = IMU() self.last_distance = self.we.get_distance() self.last_time = rospy.Time.now() self.seq = 0 def tick(self): self.imu.tick() reading = self.imu.get_reading() imu_msg = Imu() imu_msg.linear_acceleration.x = reading['ax'] imu_msg.linear_acceleration.y = reading['ay'] imu_msg.linear_acceleration.z = reading['az'] imu_msg.angular_velocity.x = reading['gx'] imu_msg.angular_velocity.y = reading['gy'] imu_msg.angular_velocity.z = reading['gz'] imu_msg.orientation = quaternion_from_euler(reading['mx'], reading['my'], reading['mz']) imu_msg.header.stamp = rospy.Time.now() self.imu_msg.header.frame_id = FRAME_ID self.imu_msg.header.seq = self.seq self.pub_imu.publish(self.imu_msg) self.seq += 1
def __init__(self): self.odom_pub = rospy.Publisher(PUB_TOPIC, Odometry, queue_size=50) # self.odom_broadcaster = tf.TransformBroadcaster() self.imu = IMU() self.last_distance = self.we.get_distance() self.last_time = rospy.Time.now() self.seq = 0
def INIT(self): print("CURRENT STATE: INIT") # use Raspberry Pi board pin numbers GPIO.setmode(GPIO.BCM) GPIO.setup(mbl_bots.TRIG, GPIO.OUT) GPIO.setup(mbl_bots.ECHO, GPIO.IN) self.lastIMU = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] self.currentIMU = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] self.camera = Cam() self.imu = IMU(self.bus) self.distance = -1 signal.signal(signal.SIGINT, self.close) self.state = mbl_bots.REST time.sleep(1)
def __init__(self, parent, gui, **options): """ Initializes TkOrthoManager object @param parent @param gui @param options """ super(TkOrthoManager,self).__init__(parent, gui, **options) self.specification = gui.specification self.initDependencyManager() if(hasattr(self.gui, 'scheduler')): self.scheduler = self.gui.scheduler else: self.scheduler = Scheduler.GetInstance() if(not self.dm.installRequired()): self.pimg = self.gui.getModule('PIL.Image') self.tkimg = self.gui.getModule('PIL.ImageTk') if(hasattr(self.gui, 'imu')): self.imu = self.gui.imu else: self.imu = self.gui.imu = IMU(self.specification, self.scheduler, (not Setting.get('imu_autostart', False))) self.shapes = {} self.cache = { 'roll': {}, 'pitch': {}, 'yaw': {} } self.basepath = AmsEnvironment.AppPath() self.initImages() self.scheduler.addTask('ortho', self.updateOrtho, 0.2, True)
def showGravity(self): if (not IMU.isAvailable()): return self.unavailable() self.open() self.serviceManager() self.gridrow += 1 self.widgets['frameLabel'] = Tkinter.Label(self.widgets['tframe'],text='IMU / Gravity', anchor=NW, bg=self.colours['bg'], fg=self.colours['headingfg'], font=self.fonts['heading']) self.widgets['frameLabel'].grid(column=0,row=self.gridrow,sticky='W') self.gridrow += 1 self.widgets['gframe'] = Tkinter.Frame(self.widgets['tframe'], bg=self.colours['bg']) self.widgets['gframe'].grid(column=0,row=self.gridrow, columnspan = 3, sticky='EW') self.widgets['gframe'].columnconfigure(0, weight=1) self.widgets['tframe'].columnconfigure(0, weight=1) self.widgets['rollLabel'] = Tkinter.Label(self.widgets['gframe'],text='TBD', bg=self.colours['bg'], fg=self.colours['fg'], font=self.fonts['heading2']) self.widgets['rollLabel'].grid(column=0,row=self.gridrow,sticky='EW') self.gridrow += 1 self.widgets['hCanvas'] = Tkinter.Canvas(self.widgets['gframe'], borderwidth=0, bg=self.colours['bg'], highlightthickness=0, width=474, height=474) self.widgets['hCanvas'].grid(column=0,row=self.gridrow, padx= 10,sticky='EW') self.shapes['skyplane'] = self.widgets['hCanvas'].create_polygon([(0, 0), (474, 0), (474, 474), (0, 474)], fill='#fff') self.shapes['groundplane'] = self.widgets['hCanvas'].create_polygon(self.groundcoords, fill='#828282') self.shapes['mask'] = self.widgets['hCanvas'].create_image(self.centre.real, self.centre.imag, image=self.getImage('mask',0)) self.shapes['reticle'] = self.widgets['hCanvas'].create_image(self.centre.real, self.centre.imag, image=self.getImage('reticle',0)) self.widgets['pitchLabel'] = Tkinter.Label(self.widgets['gframe'],text='TBD', anchor=W, bg=self.colours['bg'], fg=self.colours['fg'], font=self.fonts['heading2']) self.widgets['pitchLabel'].grid(column=1,row=self.gridrow,sticky='NS')
def __init__(self, parent, gui, **options): """ Initializes TkGravityManager object @param parent @param gui @param options """ super(TkGravityManager, self).__init__(parent, gui, **options) self.initDependencyManager() if (hasattr(self.gui, 'scheduler')): self.scheduler = self.gui.scheduler else: self.scheduler = Scheduler.GetInstance() if (not self.pm.installRequired()): if (hasattr(self.gui, 'imu')): self.imu = self.gui.imu else: self.imu = self.gui.imu = IMU( self.scheduler, (not Setting.get('imu_autostart', False))) self.groundcoords = [[0, 237], [800, 237], [800, 800], [0, 800]] self.centre = complex(237, 237) self.shapes = {} self.cache = {} self.basepath = AmsEnvironment.AppPath() self.pimg = self.gui.getModule('PIL.Image') self.tkimg = self.gui.getModule('PIL.ImageTk') self.initImages() self.scheduler.addTask('gravity_display', self.updateGravity, 0.2, True)
def __init__(self, file): super(Robot, self).__init__() (self.actuators, self.sensors) = utils.file2bot(file, mbl_bots.BOTH) self.bus = smbus.SMBus(1) self.pwm = PCA9685(self.bus, 0x40, debug=False) self.pwm.setPWMFreq(50) #print("My name is: ", self.actuators[0].name) self.names_acc = [ self.actuators[i].name for i in range(len(self.actuators)) ] self.names_sen = [ self.sensors[i].name for i in range(len(self.sensors)) ] self.idx_acc = [i for i in range(len(self.actuators))] self.idx_sen = [i for i in range(len(self.sensors))] self.acc_dic = utils.genDictionary(self.names_acc, self.idx_acc) self.sen_dic = utils.genDictionary(self.names_sen, self.idx_sen) self.state = mbl_bots.INIT self.exploreState = mbl_bots.GETDATA self.movesCode = mbl_bots.NONE self.camera = Cam() self.imu = IMU(self.bus) self.vision = Vision()
def showOrthographic(self): """ view - displays orthographic ui """ if (not IMU.isAvailable()): return self.unavailable() self.open() self.serviceManager() self.gridrow += 1 self.widgets['frameLabel'] = Tkinter.Label(self.widgets['tframe'],text='IMU / Orthographic', anchor=NW, bg=self.colours['bg'], fg=self.colours['headingfg'], font=self.fonts['heading']) self.widgets['frameLabel'].grid(column=0,row=self.gridrow,sticky='W') self.gridrow += 1 self.widgets['oframe'] = Tkinter.Frame(self.widgets['tframe'], bg=self.colours['bg']) self.widgets['oframe'].grid(column=0,row=self.gridrow, columnspan = 3, sticky='EW') self.widgets['rollLabel'] = Tkinter.Label(self.widgets['oframe'],text='Roll', bg=self.colours['bg'], fg=self.colours['headingfg'], font=self.fonts['heading2']) self.widgets['rollLabel'].grid(column=0,row=self.gridrow,sticky='EW') self.widgets['pitchLabel'] = Tkinter.Label(self.widgets['oframe'],text='Pitch', bg=self.colours['bg'], fg=self.colours['headingfg'], font=self.fonts['heading2']) self.widgets['pitchLabel'].grid(column=1,row=self.gridrow,sticky='EW') self.widgets['yawLabel'] = Tkinter.Label(self.widgets['oframe'],text='Yaw', bg=self.colours['bg'], fg=self.colours['headingfg'], font=self.fonts['heading2']) self.widgets['yawLabel'].grid(column=2,row=self.gridrow,sticky='EW') self.gridrow += 1 self.widgets['rollCanvas'] = Tkinter.Canvas(self.widgets['oframe'], borderwidth=0, bg=self.colours['bg'], highlightthickness=0, width=280, height=280) self.widgets['rollCanvas'].grid(column=0,row=self.gridrow, padx= 10,sticky='NE') self.shapes['rollImage'] = self.widgets['rollCanvas'].create_image(150,150, image=self.getImage('roll',0)) self.widgets['pitchCanvas'] = Tkinter.Canvas(self.widgets['oframe'], borderwidth=0, bg=self.colours['bg'], highlightthickness=0, width=280, height=280) self.widgets['pitchCanvas'].grid(column=1,row=self.gridrow, padx= 10,sticky='NE') self.shapes['pitchImage'] = self.widgets['pitchCanvas'].create_image(150,150, image=self.getImage('pitch',0)) self.widgets['yawCanvas'] = Tkinter.Canvas(self.widgets['oframe'], borderwidth=0, bg=self.colours['bg'], highlightthickness=0, width=280, height=280) self.widgets['yawCanvas'].grid(column=2,row=self.gridrow, padx= 10,sticky='NE') self.shapes['yawImage'] = self.widgets['yawCanvas'].create_image(150,150, image=self.getImage('yaw',0))
def main(): pressure = PressureSensor() imu = IMU() telemetry = Telemetry() logger = Logger() pressure.init() imu.init() telemetry.init() tick = 0 while True: tick += 1 p = pressure.read_pressure() i = imu.read_IMU() data = "soemthing" #telemetry.send_data(data) # update all sensors pressure.update(logger, tick) imu.update(logger, tick) # log and send data telemetry.update(logger, tick)
MOSI = 24 # Master out Slave in, used to transmit FROM the Master device CS = 25 # Chip select SPI = SPI_Master(CLK, MISO, MOSI, CS) # Output pins OUT_LED = 37 MOTOR = None # TODO Set up PWM for motor GPIO.setup(OUT_LED, GPIO.OUT) # TODO Pins for UART Communication with the IMP # Setup classes for REXUS, IMU and PiCam REXUS_Comm = REXUS() PiCam_1 = PiCam() PiCam_1.video_cut = 5 IMU_1 = IMU() IMU_1.setup_default() def flash_led(): print('Flashing LEDs') for i in range(0, 5): GPIO.output(OUT_LED, GPIO.HIGH) time.sleep(0.1) GPIO.output(OUT_LED, GPIO.LOW) time.sleep(0.1) def start_of_data_storage(): '''Backs up data between both Pi's''' print('Start of data storage')
class Robot(object): def __init__(self, file): super(Robot, self).__init__() (self.actuators, self.sensors) = utils.file2bot(file, mbl_bots.BOTH) self.bus = smbus.SMBus(1) self.pwm = PCA9685(self.bus, 0x40, debug=False) self.pwm.setPWMFreq(50) #print("My name is: ", self.actuators[0].name) self.names_acc = [ self.actuators[i].name for i in range(len(self.actuators)) ] self.names_sen = [ self.sensors[i].name for i in range(len(self.sensors)) ] self.idx_acc = [i for i in range(len(self.actuators))] self.idx_sen = [i for i in range(len(self.sensors))] self.acc_dic = utils.genDictionary(self.names_acc, self.idx_acc) self.sen_dic = utils.genDictionary(self.names_sen, self.idx_sen) self.state = mbl_bots.INIT self.exploreState = mbl_bots.GETDATA self.movesCode = mbl_bots.NONE self.camera = Cam() self.imu = IMU(self.bus) self.vision = Vision() def moveAcc(self, name, pos): poss = pos * mbl_bots.SCALE_ACC + mbl_bots.CNT_ACC if (poss > self.actuators[int(self.acc_dic[name])].max): poss = self.actuators[int(self.acc_dic[name])].max if (poss < self.actuators[int(self.acc_dic[name])].min): poss = self.actuators[int(self.acc_dic[name])].min self.pwm.setServoPulse( int(self.actuators[int(self.acc_dic[name])].adress), poss) #print('Moving ', name ,'to', poss ) def executeMove(self, file, speed): moves = utils.file2move(file) for i in range(len(moves)): self.moveAcc(moves[i].actuator, moves[i].pos) if (moves[i].delay > 0.0): time.sleep(moves[i].delay * speed) def executeMoveOBO(self, file, speed, count, correction): moves = utils.file2move(file) if (count * 2 <= len(moves)): self.moveAcc(moves[count * 2].actuator, moves[count * 2].pos) self.moveAcc(moves[count * 2 + 1].actuator, moves[count * 2 + 1].pos) #if(moves[i].delay > 0.0): # time.sleep(moves[i].delay*speed) def flat(self): print("LoCoQuad is Flat") self.executeMove("/home/pi/LoCoQuad/Code/LoCoQuad_flat.movefile.txt", 1) def stand(self): print("LoCoQuad is Standing") self.executeMove("/home/pi/LoCoQuad/Code/LoCoQuad_stand.movefile.txt", 1) def walkFront(self, speed=1): print("LoCoQuad is Walking Forward") self.executeMove( "/home/pi/LoCoQuad/Code/LoCoQuad_walkFront.movefile.txt", speed) def walkRight(self, speed=1): print("LoCoQuad is Walking Right") self.executeMove( "/home/pi/LoCoQuad/Code/LoCoQuad_walkFront.movefile.txt", speed) def walkLeft(self, speed=1): print("LoCoQuad is Walking Left") self.executeMove( "/home/pi/LoCoQuad/Code/LoCoQuad_walkFront.movefile.txt", speed) def walkBack(self, speed=1): print("LoCoQuad is Walking Backwards") self.executeMove( "/home/pi/LoCoQuad/Code/LoCoQuad_walkBack.movefile.txt", speed) def turnRight(self, speed=1): print("LoCoQuad is Turning Right") self.executeMove( "/home/pi/LoCoQuad/Code/LoCoQuad_turnRight.movefile.txt", speed) self.executeMove( "/home/pi/LoCoQuad/Code/LoCoQuad_turnRight.movefile.txt", speed) self.executeMove( "/home/pi/LoCoQuad/Code/LoCoQuad_turnRight.movefile.txt", speed) def turnLeft(self, speed=1): print("LoCoQuad is Turning Left") self.executeMove( "/home/pi/LoCoQuad/Code/LoCoQuad_turnLeft.movefile.txt", speed) def sayHello(self): print("LoCoQuad is Saying Hi!") self.executeMove( "/home/pi/LoCoQuad/Code/LoCoQuad_sayHello.movefile.txt", 1) def cameraPose(self): print("LoCoQuad ready to take Picture") self.executeMove( "/home/pi/LoCoQuad/Code/LoCoQuad_cameraPose.movefile.txt", 1) def swing(self): print("LoCoQuad is Swinging") self.executeMove("/home/pi/LoCoQuad/Code/LoCoQuad_swing.movefile.txt", 1) def shake(self): print("LoCoQuad is Shaking") self.executeMove("/home/pi/LoCoQuad/Code/LoCoQuad_shake.movefile.txt", 1) def balancePos(self, count, correction=0): print("LoCoQuad is balancing") self.executeMoveOBO( "/home/pi/LoCoQuad/Code/LoCoQuad_balance.movefile.txt", 1, count, correction) def move(self, code): moves = { 1: self.walkFront, 2: self.walkBack, 3: self.walkRight, 4: self.walkLeft, 5: self.turnRight, 6: self.turnLeft, 7: self.flat, 8: self.stand, } func = moves.get(code, lambda: None) return func() def detectCatch(self, imu): data = imu.getImuRawData() print(data) if (data[3] > 3 or data[4] > 3 or data[5] > 3): print("ROBOT CATCHED...do something!!") return True else: return False def isBalanced(self): data = self.imu.getImuRawData() if (data[0] < 0.2 or data[1] < 0.2): return True else: return False
class MotorThread(): xRotation = 0 yRotation = 0 altitute = 0 #send the offset values to telemetry screen using TXServer #sequence is as we see throttleOffset = 0 forwardRightOffset = 0 forwardLeftOffset = 0 backRightOffset = 0 backLeftOffset = 0 gps_x = 37.052666 gps_y = 30.622527 temp = 56.3 motor = None imu = None hcsr04 = None motorThread = None sensorHandlerThread = None commanderThread = None uavServerTX = None uavServerRX = None motorThreadControl = 0 """ motor handler thread listening the command if command == run_motors then motors will run with minimum speed """ command = None def __init__(self): print("Motor thread is created") self.uavServerTX = UAVServerTX() self.uavServerRX = UAVServerRX() self.imu = IMU() def motorThreadStarter(self): self.sensorHandlerThread = threading.Thread(target = self.sensorHandler) self.sensorHandlerThread.start() self.commanderThread = threading.Thread(target = self.commander) self.commanderThread.start() def motorHandler(self): print("Motor thread is started!") self.hcsr04 = HCSR_04() while self.motorThreadControl == 1: self.xRotation , self.yRotation = self.imu.readMPU6050Values() try: self.altitute = self.hcsr04.measureHCSR_04() except: self.altitute = -1 try: self.throttleOffset, self.forwardRightOffset,self.forwardLeftOffset,self.backRightOffset,self.backLeftOffset = self.motor.flight(self.xRotation,self.yRotation,self.altitute,self.command) except: print("Motor thread has an exception") print("Motor thread is finished!") """ sensor handler collect the sensor data and sent datas at client using servetTX delay is 1 seconds """ def sensorHandler(self): print("Sensor Handler is started!") delay = 0.3 while True: sensorValues = {} sensorValues["xRotation"] = self.xRotation sensorValues["yRotation"] = self.yRotation sensorValues["altitute"] = self.altitute sensorValues["throttleOffset"] = self.throttleOffset sensorValues["forwardRightOffset"] = self.forwardRightOffset sensorValues["forwardLeftOffset"] = self.forwardLeftOffset sensorValues["backRightOffset"] = self.backRightOffset sensorValues["backLeftOffset"] = self.backLeftOffset sensorValues["gps_x"] = self.gps_x sensorValues["gps_y"] = self.gps_y sensorValues["temp"] = self.temp sensorValuesJsonString = json.dumps(sensorValues) try: self.uavServerTX.sendData(sensorValuesJsonString) except: print("Server TX is dropped!") break time.sleep(delay) print("Sensor handler thread is finished!") def checkCommander(self): if(self.command == 'run_motor'): self.motor = Motor() self.motor.setupMotors() self.motor.calibrate() self.motorThreadControl = 1 self.motorThread = threading.Thread(target = self.motorHandler) self.motorThread.start() elif(self.command == 'stop_motor'): self.motorThreadControl = 0 self.data = "" def commander(self): print("Commander thread is started!") while True: try: data = self.uavServerRX.getData() print("commander data is: ", data) self.command = data except: print("Server RX is dropped") break self.checkCommander() print("Commander thread is finished!")
def begin(): rospy.init_node('picar_imu', anonymous=True) e = IMU() while not rospy.is_shutdown(): e.tick()
from Bluetooth import Bluetooth from IMUtoBT import IMUtoBT from Motor import Motor from Stablizer import Stablizer print "RollE - A Ballbot" print "Final Year Project" print "Ghulam Ishaq Khan Institute of Engineering Sciences and Technology" print "https://github.com/alyyousuf7/RollE" print "" signal.signal(signal.SIGTSTP, config.cleanup) print "Press CTRL+Z to exit." print "" imu = IMU() imu.start() bt = Bluetooth() bt.start() imu2bt = IMUtoBT(imu, bt) imu2bt.start() motor1 = Motor(config.Motor1_D1, config.Motor1_D2, config.Motor1_EN, 0) motor2 = Motor(config.Motor2_D1, config.Motor2_D2, config.Motor2_EN, 1) motor3 = Motor(config.Motor3_D1, config.Motor3_D2, config.Motor3_EN, 2) stablizer = Stablizer(imu, motor1, motor2, motor3) # Send PID Values over Bluetooth
def setIMU(self): self.imu = IMU()
class SPR4(Robot): def __init__(self): super(SPR4, self).__init__("/home/pi/SPR4/SPR4_code/SPR4.botfile.txt") #Brain method --- conscience!!!! if (len(sys.argv) == 2): print("EXECUTING TEST OF MOVEMENT", str(sys.argv[1])) while True: super(SPR4, self).executeMove(str(sys.argv[1]), 1) else: while True: self.generalFSM(self.state) #============================================================================= # GENERAL FSM #============================================================================= def generalFSM(self, state): states_list = { 0: self.INIT, 1: self.REST, 2: self.EXPLORE, 3: self.SHOWOFF, 4: self.PHOTO, } func = states_list.get(state, lambda: None) return func() #============================================================================= # INIT STATE #============================================================================= def INIT(self): print("CURRENT STATE: INIT") # use Raspberry Pi board pin numbers GPIO.setmode(GPIO.BCM) GPIO.setup(mbl_bots.TRIG, GPIO.OUT) GPIO.setup(mbl_bots.ECHO, GPIO.IN) self.lastIMU = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] self.currentIMU = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] self.camera = Cam() self.imu = IMU(self.bus) self.distance = -1 signal.signal(signal.SIGINT, self.close) self.state = mbl_bots.REST time.sleep(1) #============================================================================= # REST STATE #============================================================================= def REST(self): print("CURRENT STATE: REST") start_time = time.time() while ((time.time() - start_time) < 20): if (super(SPR4, self).detectCatch(self.imu)): for i in range(5): super(SPR4, self).shake() break else: time.sleep(0.5) time.sleep(1) self.state = mbl_bots.EXPLORE #============================================================================= # EXPLORE STATE & FSM #============================================================================= def exploreFSM(self, state): substates_explorelist = { 0: self.exploreGetData, 1: self.exploreProcessData, 2: self.exploreMove, 3: self.exploreReconTurn, } func = substates_explorelist.get(state, lambda: None) return func() def EXPLORE(self): print("CURRENT STATE: EXPLORE") super(SPR4, self).stand() #EXPLORING FiniteStateMachine while (self.state == mbl_bots.EXPLORE): self.exploreFSM(self.exploreState) def exploreGetData(self): print("CURRENT STATE: EXPLORE") print("CURRENT SUBSTATE: DATA ACQUISITION") self.distance = utils.getDistance() self.lastIMU = self.currentIMU self.currentIMU = self.imu.getImuRawData() self.exploreState = mbl_bots.PROCESSDATA def exploreProcessData(self): print("CURRENT STATE: EXPLORE") print("CURRENT SUBSTATE: DATA PROCESSING") if (self.distance < 5): self.movesCode = mbl_bots.WB elif (self.distance > 15): self.movesCode = mbl_bots.WF else: if (randint(0, 1) == 1): self.movesCode = mbl_bots.TR else: self.movesCode = mbl_bots.TL self.exploreState = mbl_bots.MOVE # if(randint(0,5)>1): # self.exploreState = mbl_bots.MOVE # else: # self.exploreState = mbl_bots.GETDATA # self.state = mbl_bots.SHOWOFF def exploreMove(self): print("CURRENT STATE: EXPLORE") print("CURRENT SUBSTATE: MOVING") super(SPR4, self).move(self.movesCode) self.exploreState = mbl_bots.GETDATA def exploreReconTurn(self): print("Not implemented...") #============================================================================= # SHOW OFF STATE #============================================================================= def SHOWOFF(self): print("CURRENT STATE: SHOWOFF") #SHOWOFF METHODS super(SPR4, self).swing() super(SPR4, self).sayHello() self.state = mbl_bots.PHOTO #============================================================================= # PHOTO STATE #============================================================================= def PHOTO(self): print("CURRENT STATE: PHOTO") super(SPR4, self).cameraPose() self.camera.takePic() super(SPR4, self).stand() self.state = mbl_bots.EXPLORE def close(self, signal, frame): print("\nTurning off SPR4 Activity...\n") GPIO.cleanup() sys.exit(0)
class Listener(QWidget): def __init__(self, core_file_name): super(Listener, self).__init__() self.core_file_name = core_file_name self.setup_ui() self.setup_timers() port = SERIAL_PORT self.serial = serial.Serial(port, BAUDE_RATE, timeout=0) self.incoming_data = [] self.imu = IMU(PLATFORM_SPECIFIC_QUOTIENTS['stm']) self.stroke = Stroke() self.selector = Selector(self.core_file_name) self.acceleration_filter = AperiodicFilter(ACCELERATION_TIME_CONST) self.stroke.widget = self.display self.stroke.on_done = self.get_stroke self.previous_time = None self.data_buffer = '' self.init_selector() def setup_ui(self): self.resize(500, 500) self.out = QLabel(self) self.out.setMinimumHeight(100) font = QFont() font.setPixelSize(80) self.out.setFont(font) self.grid = QGridLayout(self) self.display = StrokeWidget() self.letter_selector = QComboBox() self.grid.addWidget(self.display, 0, 0, 1, 1) self.grid.addWidget(self.letter_selector, 1, 0, 1, 1) self.grid.addWidget(self.out, 2, 0, 1, 1) def setup_timers(self): self.serial_timer = QTimer() self.serial_timer.setInterval(SERIAL_INTERVAL) self.serial_timer.timeout.connect(self.get_data) self.serial_timer.start() self.process_timer = QTimer() self.process_timer.setInterval(PROCESS_INTERVAL) self.process_timer.timeout.connect(self.process) self.process_timer.start() self.display_timer = QTimer() self.display_timer.setInterval(DISPLAY_TIMEOUT) self.display_timer.setSingleShot(True) self.display_timer.timeout.connect(self.set_background) def init_selector(self): sel_lines = self.selector.letters_dict.keys() sel_lines.insert(0, 'new strokes') sel_lines.insert(0, 'free run') self.letter_selector.addItems(sel_lines) self.letter_selector.currentIndexChanged.connect(self.set_background) def set_background(self): letter = str(self.letter_selector.currentText()) self.display.set_background(self.core_file_name, letter) def store_stroke(self, key, stroke, existing=True): file_name = '{key}{time}.txt'.format(key=key, time=int(time())) file_path = os.path.join(LEARNED_FOLDER, file_name) np.savetxt(file_path, stroke) if existing: self.display.set_background(self.core_file_name, key, color='g') self.display_timer.start() def get_stroke(self, data): stroke = data['stroke'] dimention = data['dimention'] if dimention < MIN_DIMENTION: print 'too small' return letter = str(self.letter_selector.currentText()) if letter == 'new strokes': self.store_stroke('_', stroke, existing=False) print 'recorded' try: letters = self.selector.check_stroke(stroke) except: #TODO: check unify_stroke return if letters: self.out.setText(self.out.text()+letters[0]) if letter == 'free run' and letters: self.store_stroke(letters[0], stroke) elif letter in letters: self.store_stroke(letter, stroke) def process(self): local_data_storage = deepcopy(self.incoming_data) self.incoming_data = [] for data in local_data_storage: if self.previous_time is None: self.previous_time = data[0] continue data[0], self.previous_time = data[0] - self.previous_time, data[0] if data[0] < MAX_DATA_TIMELAPSE: self.imu.calc(data) gyro = np.linalg.norm(np.array([data[7:]])) accel = self.imu.get_global_acceleration() accel = self.acceleration_filter.set_input(accel, data[0]) accel_magnitude = np.linalg.norm(accel) if accel_magnitude > ACCELERATION_RESET: self.execute_spell() Yr = self.imu.get_y_direction() self.stroke.set_data(Yr, gyro) self.stroke.process_size(data[0], accel) self.setVisible(not self.imu.in_calibration) def execute_spell(self): self.out.setText('') def get_data(self): try: self.data_buffer += self.serial.read(self.serial.inWaiting()) if self.data_buffer == '': return data_pieces = self.data_buffer.split(BUFFER_DELIMITER) # Put incomplete piece back to the buffer self.data_buffer = data_pieces.pop(-1) # If there are no complete data pieces - return from function if not data_pieces: return # Else - get the last of the pieces and discard the rest line = data_pieces[-1] result = [float(d) for d in line.split()] if len(result) != 9: raise ValueError('Nine pieces of data should be provided.') new_line = [time()] + result self.incoming_data.append(new_line) except KeyboardInterrupt: raise except Exception as e: # Something went wrong... nobody cares. print e
def attitude_control(self, heading_command, roll_command, pitch_command, yaw_command, throttle_command, X_offset, Y_offset, Z_offset, Head_offset, pressure): #Read the accelerometer, gyroscope and magnetometer values ACCx = IMU.readACCx() ACCy = IMU.readACCy() ACCz = IMU.readACCz() GYRx = IMU.readGYRx() GYRy = IMU.readGYRy() GYRz = IMU.readGYRz() MAGx = IMU.readMAGx() MAGy = IMU.readMAGy() MAGz = IMU.readMAGz() # Calculate loop Period(dt). How long between Gyro Reads time_b = datetime.datetime.now() - self.time_a self.time_a = datetime.datetime.now() dt = time_b.microseconds / (1000000 * 1.0) #print("Loop Time | ", dt, " |") # Read altimeter value pressure = bmp280driver.readALT() ############################################### #### Apply low pass filter #### ############################################### MAGx = MAGx * MAG_LPF_FACTOR + self.oldXMagRawValue * (1 - MAG_LPF_FACTOR) MAGy = MAGy * MAG_LPF_FACTOR + self.oldYMagRawValue * (1 - MAG_LPF_FACTOR) MAGz = MAGz * MAG_LPF_FACTOR + self.oldZMagRawValue * (1 - MAG_LPF_FACTOR) ACCx = ACCx * ACC_LPF_FACTOR + self.oldXAccRawValue * (1 - ACC_LPF_FACTOR) ACCy = ACCy * ACC_LPF_FACTOR + self.oldYAccRawValue * (1 - ACC_LPF_FACTOR) ACCz = ACCz * ACC_LPF_FACTOR + self.oldZAccRawValue * (1 - ACC_LPF_FACTOR) self.oldXMagRawValue = MAGx self.oldYMagRawValue = MAGy self.oldZMagRawValue = MAGz self.oldXAccRawValue = ACCx self.oldYAccRawValue = ACCy self.oldZAccRawValue = ACCz ######################################### #### Median filter for accelerometer #### ######################################### # cycle the table for x in range(ACC_MEDIANTABLESIZE - 1, 0, -1): self.acc_medianTable1X[x] = self.acc_medianTable1X[x - 1] self.acc_medianTable1Y[x] = self.acc_medianTable1Y[x - 1] self.acc_medianTable1Z[x] = self.acc_medianTable1Z[x - 1] # Insert the lates values self.acc_medianTable1X[0] = ACCx self.acc_medianTable1Y[0] = ACCy self.acc_medianTable1Z[0] = ACCz # Copy the tables self.acc_medianTable2X = self.acc_medianTable1X[:] self.acc_medianTable2Y = self.acc_medianTable1Y[:] self.acc_medianTable2Z = self.acc_medianTable1Z[:] # Sort table 2 self.acc_medianTable2X.sort() self.acc_medianTable2Y.sort() self.acc_medianTable2Z.sort() # The middle value is the value we are interested in ACCx = self.acc_medianTable2X[int(ACC_MEDIANTABLESIZE / 2)] ACCy = self.acc_medianTable2Y[int(ACC_MEDIANTABLESIZE / 2)] ACCz = self.acc_medianTable2Z[int(ACC_MEDIANTABLESIZE / 2)] ######################################### #### Median filter for magnetometer #### ######################################### # cycle the table for x in range(MAG_MEDIANTABLESIZE - 1, 0, -1): self.mag_medianTable1X[x] = self.mag_medianTable1X[x - 1] self.mag_medianTable1Y[x] = self.mag_medianTable1Y[x - 1] self.mag_medianTable1Z[x] = self.mag_medianTable1Z[x - 1] # Insert the latest values self.mag_medianTable1X[0] = MAGx self.mag_medianTable1Y[0] = MAGy self.mag_medianTable1Z[0] = MAGz # Copy the tables self.mag_medianTable2X = self.mag_medianTable1X[:] self.mag_medianTable2Y = self.mag_medianTable1Y[:] self.mag_medianTable2Z = self.mag_medianTable1Z[:] # Sort table 2 self.mag_medianTable2X.sort() self.mag_medianTable2Y.sort() self.mag_medianTable2Z.sort() # The middle value is the value we are interested in MAGx = self.mag_medianTable2X[int(MAG_MEDIANTABLESIZE / 2)] MAGy = self.mag_medianTable2Y[int(MAG_MEDIANTABLESIZE / 2)] MAGz = self.mag_medianTable2Z[int(MAG_MEDIANTABLESIZE / 2)] #Convert Gyro raw to degrees per second rate_gyr_x = GYRx * G_GAIN rate_gyr_y = GYRy * G_GAIN rate_gyr_z = GYRz * G_GAIN #Calculate the angles from the gyro. self.gyroXangle += rate_gyr_x * dt self.gyroYangle += rate_gyr_y * dt self.gyroZangle += rate_gyr_z * dt #print("self.gyroXangle: ", self.gyroXangle) #print("self.gyroYangle: ", self.gyroYangle) #print("self.gyroZangle: ", self.gyroZangle) #Convert Accelerometer values to degrees AccXangle = (math.atan2(ACCy, ACCz) + M_PI) * RAD_TO_DEG AccYangle = (math.atan2(ACCz, ACCx) + M_PI) * RAD_TO_DEG AccZangle = (math.atan2(ACCx, ACCy) + M_PI) * RAD_TO_DEG # Change the rotation value of the accelerometer to -/+ 180 and move the Y axis '0' point to up # Two different pieces of code are used depending on how your IMU is mounted if IMU_upside_down: # If IMU is upside down E.g Skull logo is facing up. if AccXangle > 180: AccXangle -= 360.0 AccYangle -= 90 if AccYangle > 180: AccYangle -= 360.0 else: # If IMU is up the correct way E.g Skull logo is facing down. AccXangle -= 180.0 if AccYangle > 90: AccYangle -= 270.0 else: AccYangle += 90.0 #convert the values to -180 and +180 #AccXangle -= 180.0 #AccZangle -= 180.0 #if AccYangle > 90: # AccYangle -= 270.0 #else: # AccYangle += 90.0 #Complementary filter used to combine the accelerometer and gyro values. self.CFangleX = AA * (self.CFangleX + rate_gyr_x * dt) + (1 - AA) * AccXangle self.CFangleY = AA * (self.CFangleY + rate_gyr_y * dt) + (1 - AA) * AccYangle self.CFangleZ = AA * (self.CFangleZ + rate_gyr_z * dt) + (1 - AA) * AccZangle #Kalman filter used to combine the accelerometer and gyro values. self.kalmanY = self.kalmanFilterY(AccYangle, rate_gyr_y, dt) self.kalmanX = self.kalmanFilterX(AccXangle, rate_gyr_x, dt) #self.kalmanY = self.kalmanFilterY(AccYangle, 0,dt) #self.kalmanX = self.kalmanFilterX(AccXangle, 0,dt) #self.CFangleX_reading.append(self.CFangleX) #del self.CFangleX_reading[0] #self.CFangleX_filtered = sum(self.CFangleX_reading)/max(len(self.CFangleX_reading),1) #self.CFangleY_reading.append(self.CFangleY) #del self.CFangleY_reading[0] #self.CFangleY_filtered = sum(self.CFangleY_reading)/max(len(self.CFangleY_reading),1) self.CFangleZ_reading.append(self.CFangleZ) del self.CFangleZ_reading[0] self.CFangleZ_filtered = sum(self.CFangleZ_reading) / max( len(self.CFangleZ_reading), 1) if IMU_upside_down: MAGy = -MAGy #Calculate heading heading = 180 * math.atan2(MAGy, MAGx) / M_PI #Only have our heading between 0 and 360 if heading < 0: heading += 360 #Normalize accelerometer raw values. accXnorm = ACCx / math.sqrt(ACCx * ACCx + ACCy * ACCy + ACCz * ACCz) accYnorm = ACCy / math.sqrt(ACCx * ACCx + ACCy * ACCy + ACCz * ACCz) accZnorm = ACCz / math.sqrt(ACCx * ACCx + ACCy * ACCy + ACCz * ACCz) #Calculate pitch, roll and yaw pitch = math.asin(accXnorm) if pitch > 89: pitch = 89 if pitch < -89: pitch = -89 roll_intermediate = accYnorm / math.cos(pitch) if abs(roll_intermediate) > 1.0: roll_intermediate = 1.0 roll = -math.asin(roll_intermediate) yaw = math.asin(accZnorm) #Calculate the new tilt compensated values magXcomp = MAGx * math.cos(pitch) + MAGz * math.sin(pitch) magYcomp = MAGx * math.sin(roll) * math.sin(pitch) + MAGy * math.cos( roll) - MAGz * math.sin(roll) * math.cos(pitch) #Calculate tilt compensated heading tiltCompensatedHeading = 180 * math.atan2(magYcomp, magXcomp) / M_PI #Only have our heading between 0 and 360 if tiltCompensatedHeading < 0: tiltCompensatedHeading += 360 #self.tiltHeading_reading.append(tiltCompensatedHeading) #del self.tiltHeading_reading[0] #tiltHeading_filtered = sum(self.tiltHeading_reading)/max(len(self.tiltHeading_reading),1) #Calculate altitude #altitude = 10^(log(10)*(pressure if 0: #Change to '0' to stop showing pitch, roll, yaw, heading and tilt compensated heading print( "\033[1;34;40m Pitch %5.2f Roll %5.2f Yaw %5.2f HEADING %5.2f TCH %5.2f \033[0m \n " % (pitch * 180 / M_PI, roll * 180 / M_PI, yaw * 180 / M_PI, heading, tiltCompensatedHeading)), if 0: #Change to '0' to stop showing the raw values from the Compass print( "\033[1;34;40m MAGx %5.2f MAGy %5.2f MAGz %5.2f HEADING %5.2f TCH %5.2f \033[0m \n " % (MAGx, MAGy, MAGz, heading, tiltCompensatedHeading)), if 0: #Change to '0' to stop showing the angles from the accelerometer print("\033[1;34;40mACCX Angle %5.2f ACCY Angle %5.2f \033[0m " % (AccXangle, AccYangle)), if 0: #Change to '0' to stop showing the angles from the gyro print( "\033[1;31;40m\tGRYX Angle %5.2f GYRY Angle %5.2f GYRZ Angle %5.2f" % (self.gyroXangle, self.gyroYangle, self.gyroZangle)), if 0: #Change to '0' to stop showing the angles from the complementary filter print( "\033[1;35;40m \tCFangleX Angle %5.2f \033[1;36;40m CFangleY Angle %5.2f \33[1;32;40m" % (self.CFangleX_filtered, self.CFangleY_filtered)), if 0: #Change to '0' to stop showing the heading print("HEADING %5.2f \33[1;37;40m tiltCompensatedHeading %5.2f" % (heading, tiltCompensatedHeading)) #heading_error = heading_command - tiltHeading_filtered heading_error = heading_command - tiltCompensatedHeading rudder_pos = int(rudder_mid_pos - heading_p_gain * heading_error) if (rudder_pos < rudder_servo_min): rudder_pos = rudder_servo_min if (rudder_pos > rudder_servo_max): rudder_pos = rudder_servo_max #yaw_error = yaw_command + self.CFangleZ_filtered + Z_offset yaw_error = yaw_command + self.CFangleZ_filtered + Z_offset rudder_pos = int(rudder_mid_pos - yaw_p_gain * yaw_error) if (rudder_pos < rudder_servo_min): rudder_pos = rudder_servo_min if (rudder_pos > rudder_servo_max): rudder_pos = rudder_servo_max #roll_error = roll_command - self.CFangleX_filtered + X_offset roll_error = roll_command - self.kalmanX + X_offset aileron_pos = int(aileron_mid_pos - roll_p_gain * roll_error) if (aileron_pos < aileron_servo_min): aileron_pos = aileron_servo_min if (aileron_pos > aileron_servo_max): aileron_pos = aileron_servo_max #pitch_error = pitch_command - self.CFangleY_filtered + Y_offset pitch_error = pitch_command - self.kalmanY + Y_offset elevator_pos = int(elevator_mid_pos + pitch_p_gain * pitch_error) if (elevator_pos < elevator_servo_min): elevator_pos = elevator_servo_min if (elevator_pos > elevator_servo_max): elevator_pos = elevator_servo_max #rudder_pos = int(rudder_mid_pos) #throttle_pos = int((throttle_servo_max-throttle_servo_min)*(float(throttle_command) / 100.0) + throttle_servo_min) #self.pwm.set_pwm(0, 0, throttle_pos) throttleServo.set_servo_position(throttle_command)
def __init__(self): imu = IMU()
ser = serial.Serial("COM11", 9600, timeout=TOUT) print "Connected" print "Sent Connection Call" print "Loading Calibration" xCal = list() yCal = list() zCal = list() with open('..\Data\CalibrationSignal.csv', 'rb') as csvDataFile: csvReader = csv.reader(csvDataFile) for row in csvReader: xCal.append(float(row[0])) yCal.append(float(row[1])) zCal.append(float(row[2])) imu = IMU() imu.calcOffsets(xCal, yCal, zCal) imu.getOffsets() ser.write("t") recordedData = [] start = time.time() while (end - start) <= 30.0: dataIn = [] val = ser.read() while val != '_': dataIn.append(val) val = ser.read() if dataIn[0] == '\x00':
import sys from math import pi from IMU import IMU from sensor_msgs.msg import Imu as ImuMessage import tf if __name__ == '__main__': rospy.init_node('ImuPublisher', log_level = rospy.INFO) rospy.sleep(2) rospy.loginfo("Initializing ImuPublisher node") imu = IMU('/dev/ttyUSB0') # # see the rename in the import above # imuPublisher = rospy.Publisher('imu_data', ImuMessage) imuMessage = ImuMessage() imuMessage.header.frame_id = 'imu' imuMessage.orientation.x = 0.0 imuMessage.orientation.y = 0.0 imuMessage.orientation.z = 0.0 imuMessage.orientation.w = 0.0 imuMessage.orientation_covariance = [0.0001, 0.0, 0.0, 0.0, 0.0001, 0.0, 0.0, 0.0, 100.0]
def __init__(self): super(App, self).__init__() # load background self.background = Background(filename="background.png") # get array copy of background image self.background.arr = pygame.surfarray.array3d(self.background.img) # create bw from image _, self.background.arr_bw = cv2.threshold(self.background.arr[:, :, 0], 128, 1, cv2.THRESH_BINARY) # print self.background.arr_bw.shape, self.background.arr_bw.dtype # self.background.arr_dist = cv2.distanceTransform(self.background.arr_bw, cv.CV_DIST_L1, 3) # get nearest (zero) pixel labels with corresponding distances self.background.arr_dist, self.labels = cv2.distanceTransformWithLabels( self.background.arr_bw, cv.CV_DIST_L1, 3, labelType=cv2.DIST_LABEL_PIXEL) self.distances = self.background.arr_dist ### get x,y coordinates for each label # get positions of zero points zero_points = Utils.zero_points(self.background.arr_bw) # get labels for zero points zero_labels = self.labels[zero_points[:, 0], zero_points[:, 1]] # create dictionary mapping labels to zero point positions self.label_positions = dict( zip(zero_labels, zip(zero_points[:, 0], zero_points[:, 1]))) # create hilbert curve lookup table self.hilbert = Hilbert.hilbert_lookup(*self.background.arr.shape[:2]) # provide a rgb variant of dist for display self.background.arr_dist_rgb = self.background.arr.copy() self.background.arr_dist_rgb[:, :, 0] = self.background.arr_dist self.background.arr_dist_rgb[:, :, 1] = self.background.arr_dist self.background.arr_dist_rgb[:, :, 2] = self.background.arr_dist # print a.shape self.setup_pygame() self.events = Events() self.lane = Lane(self.events) self.lane.load("parkour.sv") # self.lane.add_support_point(100,100) # self.lane.add_support_point(200,100) # self.lane.add_support_point(200,200) # self.lane.add_support_point(100,200) self.optimize = Optimize(self.lane) self.cars = [] # for k in range(1): # self.cars.append(Car(x=150+k*5,y=100,theta=np.random.randint(0,360),speed=np.random.randint(45,180))) self.cars.append(Car(x=50, y=250, theta=90, speed=1 * 1.5 * 90)) self.cars.append(Car(x=50, y=250, theta=90, speed=1 * 90)) # [1] human self.cars.append(Car(x=50, y=250, theta=90, speed=1 * 90)) # [2] ghost of ins estimating [0] self.action = None self.human = HumanController() self.heuristic = Heuristic(self.lane) Node.heuristic = self.heuristic self.onestep = OneStepLookaheadController(self.cars, self.lane, self.heuristic) self.nstep = NStepLookaheadController(self.cars, self.lane, self.heuristic, 2) self.bestfirst = BestFirstController(self.cars, self.lane, self.heuristic) self.controller = self.bestfirst self.cars[0].camview = CamView(self.cars[0], self.background.arr) self.cars[0].camview.register_events(self.events) self.cars[0].controller = self.controller self.cars[0].collision = False self.cars[0].imu = IMU(self.cars[0]) self.cars[0].ins = INS(self.cars[0].imu.calibration_noise) self.cars[0].ins.update_pose(self.cars[0].x, self.cars[0].y, self.cars[0].theta / Utils.d2r, gain=1) self.insghost = INSGhostController(self.cars[0].ins) self.cars[1].controller = self.human self.cars[2].controller = self.insghost self.cars[2].collision = False self.cars[2].size *= 1.25 self.cars[2].camview = CamView(self.cars[2], self.background.arr_dist_rgb, width=275, height=275, offset=(0, 75), angle_offset=-25) self.cars[2].camview.register_events(self.events) self.cars[0].name = "actual" self.cars[1].name = "human" self.cars[2].name = "estimate" # this causes the controller of cars[0] to use the information from cars[0].ghost but act on cars[0] # self.cars[0].ghost = self.cars[2] # self.window = Window(self.screen, self.events, 300, 200, "caption") self.grid = Grid(50, 50, *self.size) self.last_distance_grid_update = time() - 10 self.update_distance_grid() self.done = False for car in self.cars: # save original speed if not hasattr(car, "speed_on"): car.speed_on = car.speed # toggle speed car.speed = car.speed_on - car.speed # car.pause = not car.pause self.plot_window_size = 100 self.xyt_corr_ring_buffer = RingBuffer(self.plot_window_size, channels=3) self.xyt_corr_plot = RingBufferPlot(self.xyt_corr_ring_buffer) # self.normal_test_p_value_plot = RingBufferPlot(RingBuffer(self.plot_window_size,channels=self.xyt_corr_ring_buffer.channels)) self.std_plot = RingBufferPlot( RingBuffer(self.plot_window_size, channels=self.xyt_corr_ring_buffer.channels)) self.velocity_carthesian_history_plot = RingBufferPlot( self.cars[0].ins.velocity_carthesian_history) # self.hist_plot = HistogramPlot(10) self.register_events() self.spin()
def showGravity(self): """ view - displays graviry ui """ if (not IMU.isAvailable()): return self.unavailable() self.open() self.serviceManager() self.gridrow += 1 self.widgets['frameLabel'] = Tkinter.Label( self.widgets['tframe'], text='IMU / Gravity', anchor=NW, bg=self.colours['bg'], fg=self.colours['headingfg'], font=self.fonts['heading']) self.widgets['frameLabel'].grid(column=0, row=self.gridrow, sticky='W') self.gridrow += 1 self.widgets['gframe'] = Tkinter.Frame(self.widgets['tframe'], bg=self.colours['bg']) self.widgets['gframe'].grid(column=0, row=self.gridrow, columnspan=3, sticky='EW') self.widgets['gframe'].columnconfigure(0, weight=1) self.widgets['tframe'].columnconfigure(0, weight=1) self.widgets['rollLabel'] = Tkinter.Label(self.widgets['gframe'], text='TBD', bg=self.colours['bg'], fg=self.colours['fg'], font=self.fonts['heading2']) self.widgets['rollLabel'].grid(column=0, row=self.gridrow, sticky='EW') self.gridrow += 1 self.widgets['hCanvas'] = Tkinter.Canvas(self.widgets['gframe'], borderwidth=0, bg=self.colours['bg'], highlightthickness=0, width=474, height=474) self.widgets['hCanvas'].grid(column=0, row=self.gridrow, padx=10, sticky='EW') self.shapes['skyplane'] = self.widgets['hCanvas'].create_polygon( [(0, 0), (474, 0), (474, 474), (0, 474)], fill='#fff') self.shapes['groundplane'] = self.widgets['hCanvas'].create_polygon( self.groundcoords, fill='#828282') self.shapes['mask'] = self.widgets['hCanvas'].create_image( self.centre.real, self.centre.imag, image=self.getImage('mask', 0)) self.shapes['reticle'] = self.widgets['hCanvas'].create_image( self.centre.real, self.centre.imag, image=self.getImage('reticle', 0)) self.widgets['pitchLabel'] = Tkinter.Label(self.widgets['gframe'], text='TBD', anchor=W, bg=self.colours['bg'], fg=self.colours['fg'], font=self.fonts['heading2']) self.widgets['pitchLabel'].grid(column=1, row=self.gridrow, sticky='NS')
for row in csvReader: axOff.append(float(row[0])) ayOff.append(float(row[1])) azOff.append(float(row[2])) with open('IMUData60cm_x.csv', 'rb') as csvDataFile: csvReader = csv.reader(csvDataFile) for row in csvReader: data1.append(float(row[0])) data2.append(float(row[1])) data3.append(float(row[2])) originTap.append(int(row[6])) ##x_vel = integrate.cumtrapz(t, Ax, initial=0) ##x_Trap = integrate.cumtrapz(t,x_vel, initial=0) mpu = IMU() mpu.calcOffsets(axOff, ayOff, azOff) mpu.setTime(30.0) mpu.setAcceleration(data1, data2, data3) Ax, Ay, Az = mpu.getAcceleration() totalSum = 0 dt = len(Ax) / 30.0 t = mpu.time x_vel = [sum(Ax[:i]) * -dt for i in range(len(Ax))] x_pos = [sum(x_vel[:i]) * dt for i in range(len(x_vel))] y_vel = [sum(Ay[:i]) * dt for i in range(len(Ay))] y_pos = [sum(y_vel[:i]) * dt for i in range(len(y_vel))]
heading_d_gain = 0.0 yaw_d_gain = 0.0 roll_d_gain = 0.0 pitch_d_gain = 0.0 velocity_d_gain = 0.0 rudder_mid_pos = (rudder_servo_max + rudder_servo_min) / 2 aileron_mid_pos = (aileron_servo_max + aileron_servo_min) / 2 elevator_mid_pos = (elevator_servo_max + elevator_servo_min) / 2 rudder_command = 0 aileron_command = 0 elevator_command = 0 throttle_command = 0 IMU.detectIMU() #Detect if BerryIMUv1 or BerryIMUv2 is connected. IMU.initIMU() #Initialise the accelerometer, gyroscope and compass # Helper function to make setting a servo pulse width simpler. def set_servo_pulse(channel, pulse): pulse_length = 1000000 # 1,000,000 us per second pulse_length //= 60 # 60 Hz print('{0}us per period'.format(pulse_length)) pulse_length //= 4096 # 12 bits of resolution print('{0}us per bit'.format(pulse_length)) pulse *= 1000 pulse //= pulse_length pwm.set_pwm(channel, 0, pulse)
#!/usr/bin/env pybricks-micropython from pybricks import ev3brick as brick from pybricks.parameters import (Port, Button) from pybricks.tools import print, wait #Fuege den Tools Ordner zum PYTHONPATH hinzu. Nicht notwendig wenn IMU.py im selben Ordner ist import sys sys.path.append("/home/robot/LEGORoboticsPython/Tools") #Importiere IMU.py from IMU import IMU #Port und Mode des Seonsors festlegen. Moegliche Modes sind TILT, ACCEL, COMPASS, MAG, GYRO #TODO: implement ALL mode #Info zu Modes:http://docs.ev3dev.org/projects/lego-linux-drivers/en/ev3dev-stretch/sensor_data.html imu = IMU(Port.S3, 'GYRO') while not Button.DOWN in brick.buttons(): result = 'IMU: ' + str(imu.angle()) brick.display.clear() brick.display.text(result, (0, 50)) print(result) wait(0.1)
if yf > r180: yf = yf - r360 elif yf < -r180: yf = yf + r360 self.C_FL = self.euler2C(pf, rf, yf) self.att = np.array([pf, rf, yf]) # print("=======================>C_FLTR: {0}".format(np.degrees(self.att))) self.prevTime = self.elapsedTime return (self.att) if (__name__ == "__main__"): print("Test AHRS Class") myIMU = IMU() C_orient = np.zeros((3, 3)) C_orient[0][2] = -1.0 C_orient[1][1] = -1.0 C_orient[2][0] = -1.0 myIMU.setOrientation(C_orient) myAHRS = AHRS(myIMU) # imu.setOrientation(np.eye(3)) # C_orient = np.zeros( (3,3) ) # C_orient[0][2] = 1.0 # C_orient[1][1] = 1.0 # C_orient[2][0] = 1.0 # imu.setOrientation(C_orient) nSamples = 10
def showOrthographic(self): if not IMU.isAvailable(): return self.unavailable() self.open() self.serviceManager() self.gridrow += 1 self.widgets["frameLabel"] = Tkinter.Label( self.widgets["tframe"], text="IMU / Orthographic", anchor=NW, bg=self.colours["bg"], fg=self.colours["headingfg"], font=self.fonts["heading"], ) self.widgets["frameLabel"].grid(column=0, row=self.gridrow, sticky="W") self.gridrow += 1 self.widgets["oframe"] = Tkinter.Frame(self.widgets["tframe"], bg=self.colours["bg"]) self.widgets["oframe"].grid(column=0, row=self.gridrow, columnspan=3, sticky="EW") self.widgets["rollLabel"] = Tkinter.Label( self.widgets["oframe"], text="Roll", bg=self.colours["bg"], fg=self.colours["headingfg"], font=self.fonts["heading2"], ) self.widgets["rollLabel"].grid(column=0, row=self.gridrow, sticky="EW") self.widgets["pitchLabel"] = Tkinter.Label( self.widgets["oframe"], text="Pitch", bg=self.colours["bg"], fg=self.colours["headingfg"], font=self.fonts["heading2"], ) self.widgets["pitchLabel"].grid(column=1, row=self.gridrow, sticky="EW") self.widgets["yawLabel"] = Tkinter.Label( self.widgets["oframe"], text="Yaw", bg=self.colours["bg"], fg=self.colours["headingfg"], font=self.fonts["heading2"], ) self.widgets["yawLabel"].grid(column=2, row=self.gridrow, sticky="EW") self.gridrow += 1 self.widgets["rollCanvas"] = Tkinter.Canvas( self.widgets["oframe"], borderwidth=0, bg=self.colours["bg"], highlightthickness=0, width=280, height=280 ) self.widgets["rollCanvas"].grid(column=0, row=self.gridrow, padx=10, sticky="NE") self.shapes["rollImage"] = self.widgets["rollCanvas"].create_image(150, 150, image=self.getImage("roll", 0)) self.widgets["pitchCanvas"] = Tkinter.Canvas( self.widgets["oframe"], borderwidth=0, bg=self.colours["bg"], highlightthickness=0, width=280, height=280 ) self.widgets["pitchCanvas"].grid(column=1, row=self.gridrow, padx=10, sticky="NE") self.shapes["pitchImage"] = self.widgets["pitchCanvas"].create_image(150, 150, image=self.getImage("pitch", 0)) self.widgets["yawCanvas"] = Tkinter.Canvas( self.widgets["oframe"], borderwidth=0, bg=self.colours["bg"], highlightthickness=0, width=280, height=280 ) self.widgets["yawCanvas"].grid(column=2, row=self.gridrow, padx=10, sticky="NE") self.shapes["yawImage"] = self.widgets["yawCanvas"].create_image(150, 150, image=self.getImage("yaw", 0))
def calibrate(): RAD_TO_DEG = 57.29578 M_PI = 3.14159265358979323846 G_GAIN = 0.070 # [deg/s/LSB] If you change the dps for gyro, you need to update this value accordingly AA = 0.40 # Complementary filter constant gyroXangle = 0.0 gyroYangle = 0.0 gyroZangle = 0.0 CFangleX = 0.0 CFangleY = 0.0 CFangleZ = 0.0 pressure = 0.0 altitude = 0.0 IMU.detectIMU() #Detect if BerryIMUv1 or BerryIMUv2 is connected. IMU.initIMU() #Initialise the accelerometer, gyroscope and compass a = datetime.datetime.now() cal_length = 300 X_cal = [0.0] * cal_length Y_cal = [0.0] * cal_length Z_cal = [0.0] * cal_length Head_cal = [0.0] * cal_length #cal_ready = raw_input("Is the plane level and ready for IMU calibration (Y/N)? ") #type(cal_ready) cal_ready = 'Y' if (cal_ready == 'Y'): print('Entering Calibration Routine') for i in range(0, cal_length): ACCx = IMU.readACCx() ACCy = IMU.readACCy() ACCz = IMU.readACCz() GYRx = IMU.readGYRx() GYRy = IMU.readGYRy() GYRz = IMU.readGYRz() MAGx = IMU.readMAGx() MAGy = IMU.readMAGy() MAGz = IMU.readMAGz() #pressure = bmp280driver.readALT() ##Calculate loop Period(LP). How long between Gyro Reads b = datetime.datetime.now() - a a = datetime.datetime.now() LP = b.microseconds / (1000000 * 1.0) #print "Loop Time | %5.2f|" % ( LP ), #Convert Gyro raw to degrees per second rate_gyr_x = GYRx * G_GAIN rate_gyr_y = GYRy * G_GAIN rate_gyr_z = GYRz * G_GAIN #Calculate the angles from the gyro. gyroXangle += rate_gyr_x * LP gyroYangle += rate_gyr_y * LP gyroZangle += rate_gyr_z * LP #Convert Accelerometer values to degrees AccXangle = (math.atan2(ACCy, ACCz) + M_PI) * RAD_TO_DEG AccYangle = (math.atan2(ACCz, ACCx) + M_PI) * RAD_TO_DEG AccZangle = (math.atan2(ACCx, ACCy) + M_PI) * RAD_TO_DEG #convert the values to -180 and +180 AccXangle -= 180.0 AccZangle -= 180.0 if AccYangle > 90: AccYangle -= 270.0 else: AccYangle += 90.0 #Complementary filter used to combine the accelerometer and gyro values. CFangleX = AA * (CFangleX + rate_gyr_x * LP) + (1 - AA) * AccXangle CFangleY = AA * (CFangleY + rate_gyr_y * LP) + (1 - AA) * AccYangle CFangleZ = AA * (CFangleZ + rate_gyr_z * LP) + (1 - AA) * AccZangle # gyro filter array X_cal[i] = CFangleX Y_cal[i] = CFangleY Z_cal[i] = CFangleZ #Calculate heading heading = 180 * math.atan2(MAGy, MAGx) / M_PI # magnetometer filter array Head_cal[i] = heading X_offset = sum(X_cal) / max(len(X_cal), 1) Y_offset = sum(Y_cal) / max(len(Y_cal), 1) Z_offset = sum(Z_cal) / max(len(Z_cal), 1) Head_offset = sum(Head_cal) / max(len(Head_cal), 1) heading_command = Head_offset print("Calibration Complete") print("X_offset = %5.2f" % (X_offset)) print("Y_offset = %5.2f" % (Y_offset)) print("Z_offset = %5.2f" % (Z_offset)) print("Head_offset = %5.2f" % (Head_offset)) return (X_offset, Y_offset, Z_offset, Head_offset, pressure)
import RPi.GPIO as GPIO from IMU import IMU from Motor import Motor from HCSR_04 import HCSR_04 import time try: #motor = Motor() #motor.setupMotors() #motor.calibrate() #motor.flight() imu = IMU() hcsr04 = HCSR_04() motor = Motor() motor.setupMotors() motor.calibrate() while True: xRotation, yRotation = imu.readMPU6050Values() altitute = hcsr04.measureHCSR_04() command = 'move_up' throttleOffset, forwardRightOffset,forwardLeftOffset,backRightOffset,backLeftOffset = motor.flight(xRotation,yRotation,altitute,command) except KeyboardInterrupt: print("Keyboard Interrupr") finally: GPIO.cleanup()
def __init__(self): # Initialise the PCA9685 using the default address (0x40). #self.pwm = Adafruit_PCA9685.PCA9685() # Set frequency to 60hz, good for servos. #self.pwm.set_pwm_freq(60) self.throttleServo = servo.Servo(0, throttle_servo_min, throttle_servo_max, false) self.aileronServo = servo.Servo(1, aileron_servo_min, aileron_servo_max, false) self.elevatorServo = servo.Servo(2, elevator_servo_min, elevator_servo_max, true) self.rudderServo = servo.Servo(0, rudder_servo_min, rudder_servo_max, false) IMU.detectIMU() IMU.writeACC(LSM9DS1_CTRL_REG7_XL, 0b11100001) self.gyroXangle = 0.0 self.gyroYangle = 0.0 self.gyroZangle = 0.0 self.CFangleX = 0.0 self.CFangleY = 0.0 self.CFangleZ = 0.0 # Uneeded in new? self.CFangleXFiltered = 0.0 self.CFangleYFiltered = 0.0 self.kalmanX = 0.0 self.kalmanY = 0.0 self.oldXMagRawValue = 0 self.oldYMagRawValue = 0 self.oldZMagRawValue = 0 self.oldXAccRawValue = 0 self.oldYAccRawValue = 0 self.oldZAccRawValue = 0 # Setup the tables for the median filter. Fill them all with '1' so we don't get a divide by zero error self.acc_medianTable1X = [1] * ACC_MEDIANTABLESIZE self.acc_medianTable1Y = [1] * ACC_MEDIANTABLESIZE self.acc_medianTable1Z = [1] * ACC_MEDIANTABLESIZE self.acc_medianTable2X = [1] * ACC_MEDIANTABLESIZE self.acc_medianTable2Y = [1] * ACC_MEDIANTABLESIZE self.acc_medianTable2Z = [1] * ACC_MEDIANTABLESIZE self.mag_medianTable1X = [1] * MAG_MEDIANTABLESIZE self.mag_medianTable1Y = [1] * MAG_MEDIANTABLESIZE self.mag_medianTable1Z = [1] * MAG_MEDIANTABLESIZE self.mag_medianTable2X = [1] * MAG_MEDIANTABLESIZE self.mag_medianTable2Y = [1] * MAG_MEDIANTABLESIZE self.mag_medianTable2Z = [1] * MAG_MEDIANTABLESIZE # Kalman filter variables self.y_bias = 0.0 self.x_bias = 0.0 self.XP_00 = 0.0 self.XP_01 = 0.0 self.XP_10 = 0.0 self.XP_11 = 0.0 self.YP_00 = 0.0 self.YP_01 = 0.0 self.YP_10 = 0.0 self.YP_11 = 0.0 self.KFangleX = 0.0 self.KFangleY = 0.0 self.pressure = 0.0 self.altitude = 0.0 # Moving average filter settings for Roll, Pitch, Yaw readings self.combined_filter_length = 1 self.CFangleX_filter_length = self.combined_filter_length self.CFangleY_filter_length = self.combined_filter_length self.CFangleZ_filter_length = self.combined_filter_length self.tiltHeading_filter_length = self.combined_filter_length self.CFangleX_filtered = 0.0 self.CFangleX_reading = [0.0] * self.CFangleX_filter_length self.CFangleY_filtered = 0.0 self.CFangleY_reading = [0.0] * self.CFangleY_filter_length self.CFangleZ_filtered = 0.0 self.CFangleZ_reading = [0.0] * self.CFangleZ_filter_length self.tiltHeading_filtered = 0.0 self.tiltHeading_reading = [0.0] * self.tiltHeading_filter_length self.time_a = datetime.datetime.now()
import time import os from ToFs_UDP import ToFs_UDP from Robot_Slave import Robot_Slave from IMU import IMU if __name__ == "__main__": print("This process has the PID", os.getpid()) imu = IMU() imu.start() tofs_udp = ToFs_UDP() tofs_udp.start() time.sleep(0.1) robot_Slave = Robot_Slave() while True: #try: tof_right_2, tof_right_1, tof_front, tof_left_1, tof_left_2 = tofs_udp.getMeasurements( ) print(" ToF Readings : %d %d %d %d %d " % (tof_right_2, tof_right_1, tof_front, tof_left_1, tof_left_2)) roll, pitch, yaw = imu.getEuler() print(" IMU Readings : %f %f %f " % (roll, pitch, yaw)) #check_IMU_status_flag=robot_Slave.read_check_IMU_status_flag()