Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- // Teensy 3.2 32-bit Micro-controller-based Project
- // by Erik (2015)
- // Test code for Adafruit GPS modules using MTK3329/MTK3339 driver / BNO055 Bosch / Servo Motor Shield
- //
- // This code shows how to listen to the GPS module in an interrupt
- // which allows the program to have more 'freedom' - just parse
- // when a new NMEA sentence is available! Then access data when
- // desired.
- //
- // Tested and works great with the Adafruit Ultimate GPS module
- // using MTK33x9 chipset
- #include <Servo.h>
- #include <PID_v1.h>
- #include <Adafruit_GPS.h> /* Modified in Adafruit_GPS.cpp : "isDigit()" -> "isdigit()" and "isAlpha()" -> "isalpha()" cases to work with Teensy compiling. Erik */
- #include <Wire.h>
- #include <Adafruit_Sensor.h>
- #include <Adafruit_BNO055.h>
- #include <utility/imumaths.h>
- #include <stdlib.h>
- /* This driver uses the Adafruit unified sensor library (Adafruit_Sensor),
- which provides a common 'type' for sensor data and some helper functions.
- To use this driver you will also need to download the Adafruit_Sensor
- library and include it in your libraries folder.
- You should also assign a unique ID to this sensor for use with
- the Adafruit Sensor API so that you can identify this particular
- sensor in any data logs, etc. To assign a unique ID, simply
- provide an appropriate value in the constructor below (12345
- is used by default in this example).
- Connections
- ===========
- Connect SCL to analog 5
- Connect SDA to analog 4
- Connect VDD to 3-5V DC
- Connect GROUND to common ground
- History
- =======
- 2015/MAR/03 - First release (KTOWN)
- 2015/AUG/27 - Added calibration and system status helpers
- */
- /* Set the delay between fresh samples */
- #define BNO055_SAMPLERATE_DELAY_MS (100)
- Adafruit_BNO055 bno = Adafruit_BNO055(55);
- Servo myservo; // create servo object to control a servo
- // twelve servo objects can be created on most boards
- int pos = 0; // variable to store the servo position
- /**************************************************************************/
- /*
- Displays some basic information on this sensor from the unified
- sensor API sensor_t type (see Adafruit_Sensor for more information)
- */
- /**************************************************************************/
- void displaySensorDetails(void)
- {
- sensor_t sensor;
- bno.getSensor(&sensor);
- Serial.println("------------------------------------");
- Serial.print ("Sensor: "); Serial.println(sensor.name);
- Serial.print ("Driver Ver: "); Serial.println(sensor.version);
- Serial.print ("Unique ID: "); Serial.println(sensor.sensor_id);
- Serial.print ("Max Value: "); Serial.print(sensor.max_value); Serial.println(" xxx");
- Serial.print ("Min Value: "); Serial.print(sensor.min_value); Serial.println(" xxx");
- Serial.print ("Resolution: "); Serial.print(sensor.resolution); Serial.println(" xxx");
- Serial.println("------------------------------------");
- Serial.println("");
- delay(500);
- }
- /**************************************************************************/
- /*
- Display some basic info about the sensor status
- */
- /**************************************************************************/
- void displaySensorStatus(void)
- {
- /* Get the system status values (mostly for debugging purposes) */
- uint8_t system_status, self_test_results, system_error;
- system_status = self_test_results = system_error = 0;
- bno.getSystemStatus(&system_status, &self_test_results, &system_error);
- /* Display the results in the Serial Monitor */
- Serial.println("");
- Serial.print("System Status: 0x");
- Serial.println(system_status, HEX);
- Serial.print("Self Test: 0x");
- Serial.println(self_test_results, HEX);
- Serial.print("System Error: 0x");
- Serial.println(system_error, HEX);
- Serial.println("");
- delay(500);
- }
- /**************************************************************************/
- /*
- Display sensor calibration status
- */
- /**************************************************************************/
- void displayCalStatus(void)
- {
- /* Get the four calibration values (0..3) */
- /* Any sensor data reporting 0 should be ignored, */
- /* 3 means 'fully calibrated" */
- uint8_t system, gyro, accel, mag;
- system = gyro = accel = mag = 0;
- bno.getCalibration(&system, &gyro, &accel, &mag);
- /* The data should be ignored until the system calibration is > 0 */
- Serial.print("\t");
- if (!system)
- {
- Serial.print("! ");
- }
- /* Display the individual values */
- Serial.print("Sys:");
- Serial.print(system, DEC);
- Serial.print(" G:");
- Serial.print(gyro, DEC);
- Serial.print(" A:");
- Serial.print(accel, DEC);
- Serial.print(" M:");
- Serial.print(mag, DEC);
- }
- // If you're using a GPS module:
- // Connect the GPS Power pin to 5V
- // Connect the GPS Ground pin to ground
- // If using software serial (sketch example default):
- // Connect the GPS TX (transmit) pin to Digital 3
- // Connect the GPS RX (receive) pin to Digital 2
- // If using hardware serial (e.g. Arduino Mega):
- // Connect the GPS TX (transmit) pin to Arduino RX1, RX2 or RX3
- // Connect the GPS RX (receive) pin to matching TX1, TX2 or TX3
- // If you're using the Adafruit GPS shield, change
- // SoftwareSerial mySerial(3, 2); -> SoftwareSerial mySerial(8, 7);
- // and make sure the switch is set to SoftSerial
- // If using software serial, keep this line enabled
- // (you can change the pin numbers to match your wiring):
- //SoftwareSerial mySerial(3, 2);
- // If using hardware serial (e.g. Arduino Mega), comment out the
- // above SoftwareSerial line, and enable this line instead
- // (you can change the Serial number to match your wiring):
- HardwareSerial mySerial = Serial1;
- Adafruit_GPS GPS(&mySerial);
- // Set GPSECHO to 'false' to turn off echoing the GPS data to the Serial console
- // Set to 'true' if you want to debug and listen to the raw GPS sentences.
- #define GPSECHO true
- // this keeps track of whether we're using the interrupt
- // off by default!
- //boolean usingInterrupt = false;
- //void useInterrupt(boolean); // Func prototype keeps Arduino 0023 happy
- void setup()
- {
- myservo.attach(9); // attaches the servo on pin 9 to the servo object
- // connect at 115200 so we can read the GPS fast enough and echo without dropping chars
- // also spit it out
- Serial.begin(115200);
- Serial.println("Adafruit GPS library basic test!");
- // 9600 NMEA is the default baud rate for Adafruit MTK GPS's- some use 4800
- GPS.begin(9600);
- // uncomment this line to turn on RMC (recommended minimum) and GGA (fix data) including altitude
- GPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCGGA);
- // uncomment this line to turn on only the "minimum recommended" data
- //GPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY);
- // For parsing data, we don't suggest using anything but either RMC only or RMC+GGA since
- // the parser doesn't care about other sentences at this time
- // Set the update rate
- GPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ); // 1 Hz update rate
- // For the parsing code to work nicely and have time to sort thru the data, and
- // print it out we don't suggest using anything higher than 1 Hz
- // Request updates on antenna status, comment out to keep quiet
- GPS.sendCommand(PGCMD_ANTENNA);
- // the nice thing about this code is you can have a timer0 interrupt go off
- // every 1 millisecond, and read data from the GPS for you. that makes the
- // loop code a heck of a lot easier!
- // useInterrupt(true);
- delay(1000);
- // Ask for firmware version
- mySerial.println(PMTK_Q_RELEASE);
- /* Initialise the sensor */
- if(!bno.begin())
- {
- /* There was a problem detecting the BNO055 ... check your connections */
- Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!");
- while(1);
- }
- delay(1000);
- /* Display some basic information on this sensor */
- displaySensorDetails();
- /* Optional: Display current status */
- displaySensorStatus();
- bno.setExtCrystalUse(true);
- }
- /*
- // Interrupt is called once a millisecond, looks for any new GPS data, and stores it
- SIGNAL(TIMER0_COMPA_vect) {
- char c = GPS.read();
- // if you want to debug, this is a good time to do it!
- #ifdef UDR0
- if (GPSECHO)
- if (c) UDR0 = c;
- // writing direct to UDR0 is much much faster than Serial.print
- // but only one character can be written at a time.
- #endif
- }
- void useInterrupt(boolean v) {
- if (v) {
- // Timer0 is already used for millis() - we'll just interrupt somewhere
- // in the middle and call the "Compare A" function above
- OCR0A = 0xAF;
- TIMSK0 |= _BV(OCIE0A);
- usingInterrupt = true;
- } else {
- // do not call the interrupt function COMPA anymore
- TIMSK0 &= ~_BV(OCIE0A);
- usingInterrupt = false;
- }
- }
- */
- uint32_t timer = millis();
- void loop() // run over and over again
- {
- // in case you are not using the interrupt above, you'll
- // need to 'hand query' the GPS, not suggested :(
- // if (! usingInterrupt) {
- // read data from the GPS in the 'main loop'
- char c = GPS.read();
- // if you want to debug, this is a good time to do it!
- if (GPSECHO)
- if (c) Serial.print(c);
- //}
- // if a sentence is received, we can check the checksum, parse it...
- if (GPS.newNMEAreceived()) {
- // a tricky thing here is if we print the NMEA sentence, or data
- // we end up not listening and catching other sentences!
- // so be very wary if using OUTPUT_ALLDATA and trytng to print out data
- //Serial.println(GPS.lastNMEA()); // this also sets the newNMEAreceived() flag to false
- if (!GPS.parse(GPS.lastNMEA())) // this also sets the newNMEAreceived() flag to false
- return; // we can fail to parse a sentence in which case we should just wait for another
- }
- // if millis() or timer wraps around, we'll just reset it
- if (timer > millis()) timer = millis();
- // approximately every 2 seconds or so, print out the current stats
- if (millis() - timer > 2000) {
- timer = millis(); // reset the timer
- Serial.print("\nTime: ");
- Serial.print(GPS.hour, DEC); Serial.print(':');
- Serial.print(GPS.minute, DEC); Serial.print(':');
- Serial.print(GPS.seconds, DEC); Serial.print('.');
- Serial.println(GPS.milliseconds);
- Serial.print("Date: ");
- Serial.print(GPS.day, DEC); Serial.print('/');
- Serial.print(GPS.month, DEC); Serial.print("/20");
- Serial.println(GPS.year, DEC);
- Serial.print("Fix: "); Serial.print((int)GPS.fix);
- Serial.print(" quality: "); Serial.println((int)GPS.fixquality);
- if (GPS.fix) {
- Serial.print("Location: ");
- Serial.print(GPS.latitude, 4); Serial.print(GPS.lat);
- Serial.print(", ");
- Serial.print(GPS.longitude, 4); Serial.println(GPS.lon);
- Serial.print("Location (in degrees, works with Google Maps): ");
- Serial.print(GPS.latitudeDegrees, 4);
- Serial.print(", ");
- Serial.println(GPS.longitudeDegrees, 4);
- Serial.print("Speed (knots): "); Serial.println(GPS.speed);
- Serial.print("Angle: "); Serial.println(GPS.angle);
- Serial.print("Altitude: "); Serial.println(GPS.altitude);
- Serial.print("Satellites: "); Serial.println((int)GPS.satellites);
- }
- }
- /* Get a new sensor event */
- sensors_event_t event;
- bno.getEvent(&event);
- /* Display the floating point data */
- Serial.print("X: ");
- Serial.print(event.orientation.x, 4);
- Serial.print("\tY: ");
- Serial.print(event.orientation.y, 4);
- Serial.print("\tZ: ");
- Serial.print(event.orientation.z, 4);
- /* Optional: Display calibration status */
- displayCalStatus();
- /* Optional: Display sensor status (debug only) */
- //displaySensorStatus();
- /* New line for the next sample */
- Serial.println("");
- /* Wait the specified delay before requesting nex data */
- delay(BNO055_SAMPLERATE_DELAY_MS);
- /* Servo Motion
- *
- * for(pos = 0; pos <= 180; pos += 1) // goes from 0 degrees to 180 degrees
- { // in steps of 1 degree
- myservo.write(pos); // tell servo to go to position in variable 'pos'
- delay(15); // waits 15ms for the servo to reach the position
- }
- for(pos = 180; pos>=0; pos-=1) // goes from 180 degrees to 0 degrees
- {
- myservo.write(pos); // tell servo to go to position in variable 'pos'
- delay(15); // waits 15ms for the servo to reach the position
- }
- */
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement