#!/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)
