├── .gitignore ├── BME280 ├── BME280.py └── BME280_sample.py ├── BMP280 ├── BMP280.py └── BMP280_sample.py ├── HC-SR04 ├── hc-sr04_sample.py └── hcsr04.py ├── HMC5883L ├── HMC5883L_calibr.txt ├── compas.png ├── hmc5883.py ├── hmc5883_calibr.py ├── hmc5883_calibrated.py ├── hmc5883_heading.py ├── hmc5883_mpu6050.py ├── kfs360.py ├── pygame_hmc5883.py ├── pygame_hmc5883_mpu6050.py └── pygame_hmc5883_mpu6050_kfs360.py ├── LICENSE ├── MPU-6050 ├── kf.py ├── mpu6050.py ├── mpu6050_accel_kf.py ├── mpu6050_accel_no_filter.py ├── mpu6050_calibr.py ├── mpu6050_final.py ├── mpu6050_get_raw.py ├── mpu6050_get_raw_no_calibr.py ├── mpu6050_gyro.py ├── pyplay_accel.py ├── pyplay_final.py └── pyplay_gyro.py ├── MPU-9250 ├── mpu9250.py ├── mpu9250_calibration.py ├── mpu9250_get.py └── mpu9250_get_raw.py ├── PCA9685 ├── pca9685.py ├── pca9685_PWM_LED.py └── pca9685_servo.py └── README.md /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | 5 | # C extensions 6 | *.so 7 | 8 | # Distribution / packaging 9 | .Python 10 | env/ 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | *.egg-info/ 23 | .installed.cfg 24 | *.egg 25 | 26 | # PyInstaller 27 | # Usually these files are written by a python script from a template 28 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 29 | *.manifest 30 | *.spec 31 | 32 | # Installer logs 33 | pip-log.txt 34 | pip-delete-this-directory.txt 35 | 36 | # Unit test / coverage reports 37 | htmlcov/ 38 | .tox/ 39 | .coverage 40 | .coverage.* 41 | .cache 42 | nosetests.xml 43 | coverage.xml 44 | *,cover 45 | 46 | # Translations 47 | *.mo 48 | *.pot 49 | 50 | # Django stuff: 51 | *.log 52 | 53 | # Sphinx documentation 54 | docs/_build/ 55 | 56 | # PyBuilder 57 | target/ 58 | -------------------------------------------------------------------------------- /BME280/BME280.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | # Copyright (c) 2015 Andrey Koryagin 3 | # Permission is hereby granted, free of charge, to any person obtaining a copy 4 | # of this software and associated documentation files (the "Software"), to deal 5 | # in the Software without restriction, including without limitation the rights 6 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | # copies of the Software, and to permit persons to whom the Software is 8 | # furnished to do so, subject to the following conditions: 9 | # The above copyright notice and this permission notice shall be included in 10 | # all copies or substantial portions of the Software. 11 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 12 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 13 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 14 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 15 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 16 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 17 | # THE SOFTWARE. 18 | 19 | import smbus 20 | import time 21 | 22 | # BME280 default address. 23 | BME280_I2CADDR = 0x76 24 | 25 | # BME280 Registers 26 | BME280_CONTROL_MEAS = 0xF4 27 | BME280_CONTROL_HUM = 0xF2 28 | 29 | BME280_CONFIG = 0xF5 30 | BME280_PRESSURE = 0xF7 31 | BME280_TEMP = 0xFA 32 | BME280_HUMIDITY = 0xFD 33 | 34 | BME280_DIG_T1 = 0x88 35 | BME280_DIG_T2 = 0x8A 36 | BME280_DIG_T3 = 0x8C 37 | BME280_DIG_P1 = 0x8E 38 | BME280_DIG_P2 = 0x90 39 | BME280_DIG_P3 = 0x92 40 | BME280_DIG_P4 = 0x94 41 | BME280_DIG_P5 = 0x96 42 | BME280_DIG_P6 = 0x98 43 | BME280_DIG_P7 = 0x9A 44 | BME280_DIG_P8 = 0x9C 45 | BME280_DIG_P9 = 0x9E 46 | BME280_DIG_H1 = 0xA1 47 | BME280_DIG_H2 = 0xE1 48 | BME280_DIG_H3 = 0xE3 49 | BME280_DIG_H4 = 0xE4 50 | BME280_DIG_H5 = 0xE5 51 | BME280_DIG_H6 = 0xE7 52 | 53 | # Oversampling Setting 54 | BME280_OVERS_T1 = 0x20 55 | BME280_OVERS_T2 = 0x40 56 | BME280_OVERS_T4 = 0x60 57 | BME280_OVERS_T8 = 0x80 58 | BME280_OVERS_T16 = 0xA0 59 | 60 | BME280_OVERS_P1 = 0x04 61 | BME280_OVERS_P2 = 0x08 62 | BME280_OVERS_P4 = 0x0C 63 | BME280_OVERS_P8 = 0x10 64 | BME280_OVERS_P16 = 0x14 65 | 66 | BME280_OVERS_H1 = 0x01 67 | BME280_OVERS_H2 = 0x02 68 | BME280_OVERS_H4 = 0x03 69 | BME280_OVERS_H8 = 0x04 70 | BME280_OVERS_H16 = 0x05 71 | 72 | # Power Modes. 73 | # This lib uses NORMAL mode only! 74 | #BME280_SLEEP_MODE = 0x00 75 | #BME280_FORCED_MODE = 0x01 76 | BME280_NORMAL_MODE = 0x03 77 | 78 | BME280_TSB_0_5 = 0x00 79 | BME280_TSB_62_5 = 0x20 80 | BME280_TSB_125 = 0x40 81 | BME280_TSB_250 = 0x60 82 | BME280_TSB_500 = 0x80 83 | BME280_TSB_1000 = 0xA0 84 | BME280_TSB_2000 = 0xC0 85 | BME280_TSB_4000 = 0xE0 86 | 87 | BME280_FILTER_OFF = 0x00 88 | BME280_FILTER_COEFFICIENT2 = 0x04 89 | BME280_FILTER_COEFFICIENT4 = 0x08 90 | BME280_FILTER_COEFFICIENT8 = 0x0C 91 | BME280_FILTER_COEFFICIENT16 = 0x10 92 | 93 | BME280_SPI_OFF = 0x00 94 | BME280_SPI_ON = 0x01 95 | 96 | BME280_CONTROL_MEAS_SET = (BME280_OVERS_T16 | BME280_OVERS_P16 | BME280_NORMAL_MODE) 97 | BME280_CONTROL_HUM_SET = BME280_OVERS_H2 98 | BME280_CONFIG_SET = (BME280_TSB_0_5 | BME280_FILTER_COEFFICIENT16 | BME280_SPI_OFF) 99 | 100 | class BME280(object): 101 | def __init__(self, port=1, address=BME280_I2CADDR): 102 | self.bus = smbus.SMBus(port) 103 | self.address = address 104 | 105 | # Read calibration values 106 | self.dig_t1 = self.read_word(BME280_DIG_T1) # Unsigned 107 | self.dig_t2 = self.read_word_sign(BME280_DIG_T2) 108 | self.dig_t3 = self.read_word_sign(BME280_DIG_T3) 109 | self.dig_p1 = self.read_word(BME280_DIG_P1) # Unsigned 110 | self.dig_p2 = self.read_word_sign(BME280_DIG_P2) 111 | self.dig_p3 = self.read_word_sign(BME280_DIG_P3) 112 | self.dig_p4 = self.read_word_sign(BME280_DIG_P4) 113 | self.dig_p5 = self.read_word_sign(BME280_DIG_P5) 114 | self.dig_p6 = self.read_word_sign(BME280_DIG_P6) 115 | self.dig_p7 = self.read_word_sign(BME280_DIG_P7) 116 | self.dig_p8 = self.read_word_sign(BME280_DIG_P8) 117 | self.dig_p9 = self.read_word_sign(BME280_DIG_P9) 118 | 119 | self.dig_h1 = self.read_byte(BME280_DIG_H1) # unsigned char 120 | self.dig_h2 = self.read_word_sign(BME280_DIG_H2) 121 | self.dig_h3 = self.read_byte(BME280_DIG_H3) # unsigned char 122 | 123 | self.dig_h4 = (self.read_byte(BME280_DIG_H4) << 24) >> 20 124 | self.dig_h4 = self.dig_h4 | self.read_byte(BME280_DIG_H4+1) & 0x0F 125 | 126 | self.dig_h5 = (self.read_byte(BME280_DIG_H5+1) << 24) >> 20 127 | self.dig_h5 = self.dig_h5 | (self.read_byte(BME280_DIG_H5) >> 4) & 0x0F 128 | 129 | self.dig_h6 = self.read_byte(BME280_DIG_H6) # signed char 130 | if self.dig_h6 > 127: 131 | self.dig_h6 = 127-self.dig_h6 132 | 133 | # Set Configuration 134 | self.write_byte(BME280_CONFIG, BME280_CONFIG_SET) 135 | self.write_byte(BME280_CONTROL_HUM, BME280_CONTROL_HUM_SET) 136 | self.write_byte(BME280_CONTROL_MEAS, BME280_CONTROL_MEAS_SET) 137 | 138 | def get_data(self): 139 | adc_t = self.read_adc_long(BME280_TEMP) 140 | adc_p = self.read_adc_long(BME280_PRESSURE) 141 | adc_h = self.read_adc_word(BME280_HUMIDITY) 142 | 143 | var1 = (adc_t/16384.0 - self.dig_t1/1024.0) * self.dig_t2; 144 | var2 = ((adc_t/131072.0 - self.dig_t1/8192.0) * (adc_t/131072.0 - self.dig_t1/8192.0)) * self.dig_t3; 145 | t_fine = (var1 + var2); 146 | temperature = round((t_fine / 5120.0), 2); 147 | 148 | var1 = (t_fine/2.0) - 64000.0; 149 | var2 = var1 * var1 * self.dig_p6 / 32768.0; 150 | var2 = var2 + var1 * self.dig_p5 * 2.0; 151 | var2 = (var2/4.0)+(self.dig_p4 * 65536.0); 152 | var1 = (self.dig_p3 * var1 * var1 / 524288.0 + self.dig_p2 * var1) / 524288.0; 153 | var1 = (1.0 + var1 / 32768.0)*self.dig_p1; 154 | 155 | # Avoid exception caused by division by zero 156 | if (var1 == 0.0): 157 | return -1 158 | 159 | p = 1048576.0 - adc_p; 160 | p = (p - (var2 / 4096.0)) * 6250.0 / var1; 161 | var1 = self.dig_p9 * p * p / 2147483648.0; 162 | var2 = p * self.dig_p8 / 32768.0; 163 | pressure = round((p + (var1 + var2 + self.dig_p7) / 16.0), 2); 164 | 165 | var_H = t_fine - 76800.0 166 | var_H = (adc_h-(self.dig_h4*64.0+self.dig_h5 / 16384.0 * var_H))*(self.dig_h2/65536.0 * (1.0 + self.dig_h6/67108864.0 * var_H * (1.0+self.dig_h3/67108864.0 * var_H))) 167 | humidity = round(var_H * (1.0 - self.dig_h1*var_H/524288.0), 2) 168 | if (humidity > 100.0): 169 | humidity = 100.0 170 | else: 171 | if(humidity < 0.0): 172 | humidity = 0.0 173 | 174 | return {'t':temperature, 'p':pressure, 'h':humidity} 175 | 176 | def get_altitude(self, pressure): 177 | temp = pressure/101325; 178 | temp = 1-pow(temp, 0.19029); 179 | altitude = round(44330*temp, 3); 180 | return altitude; 181 | 182 | def read_byte(self, adr): 183 | return self.bus.read_byte_data(self.address, adr) 184 | 185 | def read_word(self, adr): 186 | # ATANTION! Joke from Bosch! LBS before HBS. For calibration registers only! 187 | lbs = self.bus.read_byte_data(self.address, adr) 188 | hbs = self.bus.read_byte_data(self.address, adr+1) 189 | return (hbs << 8) + lbs 190 | 191 | def read_word_sign(self, adr): 192 | val = self.read_word(adr) 193 | if (val >= 0x8000): 194 | return -((65535 - val) + 1) 195 | else: 196 | return val 197 | 198 | def read_adc_long(self, adr): 199 | mbs = self.bus.read_byte_data(self.address, adr) 200 | lbs = self.bus.read_byte_data(self.address, adr+1) 201 | xbs = self.bus.read_byte_data(self.address, adr+2) 202 | val = (mbs << 16) + (lbs << 8) + xbs 203 | val = (val >> 4) 204 | return val 205 | 206 | def read_adc_word(self, adr): 207 | mbs = self.bus.read_byte_data(self.address, adr) 208 | lbs = self.bus.read_byte_data(self.address, adr+1) 209 | val = (mbs << 8) + lbs 210 | return val 211 | 212 | def write_byte(self, adr, byte): 213 | self.bus.write_byte_data(self.address, adr, byte) 214 | -------------------------------------------------------------------------------- /BME280/BME280_sample.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | 4 | from BME280 import * 5 | 6 | ps = BME280() 7 | 8 | while True: 9 | ps_data = ps.get_data() 10 | print "Temperature:", ps_data['t'], "°C", "Pressure:", ps_data['p'], "Pa", "Altitude:", ps.get_altitude(ps_data['p']),"m", "Humidity:", ps_data['h'],"%" 11 | time.sleep(1) 12 | 13 | -------------------------------------------------------------------------------- /BMP280/BMP280.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | # Copyright (c) 2015 Andrey Koryagin 3 | # Permission is hereby granted, free of charge, to any person obtaining a copy 4 | # of this software and associated documentation files (the "Software"), to deal 5 | # in the Software without restriction, including without limitation the rights 6 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | # copies of the Software, and to permit persons to whom the Software is 8 | # furnished to do so, subject to the following conditions: 9 | # The above copyright notice and this permission notice shall be included in 10 | # all copies or substantial portions of the Software. 11 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 12 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 13 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 14 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 15 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 16 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 17 | # THE SOFTWARE. 18 | 19 | import smbus 20 | import time 21 | 22 | # BMP280 default address. 23 | BMP280_I2CADDR = 0x77 24 | 25 | # BMP280 Registers 26 | BMP280_CONTROL = 0xF4 27 | BMP280_CONFIG = 0xF5 28 | BMP280_PRESSURE = 0xF7 29 | BMP280_TEMP = 0xFA 30 | 31 | BMP280_DIG_T1 = 0x88 32 | BMP280_DIG_T2 = 0x8A 33 | BMP280_DIG_T3 = 0x8C 34 | BMP280_DIG_P1 = 0x8E 35 | BMP280_DIG_P2 = 0x90 36 | BMP280_DIG_P3 = 0x92 37 | BMP280_DIG_P4 = 0x94 38 | BMP280_DIG_P5 = 0x96 39 | BMP280_DIG_P6 = 0x98 40 | BMP280_DIG_P7 = 0x9A 41 | BMP280_DIG_P8 = 0x9C 42 | BMP280_DIG_P9 = 0x9E 43 | 44 | # Oversampling Setting 45 | BMP280_OVERS_T1 = 0x20 46 | BMP280_OVERS_T2 = 0x40 47 | BMP280_OVERS_T4 = 0x60 48 | BMP280_OVERS_T8 = 0x80 49 | BMP280_OVERS_T16 = 0xA0 50 | 51 | BMP280_OVERS_P1 = 0x04 52 | BMP280_OVERS_P2 = 0x08 53 | BMP280_OVERS_P4 = 0x0C 54 | BMP280_OVERS_P8 = 0x10 55 | BMP280_OVERS_P16 = 0x14 56 | 57 | # Power Modes. 58 | # This lib uses NORMAL mode only! 59 | #BMP280_SLEEP_MODE = 0x00 60 | #BMP280_FORCED_MODE = 0x01 61 | BMP280_NORMAL_MODE = 0x03 62 | 63 | BMP280_TSB_0_5 = 0x00 64 | BMP280_TSB_62_5 = 0x20 65 | BMP280_TSB_125 = 0x40 66 | BMP280_TSB_250 = 0x60 67 | BMP280_TSB_500 = 0x80 68 | BMP280_TSB_1000 = 0xA0 69 | BMP280_TSB_2000 = 0xC0 70 | BMP280_TSB_4000 = 0xE0 71 | 72 | BMP280_FILTER_OFF = 0x00 73 | BMP280_FILTER_COEFFICIENT2 = 0x04 74 | BMP280_FILTER_COEFFICIENT4 = 0x08 75 | BMP280_FILTER_COEFFICIENT8 = 0x0C 76 | BMP280_FILTER_COEFFICIENT16 = 0x10 77 | 78 | BMP280_SPI_OFF = 0x00 79 | BMP280_SPI_ON = 0x01 80 | 81 | BMP280_CONTROL_SET = (BMP280_OVERS_T16 | BMP280_OVERS_P16 | BMP280_NORMAL_MODE) 82 | BMP280_CONFIG_SET = (BMP280_TSB_0_5 | BMP280_FILTER_COEFFICIENT16 | BMP280_SPI_OFF) 83 | 84 | class BMP280(object): 85 | def __init__(self, port=1, address=BMP280_I2CADDR): 86 | self.bus = smbus.SMBus(port) 87 | self.address = address 88 | 89 | # Read calibration values 90 | self.dig_t1 = self.read_word(BMP280_DIG_T1) # Unsigned 91 | self.dig_t2 = self.read_word_sign(BMP280_DIG_T2) 92 | self.dig_t3 = self.read_word_sign(BMP280_DIG_T3) 93 | self.dig_p1 = self.read_word(BMP280_DIG_P1) # Unsigned 94 | self.dig_p2 = self.read_word_sign(BMP280_DIG_P2) 95 | self.dig_p3 = self.read_word_sign(BMP280_DIG_P3) 96 | self.dig_p4 = self.read_word_sign(BMP280_DIG_P4) 97 | self.dig_p5 = self.read_word_sign(BMP280_DIG_P5) 98 | self.dig_p6 = self.read_word_sign(BMP280_DIG_P6) 99 | self.dig_p7 = self.read_word_sign(BMP280_DIG_P7) 100 | self.dig_p8 = self.read_word_sign(BMP280_DIG_P8) 101 | self.dig_p9 = self.read_word_sign(BMP280_DIG_P9) 102 | 103 | # Set Configuration 104 | self.write_byte(BMP280_CONFIG, BMP280_CONFIG_SET) 105 | self.write_byte(BMP280_CONTROL, BMP280_CONTROL_SET) 106 | 107 | def get_data(self): 108 | adc_t = self.read_long(BMP280_TEMP) 109 | adc_p = self.read_long(BMP280_PRESSURE) 110 | 111 | var1 = (adc_t/16384.0 - self.dig_t1/1024.0) * self.dig_t2; 112 | var2 = ((adc_t/131072.0 - self.dig_t1/8192.0) * (adc_t/131072.0 - self.dig_t1/8192.0)) * self.dig_t3; 113 | t_fine = (var1 + var2); 114 | temperature = round((t_fine / 5120.0), 2); 115 | 116 | var1 = (t_fine/2.0) - 64000.0; 117 | var2 = var1 * var1 * self.dig_p6 / 32768.0; 118 | var2 = var2 + var1 * self.dig_p5 * 2.0; 119 | var2 = (var2/4.0)+(self.dig_p4 * 65536.0); 120 | var1 = (self.dig_p3 * var1 * var1 / 524288.0 + self.dig_p2 * var1) / 524288.0; 121 | var1 = (1.0 + var1 / 32768.0)*self.dig_p1; 122 | 123 | # Avoid exception caused by division by zero 124 | if (var1 == 0.0): 125 | return -1 126 | 127 | p = 1048576.0 - adc_p; 128 | p = (p - (var2 / 4096.0)) * 6250.0 / var1; 129 | var1 = self.dig_p9 * p * p / 2147483648.0; 130 | var2 = p * self.dig_p8 / 32768.0; 131 | pressure = round((p + (var1 + var2 + self.dig_p7) / 16.0), 2); 132 | 133 | return {'t':temperature, 'p':pressure} 134 | 135 | def get_altitude(self, pressure): 136 | temp = pressure/101325; 137 | temp = 1-pow(temp, 0.19029); 138 | altitude = round(44330*temp, 3); 139 | return altitude; 140 | 141 | def read_byte(self, adr): 142 | return self.bus.read_byte_data(self.address, adr) 143 | 144 | def read_word(self, adr): 145 | # ATANTION! Joke from Bosch! LBS before HBS. For calibration registers only! 146 | lbs = self.bus.read_byte_data(self.address, adr) 147 | hbs = self.bus.read_byte_data(self.address, adr+1) 148 | return (hbs << 8) + lbs 149 | 150 | def read_word_sign(self, adr): 151 | val = self.read_word(adr) 152 | if (val >= 0x8000): 153 | return -((65535 - val) + 1) 154 | else: 155 | return val 156 | 157 | def read_long(self, adr): 158 | mbs = self.bus.read_byte_data(self.address, adr) 159 | lbs = self.bus.read_byte_data(self.address, adr+1) 160 | xbs = self.bus.read_byte_data(self.address, adr+2) 161 | val = (mbs << 16) + (lbs << 8) + xbs 162 | val = (val >> 4) 163 | return val 164 | 165 | def write_byte(self, adr, byte): 166 | self.bus.write_byte_data(self.address, adr, byte) 167 | -------------------------------------------------------------------------------- /BMP280/BMP280_sample.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | 4 | from BMP280 import * 5 | 6 | ps = BMP280() 7 | 8 | while True: 9 | ps_data = ps.get_data() 10 | print "Temperature:", ps_data['t'], "°C", "Pressure:", ps_data['p'], "Pa", "Altitude:", ps.get_altitude(ps_data['p']),"m" 11 | time.sleep(1) 12 | 13 | -------------------------------------------------------------------------------- /HC-SR04/hc-sr04_sample.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | 4 | from hcsr04 import * 5 | 6 | sonar = hcsr04(23, 24) 7 | 8 | while True: 9 | print sonar.get_distance(), sonar.error 10 | time.sleep(0.01) 11 | 12 | -------------------------------------------------------------------------------- /HC-SR04/hcsr04.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | # Copyright (c) 2015 Andrey Koryagin 3 | # Permission is hereby granted, free of charge, to any person obtaining a copy 4 | # of this software and associated documentation files (the "Software"), to deal 5 | # in the Software without restriction, including without limitation the rights 6 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | # copies of the Software, and to permit persons to whom the Software is 8 | # furnished to do so, subject to the following conditions: 9 | # The above copyright notice and this permission notice shall be included in 10 | # all copies or substantial portions of the Software. 11 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 12 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 13 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 14 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 15 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 16 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 17 | # THE SOFTWARE. 18 | 19 | import time 20 | import RPi.GPIO as GPIO 21 | 22 | class hcsr04: 23 | distance = 0 24 | error = 0 25 | 26 | def __init__(self, TRIGGER=23, ECHO=24): 27 | self.GPIO_TRIGGER = TRIGGER 28 | self.GPIO_ECHO = ECHO 29 | 30 | # Define GPIO 31 | GPIO.setmode(GPIO.BCM) 32 | GPIO.setup(self.GPIO_TRIGGER, GPIO.OUT) # Trigger 33 | GPIO.setup(self.GPIO_ECHO, GPIO.IN) # Echo 34 | GPIO.output(self.GPIO_TRIGGER, False) 35 | 36 | #time.sleep(0.5) 37 | 38 | def get_distance(self): 39 | # Send 10us pulse to trigger 40 | GPIO.output(self.GPIO_TRIGGER, True) 41 | time.sleep(0.00001) 42 | GPIO.output(self.GPIO_TRIGGER, False) 43 | 44 | timeout = 0 45 | start_time = time.time() 46 | stop_time = start_time 47 | while GPIO.input(self.GPIO_ECHO)==0: 48 | start_time = time.time() 49 | timeout +=1 50 | if timeout > 1000: 51 | break 52 | 53 | timeout = 0 54 | while GPIO.input(self.GPIO_ECHO)==1: 55 | stop_time = time.time() 56 | timeout +=1 57 | if timeout > 1000: 58 | break 59 | 60 | # Calculate distance 61 | distance = round((stop_time-start_time)*17150, 2) 62 | 63 | # Check whether the distance is within range 64 | if distance >= 2 and distance <= 400: 65 | self.distance = distance 66 | self.error = 0 67 | return distance 68 | else: 69 | # Out Of Range (incorrect measurement) 70 | self.error = -1 71 | return self.distance 72 | 73 | def __del__(self): 74 | # Reset GPIO settings 75 | GPIO.cleanup([self.GPIO_TRIGGER, self.GPIO_ECHO]) 76 | -------------------------------------------------------------------------------- /HMC5883L/HMC5883L_calibr.txt: -------------------------------------------------------------------------------- 1 | 413 -325 -280 2 | 390 -312 -322 3 | 359 -347 -329 4 | 327 -325 -372 5 | 304 -292 -415 6 | 254 -281 -446 7 | 241 -275 -454 8 | 224 -264 -466 9 | 179 -245 -488 10 | 147 -229 -500 11 | 134 -224 -504 12 | 135 -227 -502 13 | 141 -226 -503 14 | 154 -223 -499 15 | 170 -231 -495 16 | 180 -241 -487 17 | 184 -239 -486 18 | 194 -225 -489 19 | 213 -192 -490 20 | 239 -96 -494 21 | 228 -69 -501 22 | 209 -63 -506 23 | 198 -44 -508 24 | 181 -10 -510 25 | 159 -4 -514 26 | 156 25 -513 27 | 82 45 -517 28 | 20 27 -523 29 | -16 13 -521 30 | -43 -8 -522 31 | -67 -33 -520 32 | -60 -25 -520 33 | -56 -23 -522 34 | -47 -23 -521 35 | -33 -57 -524 36 | 0 -96 -527 37 | 4 -120 -527 38 | 13 -154 -522 39 | 22 -176 -521 40 | 32 -169 -521 41 | 51 -158 -524 42 | 52 -156 -524 43 | 51 -161 -524 44 | 22 -200 -516 45 | -33 -233 -502 46 | -72 -227 -499 47 | -105 -207 -495 48 | -145 -145 -498 49 | -159 -69 -498 50 | -110 -29 -513 51 | -84 -67 -521 52 | -74 -112 -518 53 | -96 -175 -504 54 | -156 -245 -467 55 | -199 -292 -427 56 | -240 -269 -417 57 | -272 -180 -428 58 | -247 -59 -462 59 | -166 8 -495 60 | -6 226 -458 61 | 135 206 -456 62 | 165 225 -436 63 | 188 257 -406 64 | 304 220 -364 65 | 389 49 -377 66 | 359 -149 -413 67 | 323 -228 -421 68 | 202 -342 -428 69 | 122 -375 -427 70 | 87 -398 -416 71 | 43 -408 -414 72 | -27 -441 -380 73 | -52 -443 -374 74 | -53 -432 -382 75 | -78 -432 -374 76 | -127 -431 -359 77 | -157 -424 -350 78 | -144 -451 -327 79 | -14 -471 -349 80 | 89 -448 -370 81 | 250 -348 -399 82 | 294 -237 -435 83 | 317 -105 -449 84 | 323 73 -426 85 | 247 146 -441 86 | 217 209 -423 87 | 149 314 -368 88 | 98 357 -339 89 | -22 361 -342 90 | -85 360 -330 91 | -60 366 -330 92 | -57 379 -312 93 | -30 410 -275 94 | -1 471 -119 95 | -216 422 50 96 | -212 423 63 97 | -160 433 95 98 | -102 432 136 99 | -113 411 173 100 | -204 353 209 101 | -290 277 232 102 | -376 137 243 103 | -417 -52 234 104 | -398 -247 183 105 | -332 -357 169 106 | -233 -429 180 107 | -116 -476 185 108 | -27 -503 167 109 | 135 -476 183 110 | 335 -407 112 111 | 412 -350 59 112 | 505 -170 19 113 | 510 -44 49 114 | 473 103 92 115 | 384 236 143 116 | 269 360 133 117 | 198 439 -4 118 | 191 442 -118 119 | 165 421 -211 120 | 128 365 -330 121 | 205 283 -386 122 | 239 262 -382 123 | 345 196 -354 124 | 332 180 -380 125 | 382 105 -372 126 | 418 28 -362 127 | 349 80 -414 128 | 258 111 -459 129 | 108 136 -496 130 | 51 116 -509 131 | 10 131 -503 132 | -16 183 -480 133 | -53 173 -482 134 | -89 159 -479 135 | -130 177 -460 136 | -221 172 -425 137 | -342 85 -374 138 | -464 3 -236 139 | -501 -30 -136 140 | -510 -57 4 141 | -476 -58 139 142 | -413 -39 244 143 | -298 -2 343 144 | -166 -8 402 145 | -41 23 417 146 | 53 -8 415 147 | 228 -152 349 148 | 369 -198 234 149 | 444 -260 85 150 | 468 -285 -58 151 | 453 -295 -170 152 | 402 -323 -255 153 | 375 -351 -254 154 | 353 -357 -275 155 | 272 -359 -357 156 | 125 -386 -400 157 | -15 -426 -376 158 | -93 -445 -337 159 | -196 -463 -250 160 | -323 -407 -176 161 | -408 -329 -115 162 | -406 -303 -178 163 | -250 -188 -426 164 | -303 -77 -408 165 | -355 6 -366 166 | -387 52 -323 167 | -422 42 -281 168 | -450 -198 -198 169 | -414 -232 -247 170 | -409 -258 -233 171 | -430 -237 -209 172 | -389 -285 -243 173 | -330 -358 -256 174 | -153 -466 -292 175 | -44 -484 -316 176 | -109 -452 -332 177 | -279 -344 -322 178 | -447 11 -248 179 | -395 237 -185 180 | -292 323 -237 181 | -80 384 -299 182 | 68 329 -372 183 | 359 71 -396 184 | 391 -194 -370 185 | 316 -374 -327 186 | 125 -506 -283 187 | -32 -444 -372 188 | -108 -470 -314 189 | -166 -515 -177 190 | -243 -478 -168 191 | -332 -425 -24 192 | -430 -298 28 193 | -434 -261 89 194 | -431 -244 120 195 | -428 -220 148 196 | -425 -236 141 197 | -402 -259 161 198 | -306 -368 173 199 | -201 -445 166 200 | -26 -502 151 201 | 197 -482 113 202 | 279 -453 77 203 | 294 -472 -18 204 | 204 -520 -144 205 | 84 -520 -246 206 | -54 -358 -433 207 | -108 -305 -449 208 | -213 -230 -441 209 | -278 -166 -422 210 | -356 -159 -358 211 | -428 -212 -230 212 | -477 -151 -150 213 | -494 -91 -119 214 | -478 -43 -183 215 | -461 7 -221 216 | -379 -129 -337 217 | -372 -142 -339 218 | -349 -210 -338 219 | -260 -255 -394 220 | -181 -303 -415 221 | -192 -273 -427 222 | -123 -261 -463 223 | -68 -267 -476 224 | -31 -206 -506 225 | 86 -156 -516 226 | 182 -178 -492 227 | 217 -194 -476 228 | 281 230 -360 229 | 245 263 -357 230 | 156 312 -356 231 | 71 290 -396 232 | 77 238 -433 233 | 51 210 -452 234 | 22 160 -477 235 | 28 116 -494 236 | 80 88 -499 237 | 108 51 -505 238 | 132 55 -501 239 | 136 87 -492 240 | 99 179 -465 241 | -82 341 -342 242 | -158 343 -312 243 | -375 102 -320 244 | -396 94 -297 245 | -403 138 -264 246 | -409 167 -236 247 | -371 244 -220 248 | -379 298 -71 249 | -360 323 -21 250 | -334 341 35 251 | -327 339 65 252 | -316 342 92 253 | -232 376 143 254 | -66 392 206 255 | 75 365 233 256 | 158 280 283 257 | 245 183 297 258 | 454 20 136 259 | 490 10 59 260 | 501 -12 32 261 | 492 -14 63 262 | 475 -32 116 263 | 416 79 189 264 | 341 202 214 265 | 213 307 235 266 | 92 337 261 267 | -56 334 278 268 | -173 323 255 269 | -307 311 156 270 | -358 293 97 271 | -388 281 36 272 | -404 265 -34 273 | -414 250 -10 274 | -419 241 6 275 | -417 247 -30 276 | -398 273 -60 277 | -392 272 -110 278 | -452 104 -185 279 | -461 84 -183 280 | -444 73 -229 281 | -373 82 -332 282 | -254 16 -447 283 | -179 -22 -486 284 | -113 -80 -509 285 | -34 -156 -516 286 | 56 -196 -510 287 | 191 -176 -491 288 | 229 -82 -490 289 | 205 -7 -492 290 | 143 -18 -510 291 | 118 -109 -513 292 | 99 -167 -508 293 | 99 -256 -479 294 | 71 -349 -433 295 | 27 -438 -356 296 | -14 -496 -264 297 | -73 -517 -192 298 | -127 -512 -152 299 | -177 -486 -181 300 | -183 -451 -254 301 | -184 -401 -321 302 | -203 -356 -352 303 | -218 -394 -301 304 | -209 -461 -210 305 | -152 -514 -141 306 | -141 -523 -113 307 | -177 -512 -87 308 | -198 -502 -99 309 | -250 -476 -100 310 | -302 -437 -108 311 | -358 -376 -136 312 | -374 -334 -193 313 | -350 -315 -263 314 | -315 -242 -360 315 | -267 -198 -415 316 | -229 -158 -453 317 | -54 -246 -490 318 | -4 -265 -489 319 | -14 -249 -495 320 | 2 -205 -510 321 | 14 -167 -519 322 | 90 -120 -523 323 | 217 -2 -494 324 | 210 26 -493 325 | 196 18 -497 326 | 197 -2 -499 327 | 167 -62 -512 328 | 152 -22 -514 329 | 127 26 -513 330 | 53 71 -514 331 | -53 -78 -524 332 | -208 -324 -395 333 | -263 -378 -307 334 | -316 -353 -269 335 | -352 -342 -221 336 | -365 -337 -191 337 | -387 -292 -217 338 | -359 -274 -280 339 | -296 -330 -313 340 | -206 -352 -372 341 | -88 -307 -451 342 | 8 -245 -493 343 | 66 -178 -509 344 | 17 3 -517 345 | 18 173 -470 346 | 99 435 -183 347 | 85 462 -28 348 | 67 453 68 349 | 72 354 244 350 | 113 229 334 351 | 67 158 376 352 | 11 93 403 353 | -15 15 414 354 | -30 -110 409 355 | -66 -208 383 356 | -95 -293 339 357 | -103 -360 297 358 | -116 -391 266 359 | -128 -413 242 360 | -102 -434 228 361 | -101 -423 240 362 | -206 -423 185 363 | -181 -390 241 364 | -189 -323 292 365 | -217 -284 307 366 | -263 -302 265 367 | -295 -293 250 368 | -354 -287 190 369 | -394 -258 163 370 | -414 -240 146 371 | -418 -230 147 372 | -434 -212 136 373 | -445 -214 102 374 | -423 -247 130 375 | -407 -236 170 376 | -363 -283 200 377 | -239 -276 313 378 | -179 -248 355 379 | -176 -105 398 380 | -217 72 381 381 | -312 207 266 382 | -392 230 145 383 | -419 240 22 384 | -362 304 63 385 | -300 310 173 386 | -277 215 287 387 | -237 73 367 388 | -207 -80 386 389 | -154 -120 400 390 | -94 -64 415 391 | -109 25 413 392 | -215 124 362 393 | -237 64 367 394 | -235 -43 377 395 | -213 -76 386 396 | -205 -87 387 397 | -230 -74 380 398 | -255 -111 360 399 | -253 -184 347 400 | -209 -228 349 401 | -160 -275 348 402 | -100 -272 366 403 | -38 -156 413 404 | -33 -30 428 405 | -117 87 404 406 | -239 143 346 407 | -373 148 222 408 | -406 100 195 409 | -435 27 171 410 | -464 7 103 411 | -484 -43 24 412 | -484 -63 -119 413 | -462 -105 -197 414 | -472 -144 -168 415 | -467 -136 -183 416 | -472 -156 -160 417 | -485 -141 -102 418 | -498 -58 -43 419 | -496 1 -35 420 | -481 79 -53 421 | -461 141 -94 422 | -401 220 -169 423 | -316 311 -183 424 | -371 246 -196 425 | -334 117 -353 426 | -184 206 -410 427 | -182 303 -335 428 | -181 310 -328 429 | -244 318 -276 430 | -256 324 -252 431 | -282 274 -290 432 | -304 202 -330 433 | -263 249 -330 434 | -112 272 -391 435 | -27 249 -425 436 | 26 132 -486 437 | 15 -136 -514 438 | -220 -100 -453 439 | -285 -78 -418 440 | -292 -62 -413 441 | -280 -54 -424 442 | -306 10 -402 443 | -305 121 -370 444 | -173 292 -351 445 | -30 272 -410 446 | 55 181 -466 447 | 135 84 -489 448 | 238 -48 -478 449 | 288 -108 -450 450 | 288 -119 -451 451 | 268 -164 -455 452 | 328 -304 -353 453 | 263 -430 -277 454 | 128 -522 -205 455 | 10 -548 -140 456 | -96 -540 -77 457 | -181 -510 7 458 | -240 -468 55 459 | -244 -429 143 460 | -227 -403 191 461 | -220 -417 182 462 | -180 -433 189 463 | -87 -373 291 464 | -183 -234 347 465 | -151 -132 391 466 | -162 -129 390 467 | -2 -94 417 468 | -1 -102 416 469 | 53 -41 416 470 | 203 -62 377 471 | 316 -119 307 472 | 332 -95 299 473 | 368 -131 261 474 | 443 -160 162 475 | 521 -99 -50 476 | 487 -70 -262 477 | 394 -83 -391 478 | 236 -154 -488 479 | 28 -204 -510 480 | -170 -197 -469 481 | -307 -193 -380 482 | -445 -206 -145 483 | -450 -222 -26 484 | -446 -206 60 485 | -430 -197 114 486 | -430 -203 109 487 | -431 -216 87 488 | -431 -227 75 489 | -409 -292 25 490 | -297 -417 -138 491 | -130 -428 -330 492 | 49 -380 -413 493 | 237 -349 -387 494 | 344 -324 -324 495 | 319 -401 -273 496 | 412 -371 -98 497 | 410 -374 -33 498 | 402 -358 44 499 | 376 -345 123 500 | 364 -335 146 501 | 336 -372 149 502 | 241 -400 209 503 | -26 -394 291 504 | -93 -388 285 505 | -194 -342 284 506 | -291 -262 280 507 | -339 -234 251 508 | -399 -199 202 509 | -440 -170 149 510 | -448 -185 124 511 | -411 -254 135 512 | -406 -277 119 513 | -399 -300 101 514 | -391 -323 85 515 | -425 -284 43 516 | -442 -274 -9 517 | -473 -195 -47 518 | -491 -112 -78 519 | -482 -88 -140 520 | -442 -51 -256 521 | -359 -13 -364 522 | -304 3 -406 523 | -290 0 -419 524 | -246 -41 -450 525 | -251 -68 -448 526 | -215 -128 -461 527 | -100 -234 -483 528 | -5 -227 -505 529 | 111 -212 -506 530 | 215 -192 -483 531 | 274 -128 -472 532 | 302 -103 -457 533 | 284 -61 -472 534 | 218 -45 -498 535 | 91 -61 -526 536 | -28 -130 -519 537 | -83 -188 -499 538 | -28 -187 -509 539 | -18 -198 -504 540 | 28 -264 -488 541 | 352 -178 -395 542 | 400 -52 -367 543 | 383 35 -374 544 | 339 93 -393 545 | 249 114 -446 546 | 191 113 -471 547 | 107 207 -449 548 | 45 267 -419 549 | 13 268 -418 550 | -2 269 -419 551 | 13 245 -436 552 | -108 152 -468 553 | -109 71 -494 554 | -124 6 -499 555 | -132 -54 -503 556 | -209 -165 -457 557 | -283 -256 -374 558 | -320 -300 -314 559 | -348 -353 -217 560 | -359 -390 -78 561 | -347 -405 -8 562 | -405 -322 42 563 | -467 -215 32 564 | -468 -186 75 565 | -435 -183 166 566 | -419 -203 182 567 | -360 -274 214 568 | -268 -349 240 569 | -184 -402 247 570 | -93 -391 293 571 | -78 -352 326 572 | -68 -342 331 573 | -41 -322 347 574 | 54 -313 347 575 | 221 -415 204 576 | 350 -407 78 577 | 442 -338 -60 578 | 479 -201 -231 579 | 413 -83 -368 580 | 283 40 -462 581 | 183 60 -495 582 | 56 141 -496 583 | -55 190 -468 584 | -143 203 -438 585 | -215 179 -420 586 | -381 -45 -352 587 | -395 -151 -318 588 | -392 -266 -265 589 | -352 -379 -193 590 | -303 -444 -156 591 | -239 -497 -128 592 | -99 -556 -84 593 | 68 -569 -35 594 | 216 -516 42 595 | 240 -500 68 596 | 227 -491 99 597 | 272 -445 138 598 | 356 -345 165 599 | 311 -322 233 600 | 190 -342 298 601 | -394 -213 225 602 | -495 14 100 603 | -505 76 26 604 | -506 67 -32 605 | -508 -7 -108 606 | -510 -8 -85 607 | -510 36 -46 608 | -503 81 -43 609 | -423 253 -95 610 | -350 325 -144 611 | -289 367 -162 612 | -211 410 -173 613 | -66 448 -182 614 | 2 421 -256 615 | 42 357 -343 616 | 204 194 -433 617 | 254 82 -455 618 | 266 -47 -473 619 | 168 -160 -497 620 | 60 -178 -509 621 | -37 -139 -509 622 | -112 -81 -501 623 | -144 -10 -488 624 | -216 123 -425 625 | -249 224 -353 626 | -279 227 -319 627 | -286 336 -181 628 | -301 320 -176 629 | -285 343 -161 630 | -267 367 -128 631 | 23 459 -88 632 | 126 439 -110 633 | 199 417 -103 634 | 289 366 -17 635 | 321 330 47 636 | 376 253 86 637 | 443 157 68 638 | 492 40 31 639 | 500 -43 28 640 | 507 -108 -5 641 | 478 -216 27 642 | 421 -331 45 643 | 258 -489 31 644 | 112 -542 47 645 | -1 -537 84 646 | -87 -514 123 647 | -77 -490 169 648 | -167 -379 270 649 | -255 -118 359 650 | -259 26 365 651 | -207 139 363 652 | -88 234 357 653 | 25 271 336 654 | 148 220 340 655 | 199 160 345 656 | 229 207 307 657 | 282 277 220 658 | 359 297 86 659 | 405 274 28 660 | 454 210 -24 661 | 422 255 -121 662 | 358 332 -85 663 | -75 456 96 664 | -118 352 255 665 | -132 282 316 666 | -157 136 383 667 | -78 -110 419 668 | 7 -201 400 669 | 73 -234 383 670 | 226 -381 240 671 | 332 -444 33 672 | 365 -439 -95 673 | 343 -427 -218 674 | 331 -322 -368 675 | 144 -199 -503 676 | 0 -259 -493 677 | -140 -230 -474 678 | -274 -174 -422 679 | -372 -202 -324 680 | -464 -196 -157 681 | -455 -264 -25 682 | -435 -234 127 683 | -406 -169 215 684 | -335 -97 314 685 | -263 -20 364 686 | -190 44 390 687 | -108 51 409 688 | -24 113 403 689 | 52 247 344 690 | 121 331 262 691 | 192 385 147 692 | 266 399 -29 693 | 312 362 -102 694 | 395 273 -137 695 | 512 2 -86 696 | 501 -2 -172 697 | 498 -137 -158 698 | 441 -309 -134 699 | 347 -425 -133 700 | 337 -436 -114 701 | 276 -480 -61 702 | 157 -530 -110 703 | 47 -546 -135 704 | 62 -543 -144 705 | 192 -541 -137 706 | -83 -544 -90 707 | -85 -546 -94 708 | -142 -533 -58 709 | -175 -522 -78 710 | -321 -405 -192 711 | -357 -334 -248 712 | -421 -207 -258 713 | -389 -97 -343 714 | -270 -36 -444 715 | -98 -59 -513 716 | 44 -104 -525 717 | 148 -133 -510 718 | 208 -143 -493 719 | 277 -127 -465 720 | 338 -109 -430 721 | 364 -77 -411 722 | 399 -133 -375 723 | 438 -174 -317 724 | 458 -229 -250 725 | 437 -328 -180 726 | 413 -332 -236 727 | 342 -287 -372 728 | 269 -205 -457 729 | 241 -153 -480 730 | 180 -173 -497 731 | 95 -197 -506 732 | -6 -236 -494 733 | -80 -265 -468 734 | -154 -304 -421 735 | -214 -346 -356 736 | -302 -397 -192 737 | -352 -372 -116 738 | -377 -359 -14 739 | -388 -326 71 740 | -384 -312 120 741 | -373 -275 185 742 | -339 -252 244 743 | -309 -230 286 744 | -281 -207 315 745 | -271 -162 339 746 | -280 -118 345 747 | -316 -28 334 748 | -361 73 288 749 | -414 141 204 750 | -456 180 69 751 | -408 162 -252 752 | -327 104 -374 753 | -244 41 -445 754 | -177 -21 -481 755 | -113 -78 -500 756 | -57 -137 -503 757 | 5 -185 -499 758 | 65 -232 -486 759 | 92 -278 -467 760 | 126 -306 -451 761 | 190 -298 -442 762 | 293 -266 -409 763 | 362 -156 -403 764 | 357 -47 -421 765 | 247 100 -461 766 | -192 202 -419 767 | -366 168 -305 768 | -460 25 -231 769 | -478 -157 -154 770 | -453 -270 -79 771 | -392 -368 -15 772 | -261 -429 141 773 | -211 -388 236 774 | -215 -331 282 775 | -286 -287 264 776 | -373 -274 171 777 | -428 -272 15 778 | -424 -264 -105 779 | -389 -260 -202 780 | -335 -266 -275 781 | -187 -287 -393 782 | -131 -265 -433 783 | -85 -158 -486 784 | -70 -27 -503 785 | -140 80 -468 786 | -292 109 -378 787 | -421 28 -253 788 | -475 -74 -111 789 | -466 -150 16 790 | -431 -182 121 791 | -412 -148 183 792 | -419 -83 195 793 | -436 -15 173 794 | -455 18 133 795 | -470 3 75 796 | -463 -100 -92 797 | -412 -149 -210 798 | -334 -186 -304 799 | -245 -167 -383 800 | -207 -104 -412 801 | -310 -69 -333 802 | -376 -187 -194 803 | -327 -339 -37 804 | -252 -385 132 805 | -144 -343 272 806 | -62 -271 343 807 | -9 -161 388 808 | 10 -28 406 809 | -54 65 403 810 | -207 124 356 811 | -455 5 128 812 | -469 -108 18 813 | -449 -156 -100 814 | -396 -183 -217 815 | -298 -254 -303 816 | -188 -293 -364 817 | -107 -268 -419 818 | -82 -177 -461 819 | -47 -43 -486 820 | 44 45 -489 821 | 40 106 -480 822 | 32 134 -477 823 | -180 137 -438 824 | -205 216 -395 825 | -202 355 -279 826 | -352 326 -119 827 | -323 355 80 828 | -364 289 132 829 | -405 167 199 830 | -438 -51 212 831 | -374 -317 155 832 | -295 -447 64 833 | -218 -511 -26 834 | -113 -546 -138 835 | -68 -499 -292 836 | -42 -388 -418 837 | 141 -397 -402 838 | 222 -282 -445 839 | 235 -227 -461 840 | 263 -138 -470 841 | 240 -81 -484 842 | 206 -199 -476 843 | 185 -283 -448 844 | 180 -355 -405 845 | 193 -418 -341 846 | 234 -447 -273 847 | 217 -495 -179 848 | 172 -522 -145 849 | 160 -533 -118 850 | 133 -544 -60 851 | 23 -546 41 852 | -40 -527 96 853 | -32 -506 148 854 | -91 -479 181 855 | -155 -451 198 856 | -226 -384 236 857 | -415 -52 244 858 | -454 -44 187 859 | -464 42 160 860 | -456 129 132 861 | -330 277 199 862 | -123 375 233 863 | 69 413 186 864 | 267 378 92 865 | 379 302 19 866 | 427 258 -73 867 | 453 212 -73 868 | 467 180 -98 869 | 376 313 -105 870 | 326 364 -35 871 | 85 461 68 872 | -74 300 127 873 | -457 184 51 874 | -495 77 23 875 | -507 -11 -3 876 | -480 -185 -111 877 | -423 -275 -182 878 | -358 -352 -213 879 | -274 -426 -230 880 | -205 -473 -230 881 | -149 -506 -214 882 | -45 -540 -193 883 | 83 -554 -149 884 | 202 -525 -8 885 | 226 -464 135 886 | 267 -347 240 887 | 360 -94 204 888 | 415 66 205 889 | 345 265 160 890 | 297 372 40 891 | 264 404 -54 892 | 255 392 -167 893 | 210 317 -345 894 | 143 211 -452 895 | 113 155 -484 896 | 46 98 -510 897 | 41 32 -521 898 | 13 -39 -529 899 | -19 -63 -523 900 | -14 -114 -521 901 | -248 -226 -495 902 | 68 -208 -499 903 | 112 -209 -493 904 | 138 -283 -462 905 | 138 -275 -463 906 | 109 -342 -436 907 | -9 -389 -402 908 | -145 -368 -382 909 | -242 -290 -376 910 | -298 -175 -383 911 | -355 15 -348 912 | -353 84 -332 913 | -368 52 -327 914 | -366 29 -333 915 | -373 34 -325 916 | -346 34 -352 917 | -43 12 -505 918 | -23 -37 -511 919 | 12 -38 -513 920 | 50 -143 -505 921 | 21 -158 -502 922 | 24 -142 -502 923 | 59 -129 -505 924 | 110 -143 -498 925 | 91 -181 -494 926 | 10 -307 -450 927 | 6 -447 -336 928 | 54 -477 -301 929 | 88 -493 -265 930 | 165 -488 -245 931 | 190 -487 -224 932 | 235 -488 -176 933 | 220 -508 -122 934 | 231 -508 -84 935 | 249 -490 0 936 | 220 -486 67 937 | 223 -477 84 938 | 229 -415 179 939 | 157 -313 303 940 | 135 -290 327 941 | 103 -235 358 942 | 62 -221 371 943 | 11 -244 367 944 | -7 -270 354 945 | 58 -302 332 946 | 148 -362 269 947 | 277 -397 149 948 | 334 -430 -3 949 | 341 -428 -138 950 | 290 -424 -262 951 | 193 -384 -378 952 | 76 -329 -445 953 | -4 -349 -432 954 | -24 -379 -406 955 | -33 -387 -398 956 | -50 -367 -409 957 | -53 -391 -388 958 | -81 -385 -383 959 | -108 -368 -389 960 | -134 -334 -405 961 | -140 -310 -416 962 | -123 -276 -440 963 | -181 -197 -448 964 | -211 -74 -450 965 | -171 7 -462 966 | -211 126 -410 967 | -216 184 -381 968 | -124 213 -407 969 | -57 236 -407 970 | -38 216 -426 971 | -85 211 -421 972 | -165 236 -382 973 | -316 230 -278 974 | -378 251 -154 975 | -390 257 -105 976 | -398 263 -47 977 | -380 236 138 978 | -304 284 195 979 | -243 240 282 980 | -171 225 329 981 | -61 237 345 982 | 48 245 338 983 | 107 246 324 984 | 137 243 317 985 | 163 220 321 986 | 182 204 321 987 | 200 200 316 988 | 214 202 305 989 | 193 212 314 990 | 136 223 330 991 | 61 258 329 992 | -39 327 280 993 | -329 347 -15 994 | -321 326 -148 995 | -327 280 -217 996 | -341 182 -288 997 | -341 103 -331 998 | -333 -8 -360 999 | -295 -52 -393 1000 | -262 -120 -411 1001 | -------------------------------------------------------------------------------- /HMC5883L/compas.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/avislab/sensorstest/a4937f94c6aae4d0c9cadc96d13ea5fc7c93e2da/HMC5883L/compas.png -------------------------------------------------------------------------------- /HMC5883L/hmc5883.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | # Copyright (c) 2015 Andrey Koryagin 3 | # Permission is hereby granted, free of charge, to any person obtaining a copy 4 | # of this software and associated documentation files (the "Software"), to deal 5 | # in the Software without restriction, including without limitation the rights 6 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | # copies of the Software, and to permit persons to whom the Software is 8 | # furnished to do so, subject to the following conditions: 9 | # The above copyright notice and this permission notice shall be included in 10 | # all copies or substantial portions of the Software. 11 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 12 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 13 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 14 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 15 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 16 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 17 | # THE SOFTWARE. 18 | 19 | import smbus 20 | import math 21 | import time 22 | 23 | class hmc5883l: 24 | HMC5883L_DEFAULT_ADDRESS = 0x1E 25 | HMC5883L_CONFIGURATION_A = 0 26 | HMC5883L_CONFIGURATION_B = 1 27 | HMC5883L_MODE = 2 28 | HMC5883L_OUTX_MBS = 3 29 | HMC5883L_OUTX_LSB = 4 30 | HMC5883L_OUTZ_MBS = 5 31 | HMC5883L_OUTZ_LSB = 6 32 | HMC5883L_OUTY_MBS = 7 33 | HMC5883L_OUTY_LSB = 9 34 | HMC5883L_STATUS = 10 35 | HMC5883L_IDENTIFICATION_A = 11 36 | HMC5883L_IDENTIFICATION_B = 12 37 | HMC5883L_IDENTIFICATION_B = 13 38 | 39 | HMC5883L_scales = { 40 | 0.88: [0, 0.73], 41 | 1.30: [1, 0.92], 42 | 1.90: [2, 1.22], 43 | 2.50: [3, 1.52], 44 | 4.00: [4, 2.27], 45 | 4.70: [5, 2.56], 46 | 5.60: [6, 3.03], 47 | 8.10: [7, 4.35], 48 | } 49 | 50 | #### Callibration data ### 51 | calibration_matrix = [ [1.259098, 0.013830, 0.039295], 52 | [0.01380, 1.245928, -0.018922], 53 | [0.039295, -0.018922, 1.360489]] 54 | bias = [11.16, -43.55, -52.62] 55 | ################################# 56 | 57 | def __init__(self, port=1, address=HMC5883L_DEFAULT_ADDRESS, gauss=1.3): 58 | self.bus = smbus.SMBus(port) 59 | self.address = address 60 | 61 | # Set HMC5883L Settings 62 | (reg, self.scale) = self.HMC5883L_scales[gauss] 63 | # Please read HMC5883L Datasheet before changing followinf settings 64 | self.bus.write_byte_data(self.address, self.HMC5883L_CONFIGURATION_A, 0x70) # 8 Average, 15 Hz, normal measurement 65 | self.bus.write_byte_data(self.address, self.HMC5883L_CONFIGURATION_B, reg << 5) # Scale 66 | self.bus.write_byte_data(self.address, self.HMC5883L_MODE, 0x00) # Continuous measurement 67 | 68 | def read_byte(self, adr): 69 | return self.bus.read_byte_data(self.address, adr) 70 | 71 | def read_word(self, adr): 72 | high = self.bus.read_byte_data(self.address, adr) 73 | low = self.bus.read_byte_data(self.address, adr+1) 74 | val = (high << 8) + low 75 | return val 76 | 77 | def read_word_2c(self, adr): 78 | val = self.read_word(adr) 79 | if (val >= 0x8000): 80 | val = -((65535 - val) + 1) 81 | if val == -4096: return None 82 | return val 83 | 84 | def write_byte(self, adr, byte): 85 | self.bus.write_byte_data(self.address, adr, byte) 86 | 87 | def get_raw(self): 88 | data = { 89 | 'x': self.read_word_2c(self.HMC5883L_OUTX_MBS), 90 | 'y': self.read_word_2c(self.HMC5883L_OUTY_MBS), 91 | 'z': self.read_word_2c(self.HMC5883L_OUTZ_MBS) 92 | } 93 | return data 94 | 95 | def get_calibrated(self): 96 | data = self.get_raw() 97 | uncalibrated_values = [data['x'] - self.bias[0], data['y'] - self.bias[1], data['z'] - self.bias[2]] 98 | calibrated_values = [0,0,0] 99 | for i in range(0,3): 100 | for j in range(0,3): 101 | calibrated_values[i] += self.calibration_matrix[i][j] * uncalibrated_values[j]; 102 | return {'x':calibrated_values[0], 103 | 'y':calibrated_values[1], 104 | 'z':calibrated_values[2]} 105 | 106 | def get_axes(self): 107 | data = self.get_calibrated() 108 | data['x'] = data['x']*self.scale 109 | data['y'] = data['y']*self.scale 110 | data['z'] = data['z']*self.scale 111 | return data 112 | 113 | # def get_angles(self): 114 | # data = self.get_calibrated() 115 | # result = {'x':0, 'y':0, 'z':0} 116 | # result['x'] = math.pi/2 + math.atan2(data['z'], data['y']) 117 | # result['y'] = math.pi/2 + math.atan2(data['z'], data['x']) 118 | # result['z'] = -math.atan2(data['y'], data['x']) 119 | # return result 120 | 121 | # def get_angles_degrees(self): 122 | # data = self.get_angles() 123 | # result = {'x': math.degrees(data['x']), 'y': math.degrees(data['y']), 'z': math.degrees(data['z'])} 124 | # return result 125 | 126 | def heading(self, Ax=0, Ay=0): 127 | data = self.get_calibrated() 128 | 129 | Ax = Ax*math.pi/180 130 | Ay = Ay*math.pi/180 131 | 132 | Mx = [ [1.0, 0.0, 0.0], 133 | [0.0, math.cos(Ax), -math.sin(Ax)], 134 | [0.0, math.sin(Ax), math.cos(Ax)]] 135 | 136 | My = [ [math.cos(Ay), 0.0, math.sin(Ay)], 137 | [0.0, 1.0, 0.0], 138 | [-math.sin(Ay), 0.0, math.cos(Ay)]] 139 | 140 | # Vector rotation 141 | values = [data['x'],data['y'],data['z']] 142 | DATA = [0,0,0] 143 | for i in range(0,3): 144 | for j in range(0,3): 145 | DATA[i] += Mx[i][j] * values[j]; 146 | 147 | values = [DATA[0],DATA[1],DATA[2]] 148 | DATA = [0,0,0] 149 | for i in range(0,3): 150 | for j in range(0,3): 151 | DATA[i] += My[i][j] * values[j]; 152 | 153 | radians = -math.atan2(DATA[1], DATA[0]) 154 | 155 | # Convert to degrees from radians 156 | return math.degrees(radians) 157 | 158 | def callibration(self): 159 | f = open('HMC5883L_calibr.txt', 'w') 160 | print "###############################################" 161 | print "Please twirl the HMC5883L around a minute..." 162 | print "The experimental data will stored to HMC5883L_calibr.txt file" 163 | print "Please use this file to calculate calibration matrix by Magneto software." 164 | print "###############################################" 165 | for num in range(0,1000): 166 | data = self.get_raw() 167 | f.write(str(data['x'])+'\t'+str(data['y'])+'\t'+str(data['z'])+'\n') 168 | time.sleep(0.07) 169 | f.close() 170 | -------------------------------------------------------------------------------- /HMC5883L/hmc5883_calibr.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | 4 | from hmc5883 import * 5 | 6 | compass = hmc5883l() 7 | compass.callibration() 8 | -------------------------------------------------------------------------------- /HMC5883L/hmc5883_calibrated.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | 4 | from hmc5883 import * 5 | 6 | compass = hmc5883l() 7 | 8 | # calibration data 9 | compass.calibration_matrix = [ [1.259098, 0.013830, 0.039295], 10 | [0.01380, 1.245928, -0.018922], 11 | [0.039295, -0.018922, 1.360489]] 12 | compass.bias = [11.16, -43.55, -52.62] 13 | 14 | while True: 15 | print compass.get_calibrated() 16 | -------------------------------------------------------------------------------- /HMC5883L/hmc5883_heading.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | 4 | from hmc5883 import * 5 | 6 | compass = hmc5883l() 7 | 8 | # calibration data 9 | compass.calibration_matrix = [ [1.259098, 0.013830, 0.039295], 10 | [0.01380, 1.245928, -0.018922], 11 | [0.039295, -0.018922, 1.360489]] 12 | compass.bias = [11.16, -43.55, -52.62] 13 | 14 | while True: 15 | print compass.heading() 16 | time.sleep(0.1) 17 | -------------------------------------------------------------------------------- /HMC5883L/hmc5883_mpu6050.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | import sys 4 | sys.path.append('../MPU-6050') 5 | from mpu6050 import * 6 | 7 | from hmc5883 import * 8 | import time 9 | 10 | # MPU-6050 11 | mpu = MPU6050() 12 | mpu.initialize() 13 | # calibration data 14 | mpu.gyro_offs = {'x': -178, 'y': 259, 'z': -104} 15 | mpu.accel_offs = {'y': -354, 'x': 389, 'z': -1482} 16 | 17 | # HMC5883L 18 | compass = hmc5883l() 19 | # calibration data 20 | compass.calibration_matrix = [ [1.259098, 0.013830, 0.039295], 21 | [0.01380, 1.245928, -0.018922], 22 | [0.039295, -0.018922, 1.360489]] 23 | compass.bias = [11.16, -43.55, -52.62] 24 | 25 | accel_data = mpu.get_accel() 26 | x_rotation = mpu.get_x_rotation(accel_data) 27 | y_rotation = mpu.get_y_rotation(accel_data) 28 | 29 | last_time = time.time() 30 | alpha = 0.95; 31 | 32 | while True: 33 | new_time = time.time() 34 | gyro_data = mpu.get_gyro() 35 | accel_data = mpu.get_accel() 36 | 37 | dt = new_time - last_time 38 | last_time = new_time 39 | gyro_angle_x = gyro_data['x']*dt + x_rotation 40 | if gyro_angle_x > 360: 41 | gyro_angle_x -= 360 42 | if gyro_angle_x < 0: 43 | gyro_angle_x = 360 + gyro_angle_x 44 | 45 | accel_angle_x = mpu.get_x_rotation(accel_data) 46 | 47 | if abs(gyro_angle_x - accel_angle_x) > 180: 48 | gyro_angle_x = accel_angle_x 49 | 50 | x_rotation = alpha*gyro_angle_x + (1.0 - alpha)*mpu.get_x_rotation(accel_data) 51 | 52 | gyro_angle_y = gyro_data['y']*dt + y_rotation 53 | y_rotation = alpha*gyro_angle_y + (1.0 - alpha)*mpu.get_y_rotation(accel_data) 54 | 55 | z_rotation = compass.heading(x_rotation, y_rotation) 56 | 57 | print "x:", x_rotation 58 | print "y:", y_rotation 59 | print "Z:", z_rotation 60 | 61 | time.sleep(0.05) 62 | -------------------------------------------------------------------------------- /HMC5883L/kfs360.py: -------------------------------------------------------------------------------- 1 | # Kalman Filter Simple 360 - Simple filter for 360 degrees values 2 | class KFS360: 3 | X0 = 0.0 4 | P0 = 0.0 5 | Q = 0.0 6 | R = 0.0 7 | F = 0.0 8 | H = 0.0 9 | State = 0.0 10 | Covariance = 0.0 11 | 12 | def __init__(self, q, r, f, h): 13 | self.Q = q 14 | self.R = r 15 | self.F = f 16 | self.H = h 17 | 18 | def correct(self, data): 19 | # +180 -> -180 or -180 -> +180 20 | if (data > 90 and self.State < -90) or (data < -90 and self.State > 90): 21 | if data > 0: 22 | self.State = self.State + 360 23 | if data < -0: 24 | self.State = self.State - 360 25 | 26 | self.X0 = self.F*self.State 27 | self.P0 = self.F*self.Covariance*self.F + self.Q 28 | K = self.H*self.P0/(self.H*self.P0*self.H + self.R) 29 | self.State = self.X0 + K*(data - self.H*self.X0) 30 | self.Covariance = (1 - K*self.H)*self.P0 31 | 32 | if self.State > 180: 33 | self.State = self.State - 360 34 | if self.State < -180: 35 | self.State = self.State + 360 36 | 37 | -------------------------------------------------------------------------------- /HMC5883L/pygame_hmc5883.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | # -*- coding: utf-8 -*- 3 | 4 | import pygame, sys, thread, time 5 | from pygame.locals import * 6 | from hmc5883 import * 7 | 8 | rotation = 0 9 | 10 | def hmc5883l_read(): 11 | global rotation 12 | 13 | # HMC5883L 14 | compass = hmc5883l() 15 | # calibration data 16 | compass.calibration_matrix = [ [1.259098, 0.013830, 0.039295], 17 | [0.01380, 1.245928, -0.018922], 18 | [0.039295, -0.018922, 1.360489]] 19 | compass.bias = [11.16, -43.55, -52.62] 20 | 21 | while True: 22 | rotation = 360 - compass.heading() 23 | time.sleep(0.1) 24 | 25 | 26 | FPS = 25 27 | WINDOWWIDTH = 640 28 | WINDOWHEIGHT = 480 29 | CELLSIZE = 20 30 | CELLWIDTH = int(WINDOWWIDTH / CELLSIZE) 31 | CELLHEIGHT = int(WINDOWHEIGHT / CELLSIZE) 32 | 33 | # R G B 34 | WHITE = (255, 255, 255) 35 | BLACK = ( 0, 0, 0) 36 | RED = (255, 0, 0) 37 | GREEN = ( 0, 255, 0) 38 | DARKGREEN = ( 0, 155, 0) 39 | DARKGRAY = ( 40, 40, 40) 40 | BGCOLOR = BLACK 41 | 42 | 43 | def main(): 44 | global FPSCLOCK, DISPLAYSURF, BASICFONT 45 | 46 | pygame.init() 47 | FPSCLOCK = pygame.time.Clock() 48 | DISPLAYSURF = pygame.display.set_mode((WINDOWWIDTH, WINDOWHEIGHT)) 49 | BASICFONT = pygame.font.Font('freesansbold.ttf', 18) 50 | pygame.display.set_caption('HMC5883L') 51 | 52 | thread.start_new_thread(hmc5883l_read,()) 53 | while True: 54 | runGame() 55 | 56 | 57 | def runGame(): 58 | global rotation 59 | 60 | titleFont = pygame.font.Font('freesansbold.ttf', 50) 61 | titleSurf1 = titleFont.render('HMC5883L', True, WHITE) 62 | img = pygame.image.load('compas.png') 63 | imgx = 10 64 | imgy = 10 65 | 66 | while True: # main game loop 67 | for event in pygame.event.get(): # event handling loop 68 | if event.type == QUIT: 69 | terminate() 70 | elif event.type == KEYDOWN: 71 | if event.key == K_ESCAPE: 72 | terminate() 73 | 74 | 75 | DISPLAYSURF.fill(BGCOLOR) 76 | drawGrid() 77 | 78 | rotatedSurf1 = pygame.transform.rotate(img, rotation) 79 | rotatedRect1 = rotatedSurf1.get_rect() 80 | rotatedRect1.center = (WINDOWWIDTH/2, WINDOWHEIGHT/2) 81 | DISPLAYSURF.blit(rotatedSurf1, rotatedRect1) 82 | 83 | pygame.display.update() 84 | FPSCLOCK.tick(FPS) 85 | 86 | 87 | def terminate(): 88 | pygame.quit() 89 | sys.exit() 90 | 91 | 92 | def drawGrid(): 93 | for x in range(0, WINDOWWIDTH, CELLSIZE): # draw vertical lines 94 | pygame.draw.line(DISPLAYSURF, DARKGRAY, (x, 0), (x, WINDOWHEIGHT)) 95 | for y in range(0, WINDOWHEIGHT, CELLSIZE): # draw horizontal lines 96 | pygame.draw.line(DISPLAYSURF, DARKGRAY, (0, y), (WINDOWWIDTH, y)) 97 | 98 | 99 | if __name__ == '__main__': 100 | main() 101 | -------------------------------------------------------------------------------- /HMC5883L/pygame_hmc5883_mpu6050.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | # -*- coding: utf-8 -*- 3 | 4 | import pygame, sys, thread, time 5 | from pygame.locals import * 6 | from hmc5883 import * 7 | sys.path.append('../MPU-6050') 8 | from mpu6050 import * 9 | 10 | rotation = 0 11 | 12 | def hmc5883l_read(): 13 | global rotation 14 | 15 | # MPU-6050 16 | mpu = MPU6050() 17 | mpu.initialize() 18 | # calibration data 19 | mpu.gyro_offs = {'x': -178, 'y': 259, 'z': -104} 20 | mpu.accel_offs = {'y': -354, 'x': 389, 'z': -1482} 21 | 22 | # HMC5883L 23 | compass = hmc5883l() 24 | # calibration data 25 | compass.calibration_matrix = [ [1.259098, 0.013830, 0.039295], 26 | [0.01380, 1.245928, -0.018922], 27 | [0.039295, -0.018922, 1.360489]] 28 | compass.bias = [11.16, -43.55, -52.62] 29 | 30 | accel_data = mpu.get_accel() 31 | x_rotation = mpu.get_x_rotation(accel_data) 32 | y_rotation = mpu.get_y_rotation(accel_data) 33 | 34 | last_time = time.time() 35 | alpha = 0.85; 36 | 37 | while True: 38 | new_time = time.time() 39 | gyro_data = mpu.get_gyro() 40 | accel_data = mpu.get_accel() 41 | 42 | dt = new_time - last_time 43 | last_time = new_time 44 | gyro_angle_x = gyro_data['x']*dt + x_rotation 45 | if gyro_angle_x > 360: 46 | gyro_angle_x -= 360 47 | if gyro_angle_x < 0: 48 | gyro_angle_x = 360 + gyro_angle_x 49 | 50 | accel_angle_x = mpu.get_x_rotation(accel_data) 51 | 52 | if abs(gyro_angle_x - accel_angle_x) > 180: 53 | gyro_angle_x = accel_angle_x 54 | 55 | x_rotation = alpha*gyro_angle_x + (1.0 - alpha)*mpu.get_x_rotation(accel_data) 56 | 57 | gyro_angle_y = gyro_data['y']*dt + y_rotation 58 | y_rotation = alpha*gyro_angle_y + (1.0 - alpha)*mpu.get_y_rotation(accel_data) 59 | 60 | rotation = 360 - compass.heading(x_rotation, y_rotation) 61 | time.sleep(0.01) 62 | 63 | 64 | FPS = 25 65 | WINDOWWIDTH = 640 66 | WINDOWHEIGHT = 480 67 | CELLSIZE = 20 68 | CELLWIDTH = int(WINDOWWIDTH / CELLSIZE) 69 | CELLHEIGHT = int(WINDOWHEIGHT / CELLSIZE) 70 | 71 | # R G B 72 | WHITE = (255, 255, 255) 73 | BLACK = ( 0, 0, 0) 74 | RED = (255, 0, 0) 75 | GREEN = ( 0, 255, 0) 76 | DARKGREEN = ( 0, 155, 0) 77 | DARKGRAY = ( 40, 40, 40) 78 | BGCOLOR = BLACK 79 | 80 | 81 | def main(): 82 | global FPSCLOCK, DISPLAYSURF, BASICFONT 83 | 84 | pygame.init() 85 | FPSCLOCK = pygame.time.Clock() 86 | DISPLAYSURF = pygame.display.set_mode((WINDOWWIDTH, WINDOWHEIGHT)) 87 | BASICFONT = pygame.font.Font('freesansbold.ttf', 18) 88 | pygame.display.set_caption('HMC5883L & MPU6050') 89 | 90 | thread.start_new_thread(hmc5883l_read,()) 91 | while True: 92 | runGame() 93 | 94 | 95 | def runGame(): 96 | global rotation 97 | 98 | titleFont = pygame.font.Font('freesansbold.ttf', 50) 99 | titleSurf1 = titleFont.render('HMC5883L', True, WHITE) 100 | img = pygame.image.load('compas.png') 101 | imgx = 10 102 | imgy = 10 103 | 104 | while True: # main game loop 105 | for event in pygame.event.get(): # event handling loop 106 | if event.type == QUIT: 107 | terminate() 108 | elif event.type == KEYDOWN: 109 | if event.key == K_ESCAPE: 110 | terminate() 111 | 112 | 113 | DISPLAYSURF.fill(BGCOLOR) 114 | drawGrid() 115 | 116 | rotatedSurf1 = pygame.transform.rotate(img, rotation) 117 | rotatedRect1 = rotatedSurf1.get_rect() 118 | rotatedRect1.center = (WINDOWWIDTH/2, WINDOWHEIGHT/2) 119 | DISPLAYSURF.blit(rotatedSurf1, rotatedRect1) 120 | 121 | pygame.display.update() 122 | FPSCLOCK.tick(FPS) 123 | 124 | 125 | def terminate(): 126 | pygame.quit() 127 | sys.exit() 128 | 129 | 130 | def drawGrid(): 131 | for x in range(0, WINDOWWIDTH, CELLSIZE): # draw vertical lines 132 | pygame.draw.line(DISPLAYSURF, DARKGRAY, (x, 0), (x, WINDOWHEIGHT)) 133 | for y in range(0, WINDOWHEIGHT, CELLSIZE): # draw horizontal lines 134 | pygame.draw.line(DISPLAYSURF, DARKGRAY, (0, y), (WINDOWWIDTH, y)) 135 | 136 | 137 | if __name__ == '__main__': 138 | main() 139 | -------------------------------------------------------------------------------- /HMC5883L/pygame_hmc5883_mpu6050_kfs360.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | # -*- coding: utf-8 -*- 3 | 4 | import pygame, sys, thread, time 5 | from pygame.locals import * 6 | from hmc5883 import * 7 | from kfs360 import * 8 | sys.path.append('../MPU-6050') 9 | from mpu6050 import * 10 | 11 | rotation = 0 12 | 13 | def hmc5883l_read(): 14 | global rotation 15 | 16 | # MPU-6050 17 | mpu = MPU6050() 18 | mpu.initialize() 19 | 20 | # calibration data 21 | mpu.gyro_offs = {'x': -178, 'y': 259, 'z': -104} 22 | mpu.accel_offs = {'y': -354, 'x': 389, 'z': -1482} 23 | 24 | # HMC5883L 25 | compass = hmc5883l() 26 | # calibration data 27 | compass.calibration_matrix = [ [1.259098, 0.013830, 0.039295], 28 | [0.01380, 1.245928, -0.018922], 29 | [0.039295, -0.018922, 1.360489]] 30 | compass.bias = [11.16, -43.55, -52.62] 31 | 32 | accel_data = mpu.get_accel() 33 | x_rotation = mpu.get_x_rotation(accel_data) 34 | y_rotation = mpu.get_y_rotation(accel_data) 35 | 36 | last_time = time.time() 37 | alpha = 0.95; 38 | 39 | # Simple Kalman Filter for -180 +180 degrees values 40 | # Q=2; R=15; F=1; H=1 41 | # Q - noise 42 | # R - measuring erro 43 | # F - dynamics of the system 44 | # H - 1 45 | Filter = KFS360(4.0, 15.0, 1.0, 1.0); 46 | 47 | while True: 48 | new_time = time.time() 49 | gyro_data = mpu.get_gyro() 50 | accel_data = mpu.get_accel() 51 | 52 | dt = new_time - last_time 53 | last_time = new_time 54 | gyro_angle_x = gyro_data['x']*dt + x_rotation 55 | if gyro_angle_x > 360: 56 | gyro_angle_x -= 360 57 | if gyro_angle_x < 0: 58 | gyro_angle_x = 360 + gyro_angle_x 59 | 60 | accel_angle_x = mpu.get_x_rotation(accel_data) 61 | 62 | if abs(gyro_angle_x - accel_angle_x) > 180: 63 | gyro_angle_x = accel_angle_x 64 | 65 | x_rotation = alpha*gyro_angle_x + (1.0 - alpha)*mpu.get_x_rotation(accel_data) 66 | 67 | gyro_angle_y = gyro_data['y']*dt + y_rotation 68 | y_rotation = alpha*gyro_angle_y + (1.0 - alpha)*mpu.get_y_rotation(accel_data) 69 | 70 | rotation_new = compass.heading(x_rotation, y_rotation) 71 | Filter.correct(rotation_new) 72 | rotation = 360 - Filter.State 73 | 74 | time.sleep(0.1) 75 | 76 | 77 | 78 | FPS = 25 79 | WINDOWWIDTH = 640 80 | WINDOWHEIGHT = 480 81 | CELLSIZE = 20 82 | CELLWIDTH = int(WINDOWWIDTH / CELLSIZE) 83 | CELLHEIGHT = int(WINDOWHEIGHT / CELLSIZE) 84 | 85 | # R G B 86 | WHITE = (255, 255, 255) 87 | BLACK = ( 0, 0, 0) 88 | RED = (255, 0, 0) 89 | GREEN = ( 0, 255, 0) 90 | DARKGREEN = ( 0, 155, 0) 91 | DARKGRAY = ( 40, 40, 40) 92 | BGCOLOR = BLACK 93 | 94 | 95 | def main(): 96 | global FPSCLOCK, DISPLAYSURF, BASICFONT 97 | 98 | pygame.init() 99 | FPSCLOCK = pygame.time.Clock() 100 | DISPLAYSURF = pygame.display.set_mode((WINDOWWIDTH, WINDOWHEIGHT)) 101 | BASICFONT = pygame.font.Font('freesansbold.ttf', 18) 102 | pygame.display.set_caption('HMC5883L & MPU6050') 103 | 104 | thread.start_new_thread(hmc5883l_read,()) 105 | while True: 106 | runGame() 107 | 108 | 109 | def runGame(): 110 | global rotation 111 | 112 | titleFont = pygame.font.Font('freesansbold.ttf', 50) 113 | titleSurf1 = titleFont.render('HMC5883L', True, WHITE) 114 | img = pygame.image.load('compas.png') 115 | imgx = 10 116 | imgy = 10 117 | 118 | while True: # main game loop 119 | for event in pygame.event.get(): # event handling loop 120 | if event.type == QUIT: 121 | terminate() 122 | elif event.type == KEYDOWN: 123 | if event.key == K_ESCAPE: 124 | terminate() 125 | 126 | 127 | DISPLAYSURF.fill(BGCOLOR) 128 | drawGrid() 129 | 130 | rotatedSurf1 = pygame.transform.rotate(img, rotation) 131 | rotatedRect1 = rotatedSurf1.get_rect() 132 | rotatedRect1.center = (WINDOWWIDTH/2, WINDOWHEIGHT/2) 133 | DISPLAYSURF.blit(rotatedSurf1, rotatedRect1) 134 | 135 | pygame.display.update() 136 | FPSCLOCK.tick(FPS) 137 | 138 | 139 | def terminate(): 140 | pygame.quit() 141 | sys.exit() 142 | 143 | 144 | def drawGrid(): 145 | for x in range(0, WINDOWWIDTH, CELLSIZE): # draw vertical lines 146 | pygame.draw.line(DISPLAYSURF, DARKGRAY, (x, 0), (x, WINDOWHEIGHT)) 147 | for y in range(0, WINDOWHEIGHT, CELLSIZE): # draw horizontal lines 148 | pygame.draw.line(DISPLAYSURF, DARKGRAY, (0, y), (WINDOWWIDTH, y)) 149 | 150 | 151 | if __name__ == '__main__': 152 | main() 153 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | GNU GENERAL PUBLIC LICENSE 2 | Version 2, June 1991 3 | 4 | Copyright (C) 1989, 1991 Free Software Foundation, Inc., 5 | 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 6 | Everyone is permitted to copy and distribute verbatim copies 7 | of this license document, but changing it is not allowed. 8 | 9 | Preamble 10 | 11 | The licenses for most software are designed to take away your 12 | freedom to share and change it. By contrast, the GNU General Public 13 | License is intended to guarantee your freedom to share and change free 14 | software--to make sure the software is free for all its users. This 15 | General Public License applies to most of the Free Software 16 | Foundation's software and to any other program whose authors commit to 17 | using it. (Some other Free Software Foundation software is covered by 18 | the GNU Lesser General Public License instead.) You can apply it to 19 | your programs, too. 20 | 21 | When we speak of free software, we are referring to freedom, not 22 | price. Our General Public Licenses are designed to make sure that you 23 | have the freedom to distribute copies of free software (and charge for 24 | this service if you wish), that you receive source code or can get it 25 | if you want it, that you can change the software or use pieces of it 26 | in new free programs; and that you know you can do these things. 27 | 28 | To protect your rights, we need to make restrictions that forbid 29 | anyone to deny you these rights or to ask you to surrender the rights. 30 | These restrictions translate to certain responsibilities for you if you 31 | distribute copies of the software, or if you modify it. 32 | 33 | For example, if you distribute copies of such a program, whether 34 | gratis or for a fee, you must give the recipients all the rights that 35 | you have. You must make sure that they, too, receive or can get the 36 | source code. And you must show them these terms so they know their 37 | rights. 38 | 39 | We protect your rights with two steps: (1) copyright the software, and 40 | (2) offer you this license which gives you legal permission to copy, 41 | distribute and/or modify the software. 42 | 43 | Also, for each author's protection and ours, we want to make certain 44 | that everyone understands that there is no warranty for this free 45 | software. If the software is modified by someone else and passed on, we 46 | want its recipients to know that what they have is not the original, so 47 | that any problems introduced by others will not reflect on the original 48 | authors' reputations. 49 | 50 | Finally, any free program is threatened constantly by software 51 | patents. We wish to avoid the danger that redistributors of a free 52 | program will individually obtain patent licenses, in effect making the 53 | program proprietary. To prevent this, we have made it clear that any 54 | patent must be licensed for everyone's free use or not licensed at all. 55 | 56 | The precise terms and conditions for copying, distribution and 57 | modification follow. 58 | 59 | GNU GENERAL PUBLIC LICENSE 60 | TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION 61 | 62 | 0. This License applies to any program or other work which contains 63 | a notice placed by the copyright holder saying it may be distributed 64 | under the terms of this General Public License. The "Program", below, 65 | refers to any such program or work, and a "work based on the Program" 66 | means either the Program or any derivative work under copyright law: 67 | that is to say, a work containing the Program or a portion of it, 68 | either verbatim or with modifications and/or translated into another 69 | language. (Hereinafter, translation is included without limitation in 70 | the term "modification".) Each licensee is addressed as "you". 71 | 72 | Activities other than copying, distribution and modification are not 73 | covered by this License; they are outside its scope. The act of 74 | running the Program is not restricted, and the output from the Program 75 | is covered only if its contents constitute a work based on the 76 | Program (independent of having been made by running the Program). 77 | Whether that is true depends on what the Program does. 78 | 79 | 1. You may copy and distribute verbatim copies of the Program's 80 | source code as you receive it, in any medium, provided that you 81 | conspicuously and appropriately publish on each copy an appropriate 82 | copyright notice and disclaimer of warranty; keep intact all the 83 | notices that refer to this License and to the absence of any warranty; 84 | and give any other recipients of the Program a copy of this License 85 | along with the Program. 86 | 87 | You may charge a fee for the physical act of transferring a copy, and 88 | you may at your option offer warranty protection in exchange for a fee. 89 | 90 | 2. You may modify your copy or copies of the Program or any portion 91 | of it, thus forming a work based on the Program, and copy and 92 | distribute such modifications or work under the terms of Section 1 93 | above, provided that you also meet all of these conditions: 94 | 95 | a) You must cause the modified files to carry prominent notices 96 | stating that you changed the files and the date of any change. 97 | 98 | b) You must cause any work that you distribute or publish, that in 99 | whole or in part contains or is derived from the Program or any 100 | part thereof, to be licensed as a whole at no charge to all third 101 | parties under the terms of this License. 102 | 103 | c) If the modified program normally reads commands interactively 104 | when run, you must cause it, when started running for such 105 | interactive use in the most ordinary way, to print or display an 106 | announcement including an appropriate copyright notice and a 107 | notice that there is no warranty (or else, saying that you provide 108 | a warranty) and that users may redistribute the program under 109 | these conditions, and telling the user how to view a copy of this 110 | License. (Exception: if the Program itself is interactive but 111 | does not normally print such an announcement, your work based on 112 | the Program is not required to print an announcement.) 113 | 114 | These requirements apply to the modified work as a whole. If 115 | identifiable sections of that work are not derived from the Program, 116 | and can be reasonably considered independent and separate works in 117 | themselves, then this License, and its terms, do not apply to those 118 | sections when you distribute them as separate works. But when you 119 | distribute the same sections as part of a whole which is a work based 120 | on the Program, the distribution of the whole must be on the terms of 121 | this License, whose permissions for other licensees extend to the 122 | entire whole, and thus to each and every part regardless of who wrote it. 123 | 124 | Thus, it is not the intent of this section to claim rights or contest 125 | your rights to work written entirely by you; rather, the intent is to 126 | exercise the right to control the distribution of derivative or 127 | collective works based on the Program. 128 | 129 | In addition, mere aggregation of another work not based on the Program 130 | with the Program (or with a work based on the Program) on a volume of 131 | a storage or distribution medium does not bring the other work under 132 | the scope of this License. 133 | 134 | 3. You may copy and distribute the Program (or a work based on it, 135 | under Section 2) in object code or executable form under the terms of 136 | Sections 1 and 2 above provided that you also do one of the following: 137 | 138 | a) Accompany it with the complete corresponding machine-readable 139 | source code, which must be distributed under the terms of Sections 140 | 1 and 2 above on a medium customarily used for software interchange; or, 141 | 142 | b) Accompany it with a written offer, valid for at least three 143 | years, to give any third party, for a charge no more than your 144 | cost of physically performing source distribution, a complete 145 | machine-readable copy of the corresponding source code, to be 146 | distributed under the terms of Sections 1 and 2 above on a medium 147 | customarily used for software interchange; or, 148 | 149 | c) Accompany it with the information you received as to the offer 150 | to distribute corresponding source code. (This alternative is 151 | allowed only for noncommercial distribution and only if you 152 | received the program in object code or executable form with such 153 | an offer, in accord with Subsection b above.) 154 | 155 | The source code for a work means the preferred form of the work for 156 | making modifications to it. For an executable work, complete source 157 | code means all the source code for all modules it contains, plus any 158 | associated interface definition files, plus the scripts used to 159 | control compilation and installation of the executable. However, as a 160 | special exception, the source code distributed need not include 161 | anything that is normally distributed (in either source or binary 162 | form) with the major components (compiler, kernel, and so on) of the 163 | operating system on which the executable runs, unless that component 164 | itself accompanies the executable. 165 | 166 | If distribution of executable or object code is made by offering 167 | access to copy from a designated place, then offering equivalent 168 | access to copy the source code from the same place counts as 169 | distribution of the source code, even though third parties are not 170 | compelled to copy the source along with the object code. 171 | 172 | 4. You may not copy, modify, sublicense, or distribute the Program 173 | except as expressly provided under this License. Any attempt 174 | otherwise to copy, modify, sublicense or distribute the Program is 175 | void, and will automatically terminate your rights under this License. 176 | However, parties who have received copies, or rights, from you under 177 | this License will not have their licenses terminated so long as such 178 | parties remain in full compliance. 179 | 180 | 5. You are not required to accept this License, since you have not 181 | signed it. However, nothing else grants you permission to modify or 182 | distribute the Program or its derivative works. These actions are 183 | prohibited by law if you do not accept this License. Therefore, by 184 | modifying or distributing the Program (or any work based on the 185 | Program), you indicate your acceptance of this License to do so, and 186 | all its terms and conditions for copying, distributing or modifying 187 | the Program or works based on it. 188 | 189 | 6. Each time you redistribute the Program (or any work based on the 190 | Program), the recipient automatically receives a license from the 191 | original licensor to copy, distribute or modify the Program subject to 192 | these terms and conditions. You may not impose any further 193 | restrictions on the recipients' exercise of the rights granted herein. 194 | You are not responsible for enforcing compliance by third parties to 195 | this License. 196 | 197 | 7. If, as a consequence of a court judgment or allegation of patent 198 | infringement or for any other reason (not limited to patent issues), 199 | conditions are imposed on you (whether by court order, agreement or 200 | otherwise) that contradict the conditions of this License, they do not 201 | excuse you from the conditions of this License. If you cannot 202 | distribute so as to satisfy simultaneously your obligations under this 203 | License and any other pertinent obligations, then as a consequence you 204 | may not distribute the Program at all. For example, if a patent 205 | license would not permit royalty-free redistribution of the Program by 206 | all those who receive copies directly or indirectly through you, then 207 | the only way you could satisfy both it and this License would be to 208 | refrain entirely from distribution of the Program. 209 | 210 | If any portion of this section is held invalid or unenforceable under 211 | any particular circumstance, the balance of the section is intended to 212 | apply and the section as a whole is intended to apply in other 213 | circumstances. 214 | 215 | It is not the purpose of this section to induce you to infringe any 216 | patents or other property right claims or to contest validity of any 217 | such claims; this section has the sole purpose of protecting the 218 | integrity of the free software distribution system, which is 219 | implemented by public license practices. Many people have made 220 | generous contributions to the wide range of software distributed 221 | through that system in reliance on consistent application of that 222 | system; it is up to the author/donor to decide if he or she is willing 223 | to distribute software through any other system and a licensee cannot 224 | impose that choice. 225 | 226 | This section is intended to make thoroughly clear what is believed to 227 | be a consequence of the rest of this License. 228 | 229 | 8. If the distribution and/or use of the Program is restricted in 230 | certain countries either by patents or by copyrighted interfaces, the 231 | original copyright holder who places the Program under this License 232 | may add an explicit geographical distribution limitation excluding 233 | those countries, so that distribution is permitted only in or among 234 | countries not thus excluded. In such case, this License incorporates 235 | the limitation as if written in the body of this License. 236 | 237 | 9. The Free Software Foundation may publish revised and/or new versions 238 | of the General Public License from time to time. Such new versions will 239 | be similar in spirit to the present version, but may differ in detail to 240 | address new problems or concerns. 241 | 242 | Each version is given a distinguishing version number. If the Program 243 | specifies a version number of this License which applies to it and "any 244 | later version", you have the option of following the terms and conditions 245 | either of that version or of any later version published by the Free 246 | Software Foundation. If the Program does not specify a version number of 247 | this License, you may choose any version ever published by the Free Software 248 | Foundation. 249 | 250 | 10. If you wish to incorporate parts of the Program into other free 251 | programs whose distribution conditions are different, write to the author 252 | to ask for permission. For software which is copyrighted by the Free 253 | Software Foundation, write to the Free Software Foundation; we sometimes 254 | make exceptions for this. Our decision will be guided by the two goals 255 | of preserving the free status of all derivatives of our free software and 256 | of promoting the sharing and reuse of software generally. 257 | 258 | NO WARRANTY 259 | 260 | 11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY 261 | FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN 262 | OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES 263 | PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED 264 | OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 265 | MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS 266 | TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE 267 | PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING, 268 | REPAIR OR CORRECTION. 269 | 270 | 12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING 271 | WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR 272 | REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, 273 | INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING 274 | OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED 275 | TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY 276 | YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER 277 | PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE 278 | POSSIBILITY OF SUCH DAMAGES. 279 | 280 | END OF TERMS AND CONDITIONS 281 | 282 | How to Apply These Terms to Your New Programs 283 | 284 | If you develop a new program, and you want it to be of the greatest 285 | possible use to the public, the best way to achieve this is to make it 286 | free software which everyone can redistribute and change under these terms. 287 | 288 | To do so, attach the following notices to the program. It is safest 289 | to attach them to the start of each source file to most effectively 290 | convey the exclusion of warranty; and each file should have at least 291 | the "copyright" line and a pointer to where the full notice is found. 292 | 293 | {description} 294 | Copyright (C) {year} {fullname} 295 | 296 | This program is free software; you can redistribute it and/or modify 297 | it under the terms of the GNU General Public License as published by 298 | the Free Software Foundation; either version 2 of the License, or 299 | (at your option) any later version. 300 | 301 | This program is distributed in the hope that it will be useful, 302 | but WITHOUT ANY WARRANTY; without even the implied warranty of 303 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 304 | GNU General Public License for more details. 305 | 306 | You should have received a copy of the GNU General Public License along 307 | with this program; if not, write to the Free Software Foundation, Inc., 308 | 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. 309 | 310 | Also add information on how to contact you by electronic and paper mail. 311 | 312 | If the program is interactive, make it output a short notice like this 313 | when it starts in an interactive mode: 314 | 315 | Gnomovision version 69, Copyright (C) year name of author 316 | Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'. 317 | This is free software, and you are welcome to redistribute it 318 | under certain conditions; type `show c' for details. 319 | 320 | The hypothetical commands `show w' and `show c' should show the appropriate 321 | parts of the General Public License. Of course, the commands you use may 322 | be called something other than `show w' and `show c'; they could even be 323 | mouse-clicks or menu items--whatever suits your program. 324 | 325 | You should also get your employer (if you work as a programmer) or your 326 | school, if any, to sign a "copyright disclaimer" for the program, if 327 | necessary. Here is a sample; alter the names: 328 | 329 | Yoyodyne, Inc., hereby disclaims all copyright interest in the program 330 | `Gnomovision' (which makes passes at compilers) written by James Hacker. 331 | 332 | {signature of Ty Coon}, 1 April 1989 333 | Ty Coon, President of Vice 334 | 335 | This General Public License does not permit incorporating your program into 336 | proprietary programs. If your program is a subroutine library, you may 337 | consider it more useful to permit linking proprietary applications with the 338 | library. If this is what you want to do, use the GNU Lesser General 339 | Public License instead of this License. 340 | 341 | -------------------------------------------------------------------------------- /MPU-6050/kf.py: -------------------------------------------------------------------------------- 1 | class KalmanFilterSimple: 2 | X0 = 0.0 3 | P0 = 0.0 4 | Q = 0.0 5 | R = 0.0 6 | F = 0.0 7 | H = 0.0 8 | State = 0.0 9 | Covariance = 0.0 10 | 11 | def __init__(self, q, r, f, h): 12 | self.Q = q 13 | self.R = r 14 | self.F = f 15 | self.H = h 16 | 17 | def correct(self, data): 18 | self.X0 = self.F*self.State 19 | self.P0 = self.F*self.Covariance*self.F + self.Q 20 | 21 | #measurement update - correction 22 | K = self.H*self.P0/(self.H*self.P0*self.H + self.R); 23 | self.State = self.X0 + K*(data - self.H*self.X0); 24 | self.Covariance = (1 - K*self.H)*self.P0; 25 | 26 | -------------------------------------------------------------------------------- /MPU-6050/mpu6050.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | # Copyright (c) 2015 Andrey Koryagin 3 | # Permission is hereby granted, free of charge, to any person obtaining a copy 4 | # of this software and associated documentation files (the "Software"), to deal 5 | # in the Software without restriction, including without limitation the rights 6 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | # copies of the Software, and to permit persons to whom the Software is 8 | # furnished to do so, subject to the following conditions: 9 | # The above copyright notice and this permission notice shall be included in 10 | # all copies or substantial portions of the Software. 11 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 12 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 13 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 14 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 15 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 16 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 17 | # THE SOFTWARE. 18 | 19 | import smbus 20 | import math 21 | import time 22 | 23 | class MPU6050: 24 | MPU6050_ADDRESS_AD0_LOW = 0x68 # address pin low (GND), default for InvenSense evaluation board 25 | MPU6050_ADDRESS_AD0_HIGH = 0x69 # address pin high (VCC) 26 | MPU6050_DEFAULT_ADDRESS = MPU6050_ADDRESS_AD0_LOW 27 | 28 | MPU6050_RA_SELF_TEST_X = 0x0D 29 | MPU6050_RA_SELF_TEST_Y = 0x0E 30 | MPU6050_RA_SELF_TEST_Z = 0xF 31 | MPU6050_RA_SELF_TEST_A = 0x10 32 | MPU6050_RA_SMPLRT_DIV = 0x19 33 | MPU6050_RA_CONFIG = 0x1A 34 | MPU6050_RA_GYRO_CONFIG = 0x1B 35 | MPU6050_RA_ACCEL_CONFIG = 0x1C 36 | MPU6050_RA_FF_THR = 0x1D 37 | MPU6050_RA_FF_DUR = 0x1E 38 | MPU6050_RA_MOT_THR = 0x1F 39 | MPU6050_RA_FIFO_EN = 0x23 40 | MPU6050_RA_I2C_MST_CTRL = 0x24 41 | MPU6050_RA_I2C_SLV0_ADDR = 0x25 42 | MPU6050_RA_I2C_SLV0_REG = 0x26 43 | MPU6050_RA_I2C_SLV0_CTRL = 0x27 44 | MPU6050_RA_I2C_SLV1_ADDR = 0x28 45 | MPU6050_RA_I2C_SLV1_REG = 0x29 46 | MPU6050_RA_I2C_SLV1_CTRL = 0x2A 47 | MPU6050_RA_I2C_SLV2_ADDR = 0x2B 48 | MPU6050_RA_I2C_SLV2_REG = 0x2C 49 | MPU6050_RA_I2C_SLV2_CTRL = 0x2D 50 | MPU6050_RA_I2C_SLV3_ADDR = 0x2E 51 | MPU6050_RA_I2C_SLV3_REG = 0x2F 52 | MPU6050_RA_I2C_SLV3_CTRL = 0x30 53 | MPU6050_RA_I2C_SLV4_ADDR = 0x31 54 | MPU6050_RA_I2C_SLV4_REG = 0x32 55 | MPU6050_RA_I2C_SLV4_DO = 0x33 56 | MPU6050_RA_I2C_SLV4_CTRL = 0x34 57 | MPU6050_RA_I2C_SLV4_DI = 0x35 58 | MPU6050_RA_I2C_MST_STATUS = 0x36 59 | MPU6050_RA_INT_PIN_CFG = 0x37 60 | MPU6050_RA_INT_ENABLE = 0x38 61 | MPU6050_RA_DMP_INT_STATUS = 0x39 62 | MPU6050_RA_INT_STATUS = 0x3A 63 | MPU6050_RA_ACCEL_XOUT_H = 0x3B 64 | MPU6050_RA_ACCEL_XOUT_L = 0x3C 65 | MPU6050_RA_ACCEL_YOUT_H = 0x3D 66 | MPU6050_RA_ACCEL_YOUT_L = 0x3E 67 | MPU6050_RA_ACCEL_ZOUT_H = 0x3F 68 | MPU6050_RA_ACCEL_ZOUT_L = 0x40 69 | MPU6050_RA_TEMP_OUT_H = 0x41 70 | MPU6050_RA_TEMP_OUT_L = 0x42 71 | MPU6050_RA_GYRO_XOUT_H = 0x43 72 | MPU6050_RA_GYRO_XOUT_L = 0x44 73 | MPU6050_RA_GYRO_YOUT_H = 0x45 74 | MPU6050_RA_GYRO_YOUT_L = 0x46 75 | MPU6050_RA_GYRO_ZOUT_H = 0x47 76 | MPU6050_RA_GYRO_ZOUT_L = 0x48 77 | MPU6050_RA_EXT_SENS_DATA_00 = 0x49 78 | MPU6050_RA_EXT_SENS_DATA_01 = 0x4A 79 | MPU6050_RA_EXT_SENS_DATA_02 = 0x4B 80 | MPU6050_RA_EXT_SENS_DATA_03 = 0x4C 81 | MPU6050_RA_EXT_SENS_DATA_04 = 0x4D 82 | MPU6050_RA_EXT_SENS_DATA_05 = 0x4E 83 | MPU6050_RA_EXT_SENS_DATA_06 = 0x4F 84 | MPU6050_RA_EXT_SENS_DATA_07 = 0x50 85 | MPU6050_RA_EXT_SENS_DATA_08 = 0x51 86 | MPU6050_RA_EXT_SENS_DATA_09 = 0x52 87 | MPU6050_RA_EXT_SENS_DATA_10 = 0x53 88 | MPU6050_RA_EXT_SENS_DATA_11 = 0x54 89 | MPU6050_RA_EXT_SENS_DATA_12 = 0x55 90 | MPU6050_RA_EXT_SENS_DATA_13 = 0x56 91 | MPU6050_RA_EXT_SENS_DATA_14 = 0x57 92 | MPU6050_RA_EXT_SENS_DATA_15 = 0x58 93 | MPU6050_RA_EXT_SENS_DATA_16 = 0x59 94 | MPU6050_RA_EXT_SENS_DATA_17 = 0x5A 95 | MPU6050_RA_EXT_SENS_DATA_18 = 0x5B 96 | MPU6050_RA_EXT_SENS_DATA_19 = 0x5C 97 | MPU6050_RA_EXT_SENS_DATA_20 = 0x5D 98 | MPU6050_RA_EXT_SENS_DATA_21 = 0x5E 99 | MPU6050_RA_EXT_SENS_DATA_22 = 0x5F 100 | MPU6050_RA_EXT_SENS_DATA_23 = 0x60 101 | MPU6050_RA_MOT_DETECT_STATUS = 0x61 102 | MPU6050_RA_I2C_SLV0_DO = 0x63 103 | MPU6050_RA_I2C_SLV1_DO = 0x64 104 | MPU6050_RA_I2C_SLV2_DO = 0x65 105 | MPU6050_RA_I2C_SLV3_DO = 0x66 106 | MPU6050_RA_I2C_MST_DELAY_CTRL = 0x67 107 | MPU6050_RA_SIGNAL_PATH_RESET = 0x68 108 | MPU6050_RA_MOT_DETECT_CTRL = 0x69 109 | MPU6050_RA_USER_CTRL = 0x6A 110 | MPU6050_RA_PWR_MGMT_1 = 0x6B 111 | MPU6050_RA_PWR_MGMT_2 = 0x6C 112 | MPU6050_RA_BANK_SEL = 0x6D 113 | MPU6050_RA_MEM_START_ADDR = 0x6E 114 | MPU6050_RA_MEM_R_W = 0x6F 115 | MPU6050_RA_DMP_CFG_1 = 0x70 116 | MPU6050_RA_DMP_CFG_2 = 0x71 117 | MPU6050_RA_FIFO_COUNTH = 0x72 118 | MPU6050_RA_FIFO_COUNTL = 0x73 119 | MPU6050_RA_FIFO_R_W = 0x74 120 | MPU6050_RA_WHO_AM_I = 0x75 121 | 122 | gyro_offs = {'x': 0, 'y': 0, 'z': 0} 123 | accel_offs = {'y': 0, 'x': 0, 'z': 0} 124 | 125 | # construct a new object with the I2C address of the MPU6050 126 | def __init__(self, address = MPU6050_DEFAULT_ADDRESS): 127 | self.bus = smbus.SMBus(1) 128 | self.address = address 129 | 130 | def initialize(self): 131 | # Now wake the 6050 up as it starts in sleep mode 132 | # Sleep mode disable, Temperature sensor enable, CLK - internal 8Mhz 133 | self.write_byte(self.MPU6050_RA_PWR_MGMT_1, 0) 134 | ## Accel Range +-2g 135 | #self.write_byte(self.MPU6050_RA_ACCEL_CONFIG, 0) 136 | #self.accel_scale=16384.0 137 | ## Accel Range +-4g 138 | self.write_byte(self.MPU6050_RA_ACCEL_CONFIG, 8) 139 | self.accel_scale=8192.0 140 | ## Accel Range +-8g 141 | #self.write_byte(self.MPU6050_RA_ACCEL_CONFIG, 16) 142 | #self.accel_scale=4096.0 143 | ## Accel Range +-16g 144 | #self.write_byte(self.MPU6050_RA_ACCEL_CONFIG, 24) 145 | #self.accel_scale=2048.0 146 | 147 | # Gyro Range +-250 degrees/s 148 | self.write_byte(self.MPU6050_RA_GYRO_CONFIG, 0) 149 | self.gyro_scale=131 150 | ## Gyro Range +-500 degrees/s 151 | #self.write_byte(self.MPU6050_RA_GYRO_CONFIG, 8) 152 | #self.gro_scale=65.5 153 | ## Gyro Range +-1000 degrees/s 154 | #self.write_byte(self.MPU6050_RA_GYRO_CONFIG, 16) 155 | #self.gro_scale=32.8 156 | ## Gyro Range +-2000 degrees/s 157 | #self.write_byte(self.MPU6050_RA_GYRO_CONFIG, 24) 158 | #self.gro_scale=16.4 159 | 160 | #DLPF Bandwidth = 10 161 | self.write_byte(self.MPU6050_RA_CONFIG, 5) 162 | 163 | def get_gyro_raw(self): 164 | data = { 165 | 'x' : self.read_word_2c(self.MPU6050_RA_GYRO_XOUT_H), 166 | 'y' : self.read_word_2c(self.MPU6050_RA_GYRO_YOUT_H), 167 | 'z' : self.read_word_2c(self.MPU6050_RA_GYRO_ZOUT_H) } 168 | return data 169 | 170 | def get_gyro(self): 171 | data = self.get_gyro_raw(); 172 | data['x']=(data['x']-self.gyro_offs['x'])/self.gyro_scale 173 | data['y']=(data['y']-self.gyro_offs['y'])/self.gyro_scale 174 | data['z']=(data['z']-self.gyro_offs['z'])/self.gyro_scale 175 | return data 176 | 177 | def get_gyro_offs(self): 178 | data_offs = {'x':0, 'y':0, 'z':0} 179 | for num in range(0,100): 180 | data = self.get_gyro_raw(); 181 | data_offs['x']+=data['x']; 182 | data_offs['y']+=data['y']; 183 | data_offs['z']+=data['z']; 184 | data_offs['x']/=100; 185 | data_offs['y']/=100; 186 | data_offs['z']/=100; 187 | return data_offs 188 | 189 | def get_accel_raw(self): 190 | data = { 191 | 'x' : self.read_word_2c(self.MPU6050_RA_ACCEL_XOUT_H), 192 | 'y' : self.read_word_2c(self.MPU6050_RA_ACCEL_YOUT_H), 193 | 'z' : self.read_word_2c(self.MPU6050_RA_ACCEL_ZOUT_H) } 194 | return data 195 | 196 | def get_accel(self): 197 | data = self.get_accel_raw(); 198 | data['x']=(data['x']-self.accel_offs['x'])/self.accel_scale 199 | data['y']=(data['y']-self.accel_offs['y'])/self.accel_scale 200 | data['z']=(data['z']-self.accel_offs['z'])/self.accel_scale 201 | return data 202 | 203 | def get_accel_offs(self): 204 | data_offs_min = self.get_accel_raw(); 205 | data_offs_max = self.get_accel_raw(); 206 | for num in range(0,10000): 207 | data = self.get_accel_raw() 208 | data_gyro = self.get_gyro() 209 | if math.fabs(data_gyro['x']) < 2 and math.fabs(data_gyro['y']) < 2 and math.fabs(data_gyro['z']) < 2 : 210 | if data['x'] > data_offs_max['x']: 211 | data_offs_max['x'] = data['x'] 212 | 213 | if data['y'] > data_offs_max['y']: 214 | data_offs_max['y'] = data['y'] 215 | 216 | if data['z'] > data_offs_max['z']: 217 | data_offs_max['z'] = data['z'] 218 | 219 | if data['x'] < data_offs_min['x']: 220 | data_offs_min['x'] = data['x'] 221 | 222 | if data['y'] < data_offs_min['y']: 223 | data_offs_min['y'] = data['y'] 224 | 225 | if data['z'] < data_offs_min['z']: 226 | data_offs_min['z'] = data['z'] 227 | 228 | data = { 229 | 'x': data_offs_min['x']+(data_offs_max['x']-data_offs_min['x'])/2, 230 | 'y': data_offs_min['y']+(data_offs_max['y']-data_offs_min['y'])/2, 231 | 'z': data_offs_min['z']+(data_offs_max['z']-data_offs_min['z'])/2 } 232 | return data 233 | 234 | def callibration(self): 235 | print "###############################################" 236 | print "Please do not move the MPU-6050 some secconds..." 237 | print "###############################################" 238 | time.sleep(5) 239 | self.gyro_offs = self.get_gyro_offs() 240 | #print "gyro_offs = ", self.gyro_offs 241 | print "Please twirl the MPU-6050 around a minute" 242 | print "###############################################" 243 | self.accel_offs = self.get_accel_offs() 244 | print "Done. Please change variables" 245 | print "\"gyro_offs\" and \"accel_offs\"" 246 | print "to following values in mpu6050.py file:" 247 | print "###############################################" 248 | print "gyro_offs = ", self.gyro_offs 249 | print "accel_offs = ", self.accel_offs 250 | print "###############################################" 251 | 252 | def get_temp_raw(self): 253 | return self.read_word_2c(self.MPU6050_RA_TEMP_OUT_H) 254 | 255 | def get_temp(self): 256 | return self.get_temp_raw()/340+36.53 257 | 258 | def get_gravity(self): 259 | data = self.get_accel() 260 | return math.sqrt(data['x']*data['x']+data['y']*data['y']+data['z']*data['z']) 261 | 262 | def read_byte(self, adr): 263 | return self.bus.read_byte_data(self.address, adr) 264 | 265 | def read_word(self, adr): 266 | high = self.bus.read_byte_data(self.address, adr) 267 | low = self.bus.read_byte_data(self.address, adr+1) 268 | val = (high << 8) + low 269 | return val 270 | 271 | def read_word_2c(self, adr): 272 | val = self.read_word(adr) 273 | if (val >= 0x8000): 274 | return -((65535 - val) + 1) 275 | else: 276 | return val 277 | def write_byte(self, adr, byte): 278 | self.bus.write_byte_data(self.address, adr, byte) 279 | 280 | def dist(self, a,b): 281 | return math.sqrt((a*a)+(b*b)) 282 | 283 | def get_y_rotation(self, data): 284 | radians = math.atan2(data['x'], self.dist(data['y'],data['z'])) 285 | return -math.degrees(radians) 286 | 287 | def get_x_rotation(self, data): 288 | if data['z'] > 0: 289 | radians = math.atan2(data['y'], self.dist(data['x'],data['z'])) 290 | if radians < 0: 291 | radians = 2*math.pi + radians 292 | else: 293 | radians = math.pi - math.atan2(data['y'], self.dist(data['x'],data['z'])) 294 | return math.degrees(radians) 295 | 296 | -------------------------------------------------------------------------------- /MPU-6050/mpu6050_accel_kf.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | from mpu6050 import * 4 | from kf import * 5 | import time 6 | 7 | mpu = MPU6050() 8 | mpu.initialize() 9 | 10 | # Set calibration data 11 | mpu.gyro_offs = {'x': -178, 'y': 259, 'z': -104} 12 | mpu.accel_offs = {'y': -354, 'x': 389, 'z': -1482} 13 | 14 | # Simple Kalman Filter 15 | # Q=2; R=15; F=1; H=1 16 | # Q - noise 17 | # R - measuring erro 18 | # F - dynamics of the system 19 | # H - 1 20 | kf_x = KalmanFilterSimple(2.0, 1.0, 1.0, 1.0); 21 | kf_y = KalmanFilterSimple(2.0, 1.0, 1.0, 1.0); 22 | 23 | last_time = time.time() 24 | 25 | while True: 26 | new_time = time.time() 27 | accel_data = mpu.get_accel() 28 | 29 | x_rotation = mpu.get_x_rotation(accel_data) 30 | y_rotation = mpu.get_y_rotation(accel_data) 31 | 32 | kf_x.correct(x_rotation) 33 | kf_y.correct(y_rotation) 34 | 35 | print "x:", kf_x.State 36 | print "y:", kf_y.State 37 | 38 | time.sleep(0.1) 39 | -------------------------------------------------------------------------------- /MPU-6050/mpu6050_accel_no_filter.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | from mpu6050 import * 4 | import time 5 | 6 | mpu = MPU6050() 7 | mpu.initialize() 8 | 9 | # Set calibration data 10 | mpu.gyro_offs = {'x': -178, 'y': 259, 'z': -104} 11 | mpu.accel_offs = {'y': -354, 'x': 389, 'z': -1482} 12 | 13 | while True: 14 | accel_data = mpu.get_accel() 15 | x_rotation = mpu.get_x_rotation(accel_data) 16 | y_rotation = mpu.get_y_rotation(accel_data) 17 | 18 | print "x:", x_rotation 19 | print "y:", y_rotation 20 | 21 | time.sleep(0.1) 22 | -------------------------------------------------------------------------------- /MPU-6050/mpu6050_calibr.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | from mpu6050 import * 4 | from kf import * 5 | import time 6 | 7 | mpu = MPU6050() 8 | mpu.initialize() 9 | mpu.callibration() 10 | -------------------------------------------------------------------------------- /MPU-6050/mpu6050_final.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | from mpu6050 import * 4 | import time 5 | 6 | mpu = MPU6050() 7 | mpu.initialize() 8 | 9 | # Set calibration data 10 | mpu.gyro_offs = {'x': -178, 'y': 259, 'z': -104} 11 | mpu.accel_offs = {'y': -354, 'x': 389, 'z': -1482} 12 | 13 | accel_data = mpu.get_accel() 14 | x_rotation = mpu.get_x_rotation(accel_data) 15 | y_rotation = mpu.get_y_rotation(accel_data) 16 | 17 | last_time = time.time() 18 | alpha = 0.95; 19 | 20 | while True: 21 | new_time = time.time() 22 | gyro_data = mpu.get_gyro() 23 | accel_data = mpu.get_accel() 24 | 25 | dt = new_time - last_time 26 | last_time = new_time 27 | gyro_angle_x = gyro_data['x']*dt + x_rotation 28 | gyro_angle_y = gyro_data['y']*dt + y_rotation 29 | 30 | x_rotation = alpha*gyro_angle_x + (1.0 - alpha)*mpu.get_x_rotation(accel_data) 31 | y_rotation = alpha*gyro_angle_y + (1.0 - alpha)*mpu.get_y_rotation(accel_data) 32 | 33 | print "x:", x_rotation 34 | print "y:", y_rotation 35 | 36 | time.sleep(0.05) 37 | -------------------------------------------------------------------------------- /MPU-6050/mpu6050_get_raw.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | from mpu6050 import * 4 | import time 5 | 6 | 7 | mpu = MPU6050() 8 | mpu.initialize() 9 | 10 | # Set calibration data 11 | mpu.gyro_offs = {'x': -178, 'y': 259, 'z': -104} 12 | mpu.accel_offs = {'y': -354, 'x': 389, 'z': -1482} 13 | 14 | while True: 15 | gyro_data = mpu.get_gyro() 16 | accel_data = mpu.get_accel() 17 | 18 | print "Gyro: ", gyro_data 19 | print "Accel: ", accel_data 20 | print "Temperature: ",mpu.get_temp() 21 | 22 | time.sleep(0.1) 23 | -------------------------------------------------------------------------------- /MPU-6050/mpu6050_get_raw_no_calibr.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | from mpu6050 import * 4 | import time 5 | 6 | 7 | mpu = MPU6050() 8 | mpu.initialize() 9 | 10 | mpu.gyro_offs = {'x': 0, 'y': 0, 'z': 0} 11 | mpu.accel_offs = {'y': 0, 'x': 0, 'z': 0} 12 | 13 | 14 | while True: 15 | gyro_data = mpu.get_gyro() 16 | accel_data = mpu.get_accel() 17 | 18 | print "Gyro: ", gyro_data 19 | print "Accel: ", accel_data 20 | print "Temperature: ",mpu.get_temp() 21 | 22 | time.sleep(0.1) 23 | -------------------------------------------------------------------------------- /MPU-6050/mpu6050_gyro.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | # -*- coding: utf-8 -*- 3 | 4 | import time 5 | from mpu6050 import * 6 | 7 | mpu = MPU6050() 8 | mpu.initialize() 9 | 10 | # Set calibration data 11 | mpu.gyro_offs = {'x': -178, 'y': 259, 'z': -104} 12 | mpu.accel_offs = {'y': -354, 'x': 389, 'z': -1482} 13 | 14 | accel_data = mpu.get_accel() 15 | x_rotation = 0 16 | y_rotation = 0 17 | last_time = time.time() 18 | 19 | while True: 20 | new_time = time.time() 21 | gyro_data = mpu.get_gyro() 22 | 23 | dt = new_time - last_time 24 | last_time = new_time 25 | x_rotation = gyro_data['x']*dt + x_rotation 26 | 27 | if x_rotation > 360: 28 | x_rotation -= 360 29 | if x_rotation < 0: 30 | x_rotation = 360 + x_rotation 31 | 32 | y_rotation = gyro_data['y']*dt + y_rotation 33 | 34 | print "x:", x_rotation 35 | print "y:", y_rotation 36 | 37 | time.sleep(0.01) 38 | 39 | -------------------------------------------------------------------------------- /MPU-6050/pyplay_accel.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | # -*- coding: utf-8 -*- 3 | 4 | import random, pygame, sys, thread, time 5 | from pygame.locals import * 6 | from mpu6050 import * 7 | 8 | y_offset = 0 9 | rotation = 0 10 | 11 | def mpu6050_read(): 12 | global y_offset 13 | global rotation 14 | 15 | mpu = MPU6050() 16 | mpu.initialize() 17 | 18 | # Set calibration data 19 | mpu.gyro_offs = {'x': -178, 'y': 259, 'z': -104} 20 | mpu.accel_offs = {'y': -354, 'x': 389, 'z': -1482} 21 | 22 | accel_data = mpu.get_accel() 23 | x_rotation = mpu.get_x_rotation(accel_data) 24 | y_rotation = mpu.get_y_rotation(accel_data) 25 | 26 | while True: 27 | accel_data = mpu.get_accel() 28 | 29 | x_rotation = mpu.get_x_rotation(accel_data) 30 | 31 | y_rotation = mpu.get_y_rotation(accel_data) 32 | 33 | y_offset = y_rotation * 2 34 | rotation = x_rotation 35 | 36 | time.sleep(0.001) 37 | 38 | 39 | 40 | 41 | 42 | FPS = 100 43 | WINDOWWIDTH = 640 44 | WINDOWHEIGHT = 480 45 | CELLSIZE = 20 46 | CELLWIDTH = int(WINDOWWIDTH / CELLSIZE) 47 | CELLHEIGHT = int(WINDOWHEIGHT / CELLSIZE) 48 | 49 | # R G B 50 | WHITE = (255, 255, 255) 51 | BLACK = ( 0, 0, 0) 52 | RED = (255, 0, 0) 53 | GREEN = ( 0, 255, 0) 54 | DARKGREEN = ( 0, 155, 0) 55 | DARKGRAY = ( 40, 40, 40) 56 | BGCOLOR = BLACK 57 | 58 | 59 | def main(): 60 | global FPSCLOCK, DISPLAYSURF, BASICFONT 61 | 62 | pygame.init() 63 | FPSCLOCK = pygame.time.Clock() 64 | DISPLAYSURF = pygame.display.set_mode((WINDOWWIDTH, WINDOWHEIGHT)) 65 | BASICFONT = pygame.font.Font('freesansbold.ttf', 18) 66 | pygame.display.set_caption('MPU-6050') 67 | 68 | thread.start_new_thread(mpu6050_read,()) 69 | while True: 70 | runGame() 71 | 72 | 73 | def runGame(): 74 | global y_offset 75 | global rotation 76 | 77 | titleFont = pygame.font.Font('freesansbold.ttf', 50) 78 | titleSurf1 = titleFont.render('MPU-6050', True, WHITE) 79 | 80 | while True: # main game loop 81 | for event in pygame.event.get(): # event handling loop 82 | if event.type == QUIT: 83 | terminate() 84 | elif event.type == KEYDOWN: 85 | if event.key == K_ESCAPE: 86 | terminate() 87 | 88 | 89 | DISPLAYSURF.fill(BGCOLOR) 90 | drawGrid() 91 | 92 | rotatedSurf1 = pygame.transform.rotate(titleSurf1, rotation) 93 | rotatedRect1 = rotatedSurf1.get_rect() 94 | rotatedRect1.center = (WINDOWWIDTH/2, WINDOWHEIGHT/2 + y_offset) 95 | DISPLAYSURF.blit(rotatedSurf1, rotatedRect1) 96 | 97 | pygame.display.update() 98 | FPSCLOCK.tick(FPS) 99 | 100 | 101 | def terminate(): 102 | pygame.quit() 103 | sys.exit() 104 | 105 | 106 | def drawGrid(): 107 | for x in range(0, WINDOWWIDTH, CELLSIZE): # draw vertical lines 108 | pygame.draw.line(DISPLAYSURF, DARKGRAY, (x, 0), (x, WINDOWHEIGHT)) 109 | for y in range(0, WINDOWHEIGHT, CELLSIZE): # draw horizontal lines 110 | pygame.draw.line(DISPLAYSURF, DARKGRAY, (0, y), (WINDOWWIDTH, y)) 111 | 112 | 113 | if __name__ == '__main__': 114 | main() 115 | -------------------------------------------------------------------------------- /MPU-6050/pyplay_final.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | # -*- coding: utf-8 -*- 3 | 4 | import random, pygame, sys, thread, time 5 | from pygame.locals import * 6 | from mpu6050 import * 7 | 8 | y_offset = 0 9 | rotation = 0 10 | 11 | def mpu6050_read(): 12 | global y_offset 13 | global rotation 14 | 15 | mpu = MPU6050() 16 | mpu.initialize() 17 | 18 | # Set calibration data 19 | mpu.gyro_offs = {'x': -178, 'y': 259, 'z': -104} 20 | mpu.accel_offs = {'y': -354, 'x': 389, 'z': -1482} 21 | 22 | accel_data = mpu.get_accel() 23 | x_rotation = mpu.get_x_rotation(accel_data) 24 | y_rotation = mpu.get_y_rotation(accel_data) 25 | last_time = time.time() 26 | alpha = 0.98; 27 | 28 | while True: 29 | new_time = time.time() 30 | gyro_data = mpu.get_gyro() 31 | accel_data = mpu.get_accel() 32 | 33 | dt = new_time - last_time 34 | last_time = new_time 35 | gyro_angle_x = gyro_data['x']*dt + x_rotation 36 | 37 | if gyro_angle_x > 360: 38 | gyro_angle_x -= 360 39 | if gyro_angle_x < 0: 40 | gyro_angle_x = 360 + gyro_angle_x 41 | 42 | accel_angle_x = mpu.get_x_rotation(accel_data) 43 | 44 | if abs(gyro_angle_x - accel_angle_x) > 180: 45 | gyro_angle_x = accel_angle_x 46 | 47 | x_rotation = alpha*gyro_angle_x + (1.0 - alpha)*accel_angle_x; 48 | 49 | gyro_angle_y = gyro_data['y']*dt + y_rotation 50 | y_rotation = alpha*gyro_angle_y + (1.0 - alpha)*mpu.get_y_rotation(accel_data) 51 | 52 | y_offset = y_rotation * 2 53 | rotation = x_rotation 54 | 55 | time.sleep(0.001) 56 | 57 | 58 | 59 | 60 | 61 | FPS = 100 62 | WINDOWWIDTH = 640 63 | WINDOWHEIGHT = 480 64 | CELLSIZE = 20 65 | CELLWIDTH = int(WINDOWWIDTH / CELLSIZE) 66 | CELLHEIGHT = int(WINDOWHEIGHT / CELLSIZE) 67 | 68 | # R G B 69 | WHITE = (255, 255, 255) 70 | BLACK = ( 0, 0, 0) 71 | RED = (255, 0, 0) 72 | GREEN = ( 0, 255, 0) 73 | DARKGREEN = ( 0, 155, 0) 74 | DARKGRAY = ( 40, 40, 40) 75 | BGCOLOR = BLACK 76 | 77 | 78 | def main(): 79 | global FPSCLOCK, DISPLAYSURF, BASICFONT 80 | 81 | pygame.init() 82 | FPSCLOCK = pygame.time.Clock() 83 | DISPLAYSURF = pygame.display.set_mode((WINDOWWIDTH, WINDOWHEIGHT)) 84 | BASICFONT = pygame.font.Font('freesansbold.ttf', 18) 85 | pygame.display.set_caption('MPU-6050') 86 | 87 | thread.start_new_thread(mpu6050_read,()) 88 | while True: 89 | runGame() 90 | 91 | 92 | def runGame(): 93 | global y_offset 94 | global rotation 95 | 96 | titleFont = pygame.font.Font('freesansbold.ttf', 50) 97 | titleSurf1 = titleFont.render('MPU-6050', True, WHITE) 98 | 99 | while True: # main game loop 100 | for event in pygame.event.get(): # event handling loop 101 | if event.type == QUIT: 102 | terminate() 103 | elif event.type == KEYDOWN: 104 | if event.key == K_ESCAPE: 105 | terminate() 106 | 107 | 108 | DISPLAYSURF.fill(BGCOLOR) 109 | drawGrid() 110 | 111 | rotatedSurf1 = pygame.transform.rotate(titleSurf1, rotation) 112 | rotatedRect1 = rotatedSurf1.get_rect() 113 | rotatedRect1.center = (WINDOWWIDTH/2, WINDOWHEIGHT/2 + y_offset) 114 | DISPLAYSURF.blit(rotatedSurf1, rotatedRect1) 115 | 116 | pygame.display.update() 117 | FPSCLOCK.tick(FPS) 118 | 119 | 120 | def terminate(): 121 | pygame.quit() 122 | sys.exit() 123 | 124 | 125 | def drawGrid(): 126 | for x in range(0, WINDOWWIDTH, CELLSIZE): # draw vertical lines 127 | pygame.draw.line(DISPLAYSURF, DARKGRAY, (x, 0), (x, WINDOWHEIGHT)) 128 | for y in range(0, WINDOWHEIGHT, CELLSIZE): # draw horizontal lines 129 | pygame.draw.line(DISPLAYSURF, DARKGRAY, (0, y), (WINDOWWIDTH, y)) 130 | 131 | 132 | if __name__ == '__main__': 133 | main() 134 | -------------------------------------------------------------------------------- /MPU-6050/pyplay_gyro.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | # -*- coding: utf-8 -*- 3 | 4 | import random, pygame, sys, thread, time 5 | from pygame.locals import * 6 | from mpu6050 import * 7 | 8 | y_offset = 0 9 | rotation = 0 10 | 11 | def mpu6050_read(): 12 | global y_offset 13 | global rotation 14 | 15 | mpu = MPU6050() 16 | mpu.initialize() 17 | 18 | # Set calibration data 19 | mpu.gyro_offs = {'x': -178, 'y': 259, 'z': -104} 20 | mpu.accel_offs = {'y': -354, 'x': 389, 'z': -1482} 21 | 22 | accel_data = mpu.get_accel() 23 | x_rotation = 0 24 | y_rotation = 0 25 | last_time = time.time() 26 | 27 | while True: 28 | new_time = time.time() 29 | gyro_data = mpu.get_gyro() 30 | 31 | dt = new_time - last_time 32 | last_time = new_time 33 | x_rotation = gyro_data['x']*dt + x_rotation 34 | 35 | if x_rotation > 360: 36 | x_rotation -= 360 37 | if x_rotation < 0: 38 | x_rotation = 360 + x_rotation 39 | 40 | y_rotation = gyro_data['y']*dt + y_rotation 41 | 42 | y_offset = y_rotation * 2 43 | rotation = x_rotation 44 | 45 | time.sleep(0.01) 46 | 47 | 48 | 49 | 50 | 51 | FPS = 100 52 | WINDOWWIDTH = 640 53 | WINDOWHEIGHT = 480 54 | CELLSIZE = 20 55 | CELLWIDTH = int(WINDOWWIDTH / CELLSIZE) 56 | CELLHEIGHT = int(WINDOWHEIGHT / CELLSIZE) 57 | 58 | # R G B 59 | WHITE = (255, 255, 255) 60 | BLACK = ( 0, 0, 0) 61 | RED = (255, 0, 0) 62 | GREEN = ( 0, 255, 0) 63 | DARKGREEN = ( 0, 155, 0) 64 | DARKGRAY = ( 40, 40, 40) 65 | BGCOLOR = BLACK 66 | 67 | 68 | def main(): 69 | global FPSCLOCK, DISPLAYSURF, BASICFONT 70 | 71 | pygame.init() 72 | FPSCLOCK = pygame.time.Clock() 73 | DISPLAYSURF = pygame.display.set_mode((WINDOWWIDTH, WINDOWHEIGHT)) 74 | BASICFONT = pygame.font.Font('freesansbold.ttf', 18) 75 | pygame.display.set_caption('MPU-6050') 76 | 77 | thread.start_new_thread(mpu6050_read,()) 78 | while True: 79 | runGame() 80 | 81 | 82 | def runGame(): 83 | global y_offset 84 | global rotation 85 | 86 | titleFont = pygame.font.Font('freesansbold.ttf', 50) 87 | titleSurf1 = titleFont.render('MPU-6050', True, WHITE) 88 | 89 | while True: # main game loop 90 | for event in pygame.event.get(): # event handling loop 91 | if event.type == QUIT: 92 | terminate() 93 | elif event.type == KEYDOWN: 94 | if event.key == K_ESCAPE: 95 | terminate() 96 | 97 | 98 | DISPLAYSURF.fill(BGCOLOR) 99 | drawGrid() 100 | 101 | rotatedSurf1 = pygame.transform.rotate(titleSurf1, rotation) 102 | rotatedRect1 = rotatedSurf1.get_rect() 103 | rotatedRect1.center = (WINDOWWIDTH/2, WINDOWHEIGHT/2 + y_offset) 104 | DISPLAYSURF.blit(rotatedSurf1, rotatedRect1) 105 | 106 | pygame.display.update() 107 | FPSCLOCK.tick(FPS) 108 | 109 | 110 | def terminate(): 111 | pygame.quit() 112 | sys.exit() 113 | 114 | 115 | def drawGrid(): 116 | for x in range(0, WINDOWWIDTH, CELLSIZE): # draw vertical lines 117 | pygame.draw.line(DISPLAYSURF, DARKGRAY, (x, 0), (x, WINDOWHEIGHT)) 118 | for y in range(0, WINDOWHEIGHT, CELLSIZE): # draw horizontal lines 119 | pygame.draw.line(DISPLAYSURF, DARKGRAY, (0, y), (WINDOWWIDTH, y)) 120 | 121 | 122 | if __name__ == '__main__': 123 | main() 124 | -------------------------------------------------------------------------------- /MPU-9250/mpu9250.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | # Copyright (c) 2016 Andrey Koryagin 3 | # Permission is hereby granted, free of charge, to any person obtaining a copy 4 | # of this software and associated documentation files (the "Software"), to deal 5 | # in the Software without restriction, including without limitation the rights 6 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | # copies of the Software, and to permit persons to whom the Software is 8 | # furnished to do so, subject to the following conditions: 9 | # The above copyright notice and this permission notice shall be included in 10 | # all copies or substantial portions of the Software. 11 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 12 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 13 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 14 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 15 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 16 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 17 | # THE SOFTWARE. 18 | 19 | import smbus 20 | import math 21 | import time 22 | 23 | class MPU9250: 24 | #Register Map for Gyroscope and Accelerometer 25 | MPU9250_ADDRESS_AD0_LOW = 0x68 # address pin low (GND), default for InvenSense evaluation board 26 | MPU9250_ADDRESS_AD0_HIGH = 0x69 # address pin high (VCC) 27 | MPU9250_DEFAULT_ADDRESS = MPU9250_ADDRESS_AD0_LOW 28 | 29 | MPU9250_SELF_TEST_X_GYRO = 0x00 30 | MPU9250_SELF_TEST_Y_GYRO = 0x01 31 | MPU9250_SELF_TEST_Z_GYRO = 0x02 32 | 33 | MPU9250_SELF_TEST_X_ACCEL = 0x0D 34 | MPU9250_SELF_TEST_Y_ACCEL = 0x0E 35 | MPU9250_SELF_TEST_Z_ACCEL = 0x0F 36 | 37 | MPU9250_XG_OFFSET_H = 0x13 38 | MPU9250_XG_OFFSET_L = 0x14 39 | MPU9250_YG_OFFSET_H = 0x15 40 | MPU9250_YG_OFFSET_L = 0x16 41 | MPU9250_ZG_OFFSET_H = 0x17 42 | MPU9250_ZG_OFFSET_L = 0x18 43 | 44 | MPU9250_SMPLRT_DIV = 0x19 45 | MPU9250_CONFIG = 0x1A 46 | MPU9250_GYRO_CONFIG = 0x1B 47 | MPU9250_ACCEL_CONFIG = 0x1C 48 | MPU9250_ACCEL_CONFIG2 = 0x1D 49 | 50 | MPU9250_LP_ACCWL_ODR = 0x1E 51 | MPU9250_WOM_THR = 0x1F 52 | 53 | MPU9250_FIFO_EN = 0x23 54 | MPU9250_I2C_MST_CTRL = 0x24 55 | MPU9250_I2C_SLV0_ADDR = 0x25 56 | MPU9250_I2C_SLV0_REG = 0x26 57 | MPU9250_I2C_SLV0_CTRL = 0x27 58 | MPU9250_I2C_SLV1_ADDR = 0x28 59 | MPU9250_I2C_SLV1_REG = 0x29 60 | MPU9250_I2C_SLV1_CTRL = 0x2A 61 | MPU9250_I2C_SLV2_ADDR = 0x2B 62 | MPU9250_I2C_SLV2_REG = 0x2C 63 | MPU9250_I2C_SLV2_CTRL = 0x2D 64 | MPU9250_I2C_SLV3_ADDR = 0x2E 65 | MPU9250_I2C_SLV3_REG = 0x2F 66 | MPU9250_I2C_SLV3_CTRL = 0x30 67 | MPU9250_I2C_SLV4_ADDR = 0x31 68 | MPU9250_I2C_SLV4_REG = 0x32 69 | MPU9250_I2C_SLV4_DO = 0x33 70 | MPU9250_I2C_SLV4_CTRL = 0x34 71 | MPU9250_I2C_SLV4_DI = 0x35 72 | MPU9250_I2C_MST_STATUS = 0x36 73 | MPU9250_INT_PIN_CFG = 0x37 74 | MPU9250_INT_ENABLE = 0x38 75 | MPU9250_DMP_INT_STATUS = 0x39 76 | MPU9250_INT_STATUS = 0x3A 77 | 78 | MPU9250_ACCEL_XOUT_H = 0x3B 79 | MPU9250_ACCEL_XOUT_L = 0x3C 80 | MPU9250_ACCEL_YOUT_H = 0x3D 81 | MPU9250_ACCEL_YOUT_L = 0x3E 82 | MPU9250_ACCEL_ZOUT_H = 0x3F 83 | MPU9250_ACCEL_ZOUT_L = 0x40 84 | MPU9250_TEMP_OUT_H = 0x41 85 | MPU9250_TEMP_OUT_L = 0x42 86 | MPU9250_GYRO_XOUT_H = 0x43 87 | MPU9250_GYRO_XOUT_L = 0x44 88 | MPU9250_GYRO_YOUT_H = 0x45 89 | MPU9250_GYRO_YOUT_L = 0x46 90 | MPU9250_GYRO_ZOUT_H = 0x47 91 | MPU9250_GYRO_ZOUT_L = 0x48 92 | 93 | MPU9250_EXT_SENS_DATA_00 = 0x49 94 | MPU9250_EXT_SENS_DATA_01 = 0x4A 95 | MPU9250_EXT_SENS_DATA_02 = 0x4B 96 | MPU9250_EXT_SENS_DATA_03 = 0x4C 97 | MPU9250_EXT_SENS_DATA_04 = 0x4D 98 | MPU9250_EXT_SENS_DATA_05 = 0x4E 99 | MPU9250_EXT_SENS_DATA_06 = 0x4F 100 | MPU9250_EXT_SENS_DATA_07 = 0x50 101 | MPU9250_EXT_SENS_DATA_08 = 0x51 102 | MPU9250_EXT_SENS_DATA_09 = 0x52 103 | MPU9250_EXT_SENS_DATA_10 = 0x53 104 | MPU9250_EXT_SENS_DATA_11 = 0x54 105 | MPU9250_EXT_SENS_DATA_12 = 0x55 106 | MPU9250_EXT_SENS_DATA_13 = 0x56 107 | MPU9250_EXT_SENS_DATA_14 = 0x57 108 | MPU9250_EXT_SENS_DATA_15 = 0x58 109 | MPU9250_EXT_SENS_DATA_16 = 0x59 110 | MPU9250_EXT_SENS_DATA_17 = 0x5A 111 | MPU9250_EXT_SENS_DATA_18 = 0x5B 112 | MPU9250_EXT_SENS_DATA_19 = 0x5C 113 | MPU9250_EXT_SENS_DATA_20 = 0x5D 114 | MPU9250_EXT_SENS_DATA_21 = 0x5E 115 | MPU9250_EXT_SENS_DATA_22 = 0x5F 116 | MPU9250_EXT_SENS_DATA_23 = 0x60 117 | 118 | MPU9250_I2C_SLV0_DO = 0x63 119 | MPU9250_I2C_SLV1_DO = 0x64 120 | MPU9250_I2C_SLV2_DO = 0x65 121 | MPU9250_I2C_SLV3_DO = 0x66 122 | MPU9250_I2C_MST_DELAY_CTRL = 0x67 123 | MPU9250_SIGNAL_PATH_RESET = 0x68 124 | MPU9250_MOT_DETECT_CTRL = 0x69 125 | MPU9250_USER_CTRL = 0x6A 126 | MPU9250_PWR_MGMT_1 = 0x6B 127 | MPU9250_PWR_MGMT_2 = 0x6C 128 | MPU9250_FIFO_COUNTH = 0x72 129 | MPU9250_FIFO_COUNTL = 0x73 130 | MPU9250_FIFO_R_W = 0x74 131 | MPU9250_WHO_AM_I = 0x75 132 | 133 | MPU9250_XA_OFFSET_H = 0x77 134 | MPU9250_XA_OFFSET_L = 0x78 135 | MPU9250_YA_OFFSET_H = 0x7A 136 | MPU9250_YA_OFFSET_L = 0x7B 137 | MPU9250_ZA_OFFSET_H = 0x7D 138 | MPU9250_ZA_OFFSET_L = 0x7E 139 | 140 | gyro_offs = {'y': -5, 'x': 158, 'z': -100} 141 | accel_offs = {'y': 0, 'x': 0, 'z': 0} 142 | 143 | # construct a new object with the I2C address of the MPU9250 144 | def __init__(self, address = MPU9250_DEFAULT_ADDRESS): 145 | self.bus = smbus.SMBus(1) 146 | self.address = address 147 | 148 | def initialize(self): 149 | # Now wake the 9250 up as it starts in sleep mode 150 | # Sleep mode disable, Temperature sensor enable, CLK - internal 8Mhz 151 | self.write_byte(self.MPU9250_PWR_MGMT_1, 0) 152 | ## Accel Range +-2g 153 | #self.write_byte(self.MPU9250_ACCEL_CONFIG, 0) 154 | #self.accel_scale=16384.0 155 | ## Accel Range +-4g 156 | self.write_byte(self.MPU9250_ACCEL_CONFIG, 8) 157 | self.accel_scale=8192.0 158 | ## Accel Range +-8g 159 | #self.write_byte(self.MPU9250_ACCEL_CONFIG, 16) 160 | #self.accel_scale=4096.0 161 | ## Accel Range +-16g 162 | #self.write_byte(self.MPU9250_ACCEL_CONFIG, 24) 163 | #self.accel_scale=2048.0 164 | 165 | # Filter 166 | self.write_byte(self.MPU9250_ACCEL_CONFIG2, 8) 167 | 168 | # Gyro Range +-250 degrees/s 169 | self.write_byte(self.MPU9250_GYRO_CONFIG, 0) 170 | self.gyro_scale=131 171 | ## Gyro Range +-500 degrees/s 172 | #self.write_byte(self.MPU9250_GYRO_CONFIG, 8) 173 | #self.gro_scale=65.5 174 | ## Gyro Range +-1000 degrees/s 175 | #self.write_byte(self.MPU9250_GYRO_CONFIG, 16) 176 | #self.gro_scale=32.8 177 | ## Gyro Range +-2000 degrees/s 178 | #self.write_byte(self.MPU9250_GYRO_CONFIG, 24) 179 | #self.gro_scale=16.4 180 | 181 | #DLPF Bandwidth = 10 182 | self.write_byte(self.MPU9250_CONFIG, 5) 183 | 184 | # Enable Magnitometer (address 0x0c) 185 | self.write_byte(self.MPU9250_INT_PIN_CFG, 2) 186 | 187 | def get_gyro_raw(self): 188 | data = { 189 | 'x' : self.read_word_2c(self.MPU9250_GYRO_XOUT_H), 190 | 'y' : self.read_word_2c(self.MPU9250_GYRO_YOUT_H), 191 | 'z' : self.read_word_2c(self.MPU9250_GYRO_ZOUT_H) } 192 | return data 193 | 194 | def get_gyro(self): 195 | data = self.get_gyro_raw(); 196 | data['x']=(data['x']-self.gyro_offs['x'])/self.gyro_scale 197 | data['y']=(data['y']-self.gyro_offs['y'])/self.gyro_scale 198 | data['z']=(data['z']-self.gyro_offs['z'])/self.gyro_scale 199 | return data 200 | 201 | def get_gyro_offs(self): 202 | data_offs = {'x':0, 'y':0, 'z':0} 203 | for num in range(0,100): 204 | data = self.get_gyro_raw(); 205 | data_offs['x']+=data['x']; 206 | data_offs['y']+=data['y']; 207 | data_offs['z']+=data['z']; 208 | data_offs['x']/=100; 209 | data_offs['y']/=100; 210 | data_offs['z']/=100; 211 | return data_offs 212 | 213 | def get_accel_raw(self): 214 | data = { 215 | 'x' : self.read_word_2c(self.MPU9250_ACCEL_XOUT_H), 216 | 'y' : self.read_word_2c(self.MPU9250_ACCEL_YOUT_H), 217 | 'z' : self.read_word_2c(self.MPU9250_ACCEL_ZOUT_H) } 218 | return data 219 | 220 | def get_accel(self): 221 | data = self.get_accel_raw(); 222 | data['x']=(data['x']-self.accel_offs['x'])/self.accel_scale 223 | data['y']=(data['y']-self.accel_offs['y'])/self.accel_scale 224 | data['z']=(data['z']-self.accel_offs['z'])/self.accel_scale 225 | return data 226 | 227 | def get_accel_offs(self): 228 | data_offs_min = self.get_accel_raw(); 229 | data_offs_max = self.get_accel_raw(); 230 | for num in range(0,5000): 231 | data = self.get_accel_raw() 232 | data_gyro = self.get_gyro() 233 | if math.fabs(data_gyro['x']) < 2 and math.fabs(data_gyro['y']) < 2 and math.fabs(data_gyro['z']) < 2 : 234 | if data['x'] > data_offs_max['x']: 235 | data_offs_max['x'] = data['x'] 236 | 237 | if data['y'] > data_offs_max['y']: 238 | data_offs_max['y'] = data['y'] 239 | 240 | if data['z'] > data_offs_max['z']: 241 | data_offs_max['z'] = data['z'] 242 | 243 | if data['x'] < data_offs_min['x']: 244 | data_offs_min['x'] = data['x'] 245 | 246 | if data['y'] < data_offs_min['y']: 247 | data_offs_min['y'] = data['y'] 248 | 249 | if data['z'] < data_offs_min['z']: 250 | data_offs_min['z'] = data['z'] 251 | 252 | data = { 253 | 'x': data_offs_min['x']+(data_offs_max['x']-data_offs_min['x'])/2, 254 | 'y': data_offs_min['y']+(data_offs_max['y']-data_offs_min['y'])/2, 255 | 'z': data_offs_min['z']+(data_offs_max['z']-data_offs_min['z'])/2 } 256 | return data 257 | 258 | def callibration(self): 259 | print "###############################################" 260 | print "Please do not move the MPU-9250 some secconds..." 261 | print "###############################################" 262 | time.sleep(5) 263 | self.gyro_offs = self.get_gyro_offs() 264 | #print "gyro_offs = ", self.gyro_offs 265 | print "Please twirl the MPU-9250 around a minute" 266 | print "###############################################" 267 | self.accel_offs = self.get_accel_offs() 268 | print "Done. Please use following calibration values:" 269 | print "###############################################" 270 | print "gyro_offs = ", self.gyro_offs 271 | print "accel_offs = ", self.accel_offs 272 | print "###############################################" 273 | 274 | def get_temp_raw(self): 275 | return self.read_word_2c(self.MPU9250_TEMP_OUT_H) 276 | 277 | def get_temp(self): 278 | return self.get_temp_raw()/340+21.0 279 | 280 | def get_gravity(self): 281 | data = self.get_accel() 282 | return math.sqrt(data['x']*data['x']+data['y']*data['y']+data['z']*data['z']) 283 | 284 | def read_byte(self, adr): 285 | return self.bus.read_byte_data(self.address, adr) 286 | 287 | def read_word(self, adr): 288 | high = self.bus.read_byte_data(self.address, adr) 289 | low = self.bus.read_byte_data(self.address, adr+1) 290 | val = (high << 8) + low 291 | return val 292 | 293 | def read_word_2c(self, adr): 294 | val = self.read_word(adr) 295 | if (val >= 0x8000): 296 | return -((65535 - val) + 1) 297 | else: 298 | return val 299 | def write_byte(self, adr, byte): 300 | self.bus.write_byte_data(self.address, adr, byte) 301 | 302 | def dist(self, a,b): 303 | return math.sqrt((a*a)+(b*b)) 304 | 305 | def get_y_rotation(self, data): 306 | radians = math.atan2(data['x'], self.dist(data['y'],data['z'])) 307 | return -math.degrees(radians) 308 | 309 | def get_x_rotation(self, data): 310 | if data['z'] > 0: 311 | radians = math.atan2(data['y'], self.dist(data['x'],data['z'])) 312 | if radians < 0: 313 | radians = 2*math.pi + radians 314 | else: 315 | radians = math.pi - math.atan2(data['y'], self.dist(data['x'],data['z'])) 316 | return math.degrees(radians) 317 | 318 | class AK8963: 319 | #Register Map for Magnetometer 320 | AK8963_DEFAULT_ADDRESS = 0x0C 321 | 322 | AK8963_WIA = 0x00 323 | AK8963_INFO = 0x01 324 | AK8963_ST1 = 0x02 325 | AK8963_HXL = 0x03 326 | AK8963_HXH = 0x04 327 | AK8963_HYL = 0x05 328 | AK8963_HYH = 0x06 329 | AK8963_HZL = 0x07 330 | AK8963_HZH = 0x08 331 | AK8963_ST2 = 0x09 332 | AK8963_CNTL = 0x0A 333 | AK8963_RSV = 0x0B 334 | AK8963_ASTC = 0x0C 335 | AK8963_TS1 = 0x0D 336 | AK8963_TS2 = 0x0E 337 | AK8963_I2CDIS = 0x0F 338 | AK8963_ASAX = 0x10 339 | AK8963_ASAY = 0x11 340 | AK8963_ASAZ = 0x12 341 | 342 | #### Callibration data ### 343 | calibration_matrix = [ [1.560948, 0.001838, -0.011552], 344 | [0.001838, 1.521376, 0.047572], 345 | [-0.011552, 0.047572, 1.357251]] 346 | bias = [218.92, 115.072, -121.599] 347 | 348 | ################################# 349 | 350 | # construct a new object with the I2C address of the MPU9250 351 | def __init__(self, address = AK8963_DEFAULT_ADDRESS): 352 | self.bus = smbus.SMBus(1) 353 | self.address = address 354 | 355 | def read_byte(self, adr): 356 | return self.bus.read_byte_data(self.address, adr) 357 | 358 | def read_word(self, adr): 359 | low = self.read_byte(adr) 360 | high = self.read_byte(adr+1) 361 | val = (high << 8) + low 362 | if (val >= 0x8000): 363 | return -((65535 - val) + 1) 364 | else: 365 | return val 366 | 367 | def write_byte(self, adr, byte): 368 | self.bus.write_byte_data(self.address, adr, byte) 369 | 370 | def initialize(self): 371 | # Set Fuse ROM access mode 372 | self.write_byte(self.AK8963_CNTL, 0x0F) 373 | # Read Magnetic sensor axis sensitivity adjustment value 374 | self.ASAX = self.read_byte(self.AK8963_ASAX) 375 | self.ASAY = self.read_byte(self.AK8963_ASAY) 376 | self.ASAZ = self.read_byte(self.AK8963_ASAZ) 377 | # Set power-down mode before the transition to another mode. 378 | self.write_byte(self.AK8963_CNTL, 0x00) 379 | # Set Continuous measurement mode 0x12 - 8Hz & 16-bit; 0x16 - 100 Hz & 16-bit 380 | self.write_byte(self.AK8963_CNTL, 0x12) 381 | 382 | def get_raw(self): 383 | drdy = self.bus.read_byte_data(self.address, self.AK8963_ST1) & 0x01 384 | if drdy == 1: 385 | self.X = self.read_word(self.AK8963_HXL) 386 | self.X = self.read_word(self.AK8963_HXL)*((self.ASAX-128)/256+1) 387 | self.Y = self.read_word(self.AK8963_HYL)*((self.ASAY-128)/256+1) 388 | self.Z = self.read_word(self.AK8963_HZL)*((self.ASAZ-128)/256+1) 389 | hofl = self.bus.read_byte_data(self.address, self.AK8963_ST2) & 0x08 390 | return {'x':round(self.X), 'y':round(self.Y), 'z':round(self.Z)} 391 | 392 | def callibration(self): 393 | f = open('AK8963_calibr.txt', 'w') 394 | print "###############################################" 395 | print "Please twirl the AK8963 around a minute..." 396 | print "The experimental data will stored to AK8963_calibr.txt file" 397 | print "Please use this file to calculate calibration matrix by Magneto software." 398 | print "###############################################" 399 | for num in range(0,500): 400 | data = self.get_raw() 401 | f.write(str(data['x'])+'\t'+str(data['y'])+'\t'+str(data['z'])+'\n') 402 | time.sleep(0.125) 403 | f.close() 404 | 405 | def get_calibrated(self): 406 | data = self.get_raw() 407 | uncalibrated_values = [data['x'] - self.bias[0], data['y'] - self.bias[1], data['z'] - self.bias[2]] 408 | calibrated_values = [0,0,0] 409 | for i in range(0,3): 410 | for j in range(0,3): 411 | calibrated_values[i] += self.calibration_matrix[i][j] * uncalibrated_values[j]; 412 | return {'x':calibrated_values[0], 413 | 'y':calibrated_values[1], 414 | 'z':calibrated_values[2]} 415 | 416 | def heading(self, Ay=0, Ax=0): 417 | data = self.get_calibrated() 418 | 419 | Ax = Ax*math.pi/180 420 | Ay = Ay*math.pi/180 421 | 422 | Mx = [ [1.0, 0.0, 0.0], 423 | [0.0, math.cos(Ax), -math.sin(Ax)], 424 | [0.0, math.sin(Ax), math.cos(Ax)]] 425 | 426 | My = [ [math.cos(Ay), 0.0, math.sin(Ay)], 427 | [0.0, 1.0, 0.0], 428 | [-math.sin(Ay), 0.0, math.cos(Ay)]] 429 | 430 | # Vector rotation 431 | values = [data['x'],data['y'],data['z']] 432 | DATA = [0,0,0] 433 | for i in range(0,3): 434 | for j in range(0,3): 435 | DATA[i] += Mx[i][j] * values[j]; 436 | 437 | values = [DATA[0],DATA[1],DATA[2]] 438 | DATA = [0,0,0] 439 | for i in range(0,3): 440 | for j in range(0,3): 441 | DATA[i] += My[i][j] * values[j]; 442 | 443 | radians = math.atan2(DATA[0], DATA[1]) 444 | 445 | # Convert to degrees from radians 446 | return math.degrees(radians) 447 | -------------------------------------------------------------------------------- /MPU-9250/mpu9250_calibration.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | from mpu9250 import * 4 | 5 | mpu = MPU9250() 6 | mpu.initialize() 7 | 8 | mpu.callibration() 9 | 10 | compas = AK8963() 11 | compas.initialize() 12 | compas.callibration() 13 | 14 | -------------------------------------------------------------------------------- /MPU-9250/mpu9250_get.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | from mpu9250 import * 4 | import time 5 | 6 | mpu = MPU9250() 7 | mpu.initialize() 8 | 9 | compas = AK8963() 10 | compas.initialize() 11 | 12 | # Set calibration data 13 | mpu.gyro_offs = {'y': -5, 'x': 158, 'z': -100} 14 | mpu.accel_offs = {'y': 102, 'x': -34, 'z': -364} 15 | 16 | compas.calibration_matrix = [ [1.560948, 0.001838, -0.011552], 17 | [0.001838, 1.521376, 0.047572], 18 | [-0.011552, 0.047572, 1.357251]] 19 | compas.bias = [218.92, 115.072, -121.599] 20 | 21 | while True: 22 | gyro_data = mpu.get_gyro() 23 | accel_data = mpu.get_accel() 24 | compas_data = compas.get_calibrated() 25 | 26 | print "GYROSCOPE: ", gyro_data 27 | print "ACCELEROMETER: ", accel_data 28 | print "TEMPERATURE: ",mpu.get_temp() 29 | print "MAGNETOMETER: ", compas_data, "\n\n" 30 | 31 | time.sleep(1) 32 | -------------------------------------------------------------------------------- /MPU-9250/mpu9250_get_raw.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | from mpu9250 import * 4 | import time 5 | 6 | mpu = MPU9250() 7 | mpu.initialize() 8 | 9 | compas = AK8963() 10 | compas.initialize() 11 | 12 | while True: 13 | gyro_data = mpu.get_gyro_raw() 14 | accel_data = mpu.get_accel_raw() 15 | compad_data = compas.get_raw() 16 | 17 | print "GYROSCOPE: ", gyro_data 18 | print "ACCELEROMETER: ", accel_data 19 | print "TEMPERATURE: ",mpu.get_temp() 20 | print "MAGNETOMETER: ", compad_data, "\n\n" 21 | time.sleep(1) 22 | -------------------------------------------------------------------------------- /PCA9685/pca9685.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | # Copyright (c) 2016 Andrey Koryagin 3 | # Permission is hereby granted, free of charge, to any person obtaining a copy 4 | # of this software and associated documentation files (the "Software"), to deal 5 | # in the Software without restriction, including without limitation the rights 6 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | # copies of the Software, and to permit persons to whom the Software is 8 | # furnished to do so, subject to the following conditions: 9 | # The above copyright notice and this permission notice shall be included in 10 | # all copies or substantial portions of the Software. 11 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 12 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 13 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 14 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 15 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 16 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 17 | # THE SOFTWARE. 18 | 19 | import smbus, math, time 20 | 21 | SERVO_MIN = 204 # 1us ; 4096 - 20 us ; 50 Hz 22 | SERVO_CENTER = 307 23 | SERVO_MAX = 409 # 2us 24 | SERVO_ZONE = SERVO_MAX - SERVO_MIN 25 | 26 | class PCA9685: 27 | PCA9685_MODE1 = 0x00 28 | PCA9685_MODE2 = 0x01 29 | PCA9685_SUBADR1 = 0x02 30 | PCA9685_SUBADR2 = 0x03 31 | PCA9685_SUBADR3 = 0x04 32 | PCA9685_PRESCALE = 0xFE 33 | PCA9685_LED0_ON_L = 0x06 34 | PCA9685_LED0_ON_H = 0x07 35 | PCA9685_LED0_OFF_L = 0x08 36 | PCA9685_LED0_OFF_H = 0x09 37 | PCA9685_ALL_LED_ON_L = 0xFA 38 | PCA9685_ALL_LED_ON_H = 0xFB 39 | PCA9685_ALL_LED_OFF_L = 0xFC 40 | PCA9685_ALL_LED_OFF_H = 0xFD 41 | 42 | # Bits 43 | PCA9685_RESTART = 0x80 44 | PCA9685_SLEEP = 0x10 45 | PCA9685_ALLCALL = 0x01 46 | PCA9685_INVRT = 0x10 47 | PCA9685_OUTDRV = 0x04 48 | 49 | def __init__(self, address=0x40, freq=50): 50 | self.bus = smbus.SMBus(1) 51 | self.address = address 52 | prescale = int(math.floor(25000000.0/4096/freq -0.5)) 53 | 54 | self.write_byte(self.PCA9685_MODE2, self.PCA9685_OUTDRV) 55 | self.write_byte(self.PCA9685_MODE1, self.PCA9685_SLEEP) 56 | time.sleep(0.005) 57 | self.write_byte(self.PCA9685_PRESCALE, prescale) 58 | self.write_byte(self.PCA9685_MODE1, self.PCA9685_ALLCALL | self.PCA9685_RESTART) 59 | time.sleep(0.005) 60 | 61 | self.servos = [] 62 | for idx in range(16): 63 | servo = Servo() 64 | self.servos.append(servo) 65 | 66 | def setPWM(self, channel, on, off): 67 | self.write_byte(self.PCA9685_LED0_ON_L+4*channel, on & 0xFF) 68 | self.write_byte(self.PCA9685_LED0_ON_H+4*channel, on >> 8) 69 | self.write_byte(self.PCA9685_LED0_OFF_L+4*channel, off & 0xFF) 70 | self.write_byte(self.PCA9685_LED0_OFF_H+4*channel, off >> 8) 71 | 72 | def setServo(self, channel, dutycycle): 73 | # Reverse 74 | if self.servos[channel].reverse == True: 75 | if self.servos[channel].signed == True: 76 | dutycycle = -dutycycle 77 | else: 78 | dutycycle = 100.0-dutycycle 79 | 80 | # Exp 81 | dutycycle = dutycycle/100.0 82 | exp_val = dutycycle - (self.servos[channel].exp/100.0)*(dutycycle-dutycycle**3) 83 | 84 | if self.servos[channel].signed == True: 85 | value = SERVO_CENTER + self.servos[channel].servo_trim + (self.servos[channel].servo_max-self.servos[channel].servo_min)*exp_val/2.0 86 | else: 87 | value = self.servos[channel].servo_min + self.servos[channel].servo_trim + (self.servos[channel].servo_max-self.servos[channel].servo_min)*exp_val 88 | 89 | # Check Value limits 90 | if value < self.servos[channel].servo_min: 91 | value = self.servos[channel].servo_min 92 | if value > self.servos[channel].servo_max: 93 | value = self.servos[channel].servo_max 94 | 95 | value = int(value) 96 | 97 | self.setPWM(channel, 0, value) 98 | 99 | def off(self): 100 | self.write_byte(self.PCA9685_ALL_LED_ON_L, 0) 101 | self.write_byte(self.PCA9685_ALL_LED_ON_H, 0) 102 | self.write_byte(self.PCA9685_ALL_LED_OFF_L, 0) 103 | self.write_byte(self.PCA9685_ALL_LED_OFF_H, 0) 104 | 105 | def read_byte(self, adr): 106 | return self.bus.read_byte_data(self.address, adr) 107 | 108 | def write_byte(self, adr, byte): 109 | self.bus.write_byte_data(self.address, adr, byte) 110 | 111 | class Servo: 112 | signed = False #signed=True -100%...+100% signed=False 0...100% 113 | reverse = False # reverse 114 | min = 100.0 # 80%..120% 115 | max = 100.0 # 80%..120% 116 | trim = 0.0 # -50..50 117 | exp = 0.0 # 0..100% 118 | 119 | servo_min = SERVO_MIN 120 | servo_max = SERVO_MAX 121 | servo_trim = 0 122 | 123 | def __init__(self): 124 | return 125 | def set(self, signed, reverse, min, max, trim, exp): 126 | self.signed = signed 127 | self.reverse = reverse 128 | self.max = max 129 | self.min = min 130 | self.trim = trim 131 | self.exp = exp 132 | # Servo Limits % 120%...80% Default 100% 133 | self.servo_min = SERVO_MIN + SERVO_ZONE*(100.0-self.min)/100.0 134 | self.servo_max = SERVO_MAX + SERVO_ZONE*(self.max-100.0)/100.0 135 | # Servo Trim 136 | self.servo_trim = (self.servo_max-self.servo_min)*self.trim/100.0 137 | 138 | return 139 | -------------------------------------------------------------------------------- /PCA9685/pca9685_PWM_LED.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | 4 | import smbus, math, time 5 | 6 | class PCA9685: 7 | PCA9685_MODE1 = 0x00 8 | PCA9685_MODE2 = 0x01 9 | PCA9685_SUBADR1 = 0x02 10 | PCA9685_SUBADR2 = 0x03 11 | PCA9685_SUBADR3 = 0x04 12 | PCA9685_PRESCALE = 0xFE 13 | PCA9685_LED0_ON_L = 0x06 14 | PCA9685_LED0_ON_H = 0x07 15 | PCA9685_LED0_OFF_L = 0x08 16 | PCA9685_LED0_OFF_H = 0x09 17 | PCA9685_ALL_LED_ON_L = 0xFA 18 | PCA9685_ALL_LED_ON_H = 0xFB 19 | PCA9685_ALL_LED_OFF_L = 0xFC 20 | PCA9685_ALL_LED_OFF_H = 0xFD 21 | 22 | # Bits 23 | PCA9685_RESTART = 0x80 24 | PCA9685_SLEEP = 0x10 25 | PCA9685_ALLCALL = 0x01 26 | PCA9685_INVRT = 0x10 27 | PCA9685_OUTDRV = 0x04 28 | 29 | def __init__(self, address=0x40, freq=50): 30 | self.bus = smbus.SMBus(1) 31 | self.address = address 32 | prescale = int(math.floor(25000000.0/4096/freq -0.5)) 33 | 34 | self.write_byte(self.PCA9685_MODE2, self.PCA9685_OUTDRV) 35 | self.write_byte(self.PCA9685_MODE1, self.PCA9685_SLEEP) 36 | time.sleep(0.005) 37 | self.write_byte(self.PCA9685_PRESCALE, prescale) 38 | self.write_byte(self.PCA9685_MODE1, self.PCA9685_ALLCALL | self.PCA9685_RESTART) 39 | time.sleep(0.005) 40 | 41 | def setPWM(self, channel, on, off): 42 | self.write_byte(self.PCA9685_LED0_ON_L+4*channel, on & 0xFF) 43 | self.write_byte(self.PCA9685_LED0_ON_H+4*channel, on >> 8) 44 | self.write_byte(self.PCA9685_LED0_OFF_L+4*channel, off & 0xFF) 45 | self.write_byte(self.PCA9685_LED0_OFF_H+4*channel, off >> 8) 46 | 47 | def off(self): 48 | self.write_byte(self.PCA9685_ALL_LED_ON_L, 0) 49 | self.write_byte(self.PCA9685_ALL_LED_ON_H, 0) 50 | self.write_byte(self.PCA9685_ALL_LED_OFF_L, 0) 51 | self.write_byte(self.PCA9685_ALL_LED_OFF_H, 0) 52 | 53 | def read_byte(self, adr): 54 | return self.bus.read_byte_data(self.address, adr) 55 | 56 | def write_byte(self, adr, byte): 57 | self.bus.write_byte_data(self.address, adr, byte) 58 | 59 | 60 | if __name__ == "__main__": 61 | LED = PCA9685(0x40, 1000) 62 | LED.setPWM(1,0,0) # channel 1 63 | LED.setPWM(2,0,0) # channel 2 64 | LED.setPWM(3,0,0) # channel 3 65 | for channel in range(1, 4): 66 | for pwm_led in range (0, 4096, 20): 67 | LED.setPWM(channel,0,pwm_led) 68 | 69 | for pwm_led in range (4095, 0, -20): 70 | LED.setPWM(channel,0,pwm_led) 71 | 72 | LED.setPWM(channel,0,0) 73 | LED.off() 74 | -------------------------------------------------------------------------------- /PCA9685/pca9685_servo.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | 4 | from pca9685 import * 5 | 6 | servo = PCA9685() 7 | test_channel = 0 8 | 9 | 10 | print "Servo tester..." 11 | print "Cnannel", test_channel 12 | print "-------------------" 13 | print "Unsigned servo value 0..100%" 14 | print "-------------------" 15 | servo.servos[test_channel].set(signed=False, reverse=False, min=100, max=100, trim=0, exp=0) 16 | print "Servo to start position (0%)" 17 | servo.setServo(test_channel, 0) 18 | time.sleep(1) 19 | print "Servo to end position (100%)" 20 | servo.setServo(test_channel, 100) 21 | time.sleep(1) 22 | print "Servo to center (50%)" 23 | servo.setServo(test_channel, 50) 24 | time.sleep(1) 25 | print "Servo to start position (0%)" 26 | servo.setServo(test_channel, 0) 27 | time.sleep(3) 28 | 29 | print "\n" 30 | 31 | print "-------------------" 32 | print "Signed servo value -100%..100% with Reverse and expand limits to 120%" 33 | print "-------------------" 34 | 35 | servo.servos[test_channel].set(signed=True, reverse=True, min=120, max=120, trim=0, exp=0) 36 | print "Servo to Zero (center) position (0%)" 37 | servo.setServo(test_channel, 0) 38 | time.sleep(1) 39 | print "Servo to +100%" 40 | servo.setServo(test_channel, 100) 41 | time.sleep(1) 42 | print "Servo to -100%" 43 | servo.setServo(test_channel, -100) 44 | time.sleep(3) 45 | 46 | print "\n" 47 | 48 | print "-------------------" 49 | print "Unsigned servo value 0%..100% with curve = 100%" 50 | print "-------------------" 51 | servo.servos[test_channel].set(signed=False, reverse=False, min=100, max=100, trim=0, exp=100) 52 | servo.setServo(test_channel, 0) 53 | time.sleep(1) 54 | 55 | for value in range(100): 56 | servo.setServo(test_channel, value) 57 | time.sleep(0.01) 58 | 59 | for value in range(100): 60 | servo.setServo(test_channel, 100-value) 61 | time.sleep(0.01) 62 | 63 | servo.off() 64 | print "Done." 65 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Sensortest 2 | Python Class Library for Raspberry Pi 3 | 4 | Supported device list: MPU-6050, MPU-9250, HMC5883L, BMP280, BME280, HC-SR04, PCA9685 5 | 6 | Requirements: 7 | Hardware: Raspberry Pi 8 | Software: Raspbian, python 9 | 10 | More information: 11 | http://avislab.com/blog 12 | 13 | http://www.youtube.com/channel/UCRu6XayQfW-augguSYhStyg 14 | --------------------------------------------------------------------------------