Пример #1
0
    def __init__(self):
        print("Motor thread is created")

        self.uavServerTX = UAVServerTX()
        self.uavServerRX = UAVServerRX()

        self.imu = IMU()
Пример #2
0
    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()
Пример #3
0
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
Пример #4
0
    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
Пример #5
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)
Пример #6
0
	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)
Пример #7
0
	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')
Пример #8
0
    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)
Пример #9
0
 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()
Пример #10
0
	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))
Пример #11
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)
Пример #12
0
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')
Пример #13
0
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
Пример #14
0
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!")
Пример #15
0
def begin():
    rospy.init_node('picar_imu', anonymous=True)
    e = IMU()

    while not rospy.is_shutdown():
        e.tick()
Пример #16
0
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
Пример #17
0
 def setIMU(self):
     self.imu = IMU()
Пример #18
0
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)
Пример #19
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
Пример #20
0
    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)
Пример #21
0
 def __init__(self):
     imu = IMU()
Пример #22
0
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':
Пример #23
0
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]
Пример #24
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()
Пример #25
0
    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')
Пример #26
0
    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))]
Пример #27
0
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)

Пример #28
0
#!/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)
Пример #29
0
        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
Пример #30
0
    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))
Пример #31
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)
Пример #32
0
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()
Пример #33
0
    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()
Пример #34
0
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()