#!/usr/bin/python # # FishPi - An autonomous drop in the ocean # # Simple test of compass sensor # import raspberrypi from compass_CMPS10 import Cmps10_Sensor if __name__ == "__main__": print "Testing compass sensor..." compassSensor = Cmps10_Sensor(i2c_bus=raspberrypi.i2c_bus(), debug=True) # heading (heading, pitch, roll) = compassSensor.read_sensor() print "Heading %f, pitch %f, roll %f" % (heading, pitch, roll) # raw values (m_x, m_y, m_z, a_x, a_y, a_z) = compassSensor.read_sensor_raw() print "Raw values: M(x,y,z)=(%f,%f,%f) A(x,y,z)=(%f,%f,%f)" % (m_x, m_y, m_z, a_x, a_y, a_z)
# # FishPi - An autonomous drop in the ocean # # Calibrate ESC through PWM controller # import raspberrypi from time import sleep from drive_controller import DriveController from drive_controller import AdafruitDriveController if __name__ == "__main__": print "Calibrating ESC" drive = AdafruitDriveController(debug=True, i2c_bus=raspberrypi.i2c_bus()) raw_input("Power on ESC and enter calibration mode... Then press <ENTER>...") print "run full ahead for 5 sec..." drive.set_throttle(1.0) sleep(5) print "returning to neutral for 5 sec" drive.set_throttle(0.0) sleep(5) print "run full reverse for 5 sec" drive.set_throttle(-1.0) sleep(5)
def lookup(self, addr, debug=False): """ lookup available device drivers by hex address, import and create driver class, setup particular devices so easily retrieved by consumers. """ if(debug): logging.debug("CFG:\tChecking for driver for device at i2c %s" % addr) # TODO replace with reading from config? probably use ConfigParser? # note: i2c addresses can conflict # could scan registers etc to confirm count etc? import raspberrypi if addr == 0x68: return "RTC", "Driver not loaded - DS1307" elif addr == 0x20: try: from sensor.GPS_I2C import GPS_NavigatronSensor self.gps_sensor = GPS_NavigatronSensor(i2c_bus=raspberrypi.i2c_bus(), debug=debug) except Exception as ex: logging.warning("CFG:\tError setting up GPS over i2c - %s" % ex) return "GPS", self.gps_sensor elif addr == 0x48: try: from sensor.temperature_TMP102 import TemperatureSensor self.temperature_sensor = TemperatureSensor(i2c_bus=raspberrypi.i2c_bus(), debug=debug) except Exception as ex: logging.warning("CFG:\tError setting up TEMPERATURE over i2c - %s" % ex) return "TEMPERATURE", self.temperature_sensor elif addr == 0x60: try: from sensor.compass_CMPS10 import Cmps10_Sensor self.compass_sensor = Cmps10_Sensor(i2c_bus=raspberrypi.i2c_bus(), debug=debug) except Exception as ex: logging.warning("CFG:\tError setting up COMPASS over i2c - %s" % ex) return "COMPASS", self.compass_sensor elif addr == 0x40: #or addr == 0x70: # DriveController (using Adafruit PWM board) (not sure what 0x70 address is for...) try: from vehicle.drive_controller import AdafruitDriveController # TODO pwm addresses from config? self.drive_controller = AdafruitDriveController(i2c_addr=addr, i2c_bus=raspberrypi.i2c_bus(), debug=debug) except Exception as ex: logging.info("CFG:\tError setting up DRIVECONTROLLER over i2c - %s" % ex) self.drive_controller = DummyDriveController() return "DRIVECONTROLLER", self.drive_controller elif addr == 0x32: # DriveController (using RaspyJuice) try: from vehicle.drive_controller import PyJuiceDriveController # TODO pwm addresses from config? self.drive_controller = PyJuiceDriveController(i2c_addr=addr, i2c_bus=raspberrypi.i2c_bus(), debug=debug) except Exception as ex: logging.info("CFG:\tError setting up DRIVECONTROLLER over i2c - %s" % ex) self.drive_controller = DummyDriveController() return "DRIVECONTROLLER", self.drive_controller elif addr == 0x1E: return "COMPASS", "Driver not loaded - HMC5883L" elif addr == 0x53 or addr == 0x1D: # 0x53 when ALT connected to HIGH # 0x1D when ALT connected to LOW return "ACCELEROMETER", "Driver not loaded - ADXL345" elif addr == 0x69: # 0x68 when AD0 connected to LOW - conflicts with DS1307! # 0x69 when AD0 connected to HIGH return "GYRO", "Driver not loaded - ITG3200" else: return "unknown", None
test_values(0x640, 100) test_values(0x500, 80) test_values(0x4B0, 75) test_values(0x320, 50) test_values(0x190, 25) test_values(0x004, 0.25) test_values(0x000, 0) test_values(0xFFC, -0.25) test_values(0xE70, -25) test_values(0xC90, -55) def test_values(value_in, value_expected): value_out = TemperatureSensor()._convert_12b(value_in) if value_out == value_expected: print "MATCH: got %f, expected %f for %s" % (value_out, value_expected, bin(value_in)) else: print "FAIL: got %f, expected %f for %s" % (value_out, value_expected, bin(value_in)) if __name__ == "__main__": #print "Testing value conversions..." #test_conversions() print "Testing temperature sensor..." tmpSensor = TemperatureSensor(debug=True, interface=raspberrypi.i2c_bus()) value = tmpSensor.read_sensor() print "Current temperature: %f" % value
def lookup(self, addr, debug=False): """ lookup available device drivers by hex address, import and create driver class, setup particular devices so easily retrieved by consumers. """ if (debug): logging.debug("CFG:\tChecking for driver for device at i2c %s" % addr) # TODO replace with reading from config? probably use ConfigParser? # note: i2c addresses can conflict # could scan registers etc to confirm count etc? import raspberrypi if addr == 0x68: return "RTC", "Driver not loaded - DS1307" elif addr == 0x20: try: from sensor.GPS_I2C import GPS_NavigatronSensor self.gps_sensor = GPS_NavigatronSensor( i2c_bus=raspberrypi.i2c_bus(), debug=debug) except Exception as ex: logging.warning("CFG:\tError setting up GPS over i2c - %s" % ex) return "GPS", self.gps_sensor elif addr == 0x48: try: from sensor.temperature_TMP102 import TemperatureSensor self.temperature_sensor = TemperatureSensor( i2c_bus=raspberrypi.i2c_bus(), debug=debug) except Exception as ex: logging.warning( "CFG:\tError setting up TEMPERATURE over i2c - %s" % ex) return "TEMPERATURE", self.temperature_sensor elif addr == 0x60: try: from sensor.compass_CMPS10 import Cmps10_Sensor self.compass_sensor = Cmps10_Sensor( i2c_bus=raspberrypi.i2c_bus(), debug=debug) except Exception as ex: logging.warning( "CFG:\tError setting up COMPASS over i2c - %s" % ex) return "COMPASS", self.compass_sensor elif addr == 0x40: #or addr == 0x70: # DriveController (using Adafruit PWM board) (not sure what 0x70 address is for...) try: from vehicle.drive_controller import AdafruitDriveController # TODO pwm addresses from config? self.drive_controller = AdafruitDriveController( i2c_addr=addr, i2c_bus=raspberrypi.i2c_bus(), debug=debug) except Exception as ex: logging.info( "CFG:\tError setting up DRIVECONTROLLER over i2c - %s" % ex) self.drive_controller = DummyDriveController() return "DRIVECONTROLLER", self.drive_controller elif addr == 0x32: # DriveController (using RaspyJuice) try: from vehicle.drive_controller import PyJuiceDriveController # TODO pwm addresses from config? self.drive_controller = PyJuiceDriveController( i2c_addr=addr, i2c_bus=raspberrypi.i2c_bus(), debug=debug) except Exception as ex: logging.info( "CFG:\tError setting up DRIVECONTROLLER over i2c - %s" % ex) self.drive_controller = DummyDriveController() return "DRIVECONTROLLER", self.drive_controller elif addr == 0x1E: return "COMPASS", "Driver not loaded - HMC5883L" elif addr == 0x53 or addr == 0x1D: # 0x53 when ALT connected to HIGH # 0x1D when ALT connected to LOW return "ACCELEROMETER", "Driver not loaded - ADXL345" elif addr == 0x69: # 0x68 when AD0 connected to LOW - conflicts with DS1307! # 0x69 when AD0 connected to HIGH return "GYRO", "Driver not loaded - ITG3200" else: return "unknown", None
buffer_1 = [0, 0, 0, 0, 0, 0, 0, 0] lat_1, lon_1 = gps_sensor.convert_buffer(buffer_1) print "%s maps to (%s, %s)" % (buffer_1, lat_1, lon_1) buffer_2 = [18, 8, 64, 31, 99, 150, 87, 254] lat_2, lon_2 = gps_sensor.convert_buffer(buffer_2) print "%s maps to (%s, %s)" % (buffer_2, lat_2, lon_2) if __name__ == "__main__": logger = logging.getLogger() logger.setLevel(logging.DEBUG) logger.addHandler(logging.StreamHandler()) print "Testing GPS sensor (running 5x with 5s pause)..." print "Initialising..." gps_sensor = GPS_NavigatronSensor(interface=raspberrypi.i2c_bus(), debug=True) # test raw conversion logic #test_buffer_conversion(gps_sensor) # heading print "Reading 1..." (fix, lat, lon, heading, speed, altitude, num_sat, timestamp, datestamp) = gps_sensor.read_sensor() print fix, lat, lon, heading, speed, altitude, num_sat, timestamp, datestamp sleep(5) print "Reading 2..." (fix, lat, lon, heading, speed, altitude, num_sat, timestamp, datestamp) = gps_sensor.read_sensor() print fix, lat, lon, heading, speed, altitude, num_sat, timestamp, datestamp sleep(5)
#!/usr/bin/python # # FishPi - An autonomous drop in the ocean # # Simple test of PWM motor and servo drive # import raspberrypi from time import sleep from drive_controller import AdafruitDriveController if __name__ == "__main__": print "stopping drive controller..." drive = AdafruitDriveController(debug=True, i2c_bus=raspberrypi.i2c_bus()) drive.set_throttle(0.0) drive.set_steering(0.0)
lat_1, lon_1 = gps_sensor.convert_buffer(buffer_1) print "%s maps to (%s, %s)" % (buffer_1, lat_1, lon_1) buffer_2 = [18, 8, 64, 31, 99, 150, 87, 254] lat_2, lon_2 = gps_sensor.convert_buffer(buffer_2) print "%s maps to (%s, %s)" % (buffer_2, lat_2, lon_2) if __name__ == "__main__": logger = logging.getLogger() logger.setLevel(logging.DEBUG) logger.addHandler(logging.StreamHandler()) print "Testing GPS sensor (running 5x with 5s pause)..." print "Initialising..." gps_sensor = GPS_NavigatronSensor(interface=raspberrypi.i2c_bus(), debug=True) # test raw conversion logic #test_buffer_conversion(gps_sensor) # heading print "Reading 1..." (fix, lat, lon, heading, speed, altitude, num_sat, timestamp, datestamp) = gps_sensor.read_sensor() print fix, lat, lon, heading, speed, altitude, num_sat, timestamp, datestamp sleep(5) print "Reading 2..." (fix, lat, lon, heading, speed, altitude, num_sat, timestamp, datestamp) = gps_sensor.read_sensor()
#!/usr/bin/python # # FishPi - An autonomous drop in the ocean # # Simple test of compass sensor # import raspberrypi from compass_CMPS10 import Cmps10_Sensor if __name__ == "__main__": print "Testing compass sensor..." compassSensor = Cmps10_Sensor(interface=raspberrypi.i2c_bus(), debug=True) # heading (heading, pitch, roll) = compassSensor.read_sensor() print "Heading %f, pitch %f, roll %f" % (heading, pitch, roll) # raw values (m_x, m_y, m_z, a_x, a_y, a_z) = compassSensor.read_sensor_raw() print "Raw values: M(x,y,z)=(%f,%f,%f) A(x,y,z)=(%f,%f,%f)" % (m_x, m_y, m_z, a_x, a_y, a_z)
def test_conversions(): #test_values(0x7FF, 128.0) test_values(0x7FF, 127.9375) test_values(0x640, 100) test_values(0x500, 80) test_values(0x4B0, 75) test_values(0x320, 50) test_values(0x190, 25) test_values(0x004, 0.25) test_values(0x000, 0) test_values(0xFFC, -0.25) test_values(0xE70, -25) test_values(0xC90, -55) def test_values(value_in, value_expected): value_out = TemperatureSensor()._convert_12b(value_in) if value_out == value_expected: print "MATCH: got %f, expected %f for %s" % (value_out, value_expected, bin(value_in)) else: print "FAIL: got %f, expected %f for %s" % (value_out, value_expected, bin(value_in)) if __name__ == "__main__": #print "Testing value conversions..." #test_conversions() print "Testing temperature sensor..." tmpSensor = TemperatureSensor(debug=True, interface=raspberrypi.i2c_bus()) value = tmpSensor.read_sensor() print "Current temperature: %f" % value
print "and back to neutral..." drive.set_steering(0.0) sleep(5) print "steer to starboard for 5 sec" drive.set_steering(0.3927) sleep(5) print "steer hard to starboard for 5 sec" drive.set_steering(0.785398) sleep(5) print "and back to neutral..." drive.set_steering(0.0) sleep(5) if __name__ == "__main__": # init i2c_bus=raspberrypi.i2c_bus() #i2c_bus.write_word_data(0x32, 1, 1500) # test servo #test_servo(i2c_bus, 0x32, 1) # test drive test_drive(i2c_bus) # test steer test_steer(i2c_bus)