/*
This code is in the public domain.
*/
#include <PID_v1.h>
#include <TinyGPS++.h>
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>
#include <Servo.h>
#include <PulsePosition.h>
// Simple loopback test: create 1 output to transmit
// test pulses, and 1 input to receive the pulses
PulsePositionOutput myOut;
PulsePositionInput myIn;
const unsigned X_AXIS_PIN = 0;
const unsigned Y_AXIS_PIN = 1;
const unsigned Z_AXIS_PIN = 2;
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
//Define Variables we'll be connecting to
double Setpoint, Input, Output;
//Define the aggressive and conservative Tuning Parameters
double aggKp=4, aggKi=0.2, aggKd=1;
double consKp=0.75, consKi=0.025, consKd=0.025;
//Specify the links and initial tuning parameters
PID myPID(&Input, &Output, &Setpoint,2,0.01,0.01, DIRECT);
// The TinyGPS++ object
TinyGPSPlus gps;
/* 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.3-5V DC
Connect GROUND to common ground
History
=======
2015/MAR/03 - First release (KTOWN)
*/
/* Set the delay between fresh samples */
#define BNO055_SAMPLERATE_DELAY_MS (100)
Adafruit_BNO055 bno = Adafruit_BNO055(55);
/**************************************************************************/
/*
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);
}
const int reset_pin = 4;
const int led_pin = 13; // 13 = Teensy 3.X & LC
// 11 = Teensy 2.0
// 6 = Teensy++ 2.0
// set this to the hardware serial port you wish to use
#define HWSERIAL Serial1
/**************************************************************************
Arduino setup function (automatically called at startup)
**************************************************************************/
void setup(void)
{
HWSERIAL.begin(9600);
HWSERIAL.println("$PMTK220,100*2F");
HWSERIAL.println("$PMTK314,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*28");
pinMode(led_pin, OUTPUT);
digitalWrite(led_pin, LOW);
digitalWrite(reset_pin, HIGH);
pinMode(reset_pin, OUTPUT);
Serial.begin(115200);
Serial.println("Orientation Sensor Test"); Serial.println("");
/* 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(2000);
/* Display some basic information on this sensor */
displaySensorDetails();
/* Servo Code */
// myservo.attach(9);
//myservo.write(0);
//initialize the variables we're linked to
//Input = analogRead(0);
// Serial.println(myservo.read());
//Serial.println(myservo.read());
//Input = myservo.read();
Setpoint = 1.00;
myPID.SetOutputLimits(-360.00, 360.00);
//turn the PID on
myPID.SetMode(AUTOMATIC);
myIn.begin(5);
}
long led_on_time=0;
unsigned char prev_dtr = 0;
/**************************************************************************/
/*
Arduino loop function, called once 'setup' is complete (your own code
should go here)
*/
/**************************************************************************/
void loop(void)
{
unsigned char dtr;
int incomingByte;
/* Get a new sensor event */
sensors_event_t event;
bno.getEvent(&event);
/* Board layout:
+----------+
| *| RST PITCH ROLL HEADING
ADR |* *| SCL
INT |* *| SDA ^ /->
PS1 |* *| GND | |
PS0 |* *| 3VO Y Z--> \-X
| *| VIN
+----------+
*/
if (HWSERIAL.available() > 0) {
incomingByte = HWSERIAL.read();
if (gps.encode(incomingByte)) { displayInfo();
/* The processing sketch expects data as roll, pitch, heading */
Serial.print(F("Orientation: "));
Serial.print((float)event.orientation.x);
Serial.print(F(" "));
Serial.print((float)event.orientation.y);
Serial.print(F(" "));
Serial.print((float)event.orientation.z);
Serial.println(F(""));
/* Display the current temperature */
int8_t temp = bno.getTemp();
Serial.print("Current Temperature: ");
Serial.print(temp);
Serial.println(" C");
Serial.println("");
/* Also send calibration data for each sensor. */
uint8_t sys, gyro, accel, mag = 0;
bno.getCalibration(&sys, &gyro, &accel, &mag);
Serial.print(F("Calibration: "));
Serial.print(sys, DEC);
Serial.print(F(" "));
Serial.print(gyro, DEC);
Serial.print(F(" "));
Serial.print(accel, DEC);
Serial.print(F(" "));
Serial.println(mag, DEC);
/* PID Control of Heading */
Input = event.orientation.x;
myPID.Compute();
Serial.print("Calculated Output of PID Control based on Input and Setpoint of 1.00 Degrees: ");
Serial.println(Output);
// turn on the LED to indicate activity
digitalWrite(led_pin, HIGH);
led_on_time = millis(); }
// Every time new data arrives from the CPPM signal, simply print it
// to the Arduino Serial Monitor.
if (myIn.available() > 0){
float val = myIn.read(3);
Serial.print("CPPM Channel 3 :");
Serial.println(val);
}
}
// check if the USB virtual serial port has raised DTR
dtr = Serial.dtr();
if (dtr && !prev_dtr) {
digitalWrite(reset_pin, LOW);
delayMicroseconds(250);
digitalWrite(reset_pin, HIGH);
}
prev_dtr = dtr;
// if the LED has been left on without more activity, turn it off
if (millis() - led_on_time > 3) {
digitalWrite(led_pin, LOW);
}
// delay(BNO055_SAMPLERATE_DELAY_MS);
/* Servo Code
Input = myservo.read();
myPID.Compute();
myservo.write(Output);
Serial.print(Output);
Serial.print(" Servo: ");
Serial.println(myservo.read());
*/
}
void displayInfo()
{
Serial.print(F("Location: "));
if (gps.location.isValid())
{
Serial.print(gps.location.lat(), 6);
Serial.print(F(","));
Serial.print(gps.location.lng(), 6);
Serial.print(F(" Altitide (meters) : "));
Serial.print(gps.altitude.meters());
}
else
{
Serial.print(F("INVALID"));
}
Serial.print(F(" Date/Time: "));
if (gps.date.isValid())
{
Serial.print(gps.date.month());
Serial.print(F("/"));
Serial.print(gps.date.day());
Serial.print(F("/"));
Serial.print(gps.date.year());
}
else
{
Serial.print(F("INVALID"));
}
Serial.print(F(" "));
if (gps.time.isValid())
{
if (gps.time.hour() < 10) Serial.print(F("0"));
Serial.print(gps.time.hour());
Serial.print(F(":"));
if (gps.time.minute() < 10) Serial.print(F("0"));
Serial.print(gps.time.minute());
Serial.print(F(":"));
if (gps.time.second() < 10) Serial.print(F("0"));
Serial.print(gps.time.second());
Serial.print(F("."));
if (gps.time.centisecond() < 10) Serial.print(F("0"));
Serial.print(gps.time.centisecond());
}
else
{
Serial.print(F("INVALID"));
}
Serial.println();
}