Add navigation page and autopilot task

lib/Arduino-PID-Library/PID_v1.cpp

@@ -0,0 +1,195 @@
+/**********************************************************************************************
+ * Arduino PID Library - Version 1.1.1
+ * by Brett Beauregard <[email protected]> brettbeauregard.com
+ *
+ * This Library is licensed under a GPLv3 License
+ **********************************************************************************************/
+
+#if ARDUINO >= 100
+  #include "Arduino.h"
+#else
+  #include "WProgram.h"
+#endif
+
+#include <PID_v1.h>
+
+/*Constructor (...)*********************************************************
+ *    The parameters specified here are those for for which we can't set up 
+ *    reliable defaults, so we need to have the user set them.
+ ***************************************************************************/
+PID::PID(double* Input, double* Output, double* Setpoint,
+        double Kp, double Ki, double Kd, int ControllerDirection)
+{
+
+    myOutput = Output;
+    myInput = Input;
+    mySetpoint = Setpoint;
+	inAuto = false;
+
+	PID::SetOutputLimits(0, 255);				//default output limit corresponds to 
+												//the arduino pwm limits
+
+    SampleTime = 100;							//default Controller Sample Time is 0.1 seconds
+
+    PID::SetControllerDirection(ControllerDirection);
+    PID::SetTunings(Kp, Ki, Kd);
+
+    lastTime = millis()-SampleTime;				
+}
+
+
+/* Compute() **********************************************************************
+ *     This, as they say, is where the magic happens.  this function should be called
+ *   every time "void loop()" executes.  the function will decide for itself whether a new
+ *   pid Output needs to be computed.  returns true when the output is computed,
+ *   false when nothing has been done.
+ **********************************************************************************/ 
+bool PID::Compute()
+{
+   if(!inAuto) return false;
+   unsigned long now = millis();
+   unsigned long timeChange = (now - lastTime);
+   if(timeChange>=SampleTime)
+   {
+      /*Compute all the working error variables*/
+	  double input = *myInput;
+      double error = *mySetpoint - input;
+      ITerm+= (ki * error);
+      if(ITerm > outMax) ITerm= outMax;
+      else if(ITerm < outMin) ITerm= outMin;
+      double dInput = (input - lastInput);
+
+      /*Compute PID Output*/
+      double output = kp * error + ITerm- kd * dInput;
+
+	  if(output > outMax) output = outMax;
+      else if(output < outMin) output = outMin;
+	  *myOutput = output;
+
+      /*Remember some variables for next time*/
+      lastInput = input;
+      lastTime = now;
+	  return true;
+   }
+   else return false;
+}
+
+
+/* SetTunings(...)*************************************************************
+ * This function allows the controller's dynamic performance to be adjusted. 
+ * it's called automatically from the constructor, but tunings can also
+ * be adjusted on the fly during normal operation
+ ******************************************************************************/ 
+void PID::SetTunings(double Kp, double Ki, double Kd)
+{
+   if (Kp<0 || Ki<0 || Kd<0) return;
+
+   dispKp = Kp; dispKi = Ki; dispKd = Kd;
+
+   double SampleTimeInSec = ((double)SampleTime)/1000;  
+   kp = Kp;
+   ki = Ki * SampleTimeInSec;
+   kd = Kd / SampleTimeInSec;
+
+  if(controllerDirection ==REVERSE)
+   {
+      kp = (0 - kp);
+      ki = (0 - ki);
+      kd = (0 - kd);
+   }
+}
+
+/* SetSampleTime(...) *********************************************************
+ * sets the period, in Milliseconds, at which the calculation is performed	
+ ******************************************************************************/
+void PID::SetSampleTime(int NewSampleTime)
+{
+   if (NewSampleTime > 0)
+   {
+      double ratio  = (double)NewSampleTime
+                      / (double)SampleTime;
+      ki *= ratio;
+      kd /= ratio;
+      SampleTime = (unsigned long)NewSampleTime;
+   }
+}
+
+/* SetOutputLimits(...)****************************************************
+ *     This function will be used far more often than SetInputLimits.  while
+ *  the input to the controller will generally be in the 0-1023 range (which is
+ *  the default already,)  the output will be a little different.  maybe they'll
+ *  be doing a time window and will need 0-8000 or something.  or maybe they'll
+ *  want to clamp it from 0-125.  who knows.  at any rate, that can all be done
+ *  here.
+ **************************************************************************/
+void PID::SetOutputLimits(double Min, double Max)
+{
+   if(Min >= Max) return;
+   outMin = Min;
+   outMax = Max;
+
+   if(inAuto)
+   {
+	   if(*myOutput > outMax) *myOutput = outMax;
+	   else if(*myOutput < outMin) *myOutput = outMin;
+
+	   if(ITerm > outMax) ITerm= outMax;
+	   else if(ITerm < outMin) ITerm= outMin;
+   }
+}
+
+/* SetMode(...)****************************************************************
+ * Allows the controller Mode to be set to manual (0) or Automatic (non-zero)
+ * when the transition from manual to auto occurs, the controller is
+ * automatically initialized
+ ******************************************************************************/ 
+void PID::SetMode(int Mode)
+{
+    bool newAuto = (Mode == AUTOMATIC);
+    if(newAuto && !inAuto)
+    {  /*we just went from manual to auto*/
+        PID::Initialize();
+    }
+    inAuto = newAuto;
+}
+
+/* Initialize()****************************************************************
+ *	does all the things that need to happen to ensure a bumpless transfer
+ *  from manual to automatic mode.
+ ******************************************************************************/ 
+void PID::Initialize()
+{
+   ITerm = *myOutput;
+   lastInput = *myInput;
+   if(ITerm > outMax) ITerm = outMax;
+   else if(ITerm < outMin) ITerm = outMin;
+}
+
+/* SetControllerDirection(...)*************************************************
+ * The PID will either be connected to a DIRECT acting process (+Output leads 
+ * to +Input) or a REVERSE acting process(+Output leads to -Input.)  we need to
+ * know which one, because otherwise we may increase the output when we should
+ * be decreasing.  This is called from the constructor.
+ ******************************************************************************/
+void PID::SetControllerDirection(int Direction)
+{
+   if(inAuto && Direction !=controllerDirection)
+   {
+	  kp = (0 - kp);
+      ki = (0 - ki);
+      kd = (0 - kd);
+   }   
+   controllerDirection = Direction;
+}
+
+/* Status Funcions*************************************************************
+ * Just because you set the Kp=-1 doesn't mean it actually happened.  these
+ * functions query the internal state of the PID.  they're here for display 
+ * purposes.  this are the functions the PID Front-end uses for example
+ ******************************************************************************/
+double PID::GetKp(){ return  dispKp; }
+double PID::GetKi(){ return  dispKi;}
+double PID::GetKd(){ return  dispKd;}
+int PID::GetMode(){ return  inAuto ? AUTOMATIC : MANUAL;}
+int PID::GetDirection(){ return controllerDirection;}
+

lib/Arduino-PID-Library/PID_v1.h

@@ -0,0 +1,81 @@
+#ifndef PID_v1_h
+#define PID_v1_h
+#define LIBRARY_VERSION	1.1.1
+
+class PID
+{
+
+
+  public:
+
+  //Constants used in some of the functions below
+  #define AUTOMATIC	1
+  #define MANUAL	0
+  #define DIRECT  0
+  #define REVERSE  1
+
+  //commonly used functions **************************************************************************
+
+    PID(double*, double*, double*,        // * constructor.  links the PID to the Input, Output, and
+        double, double, double, int);     //   Setpoint.  Initial tuning parameters are also set here
+
+    void SetMode(int Mode);               // * sets PID to either Manual (0) or Auto (non-0)
+
+    bool Compute();                       // * performs the PID calculation.  it should be
+                                          //   called every time loop() cycles. ON/OFF and
+                                          //   calculation frequency can be set using SetMode
+                                          //   SetSampleTime respectively
+
+    void SetOutputLimits(double, double); //clamps the output to a specific range. 0-255 by default, but
+										  //it's likely the user will want to change this depending on
+										  //the application
+
+
+
+  //available but not commonly used functions ********************************************************
+    void SetTunings(double, double,       // * While most users will set the tunings once in the 
+                    double);         	  //   constructor, this function gives the user the option
+                                          //   of changing tunings during runtime for Adaptive control
+	void SetControllerDirection(int);	  // * Sets the Direction, or "Action" of the controller. DIRECT
+										  //   means the output will increase when error is positive. REVERSE
+										  //   means the opposite.  it's very unlikely that this will be needed
+										  //   once it is set in the constructor.
+    void SetSampleTime(int);              // * sets the frequency, in Milliseconds, with which 
+                                          //   the PID calculation is performed.  default is 100
+
+
+
+  //Display functions ****************************************************************
+	double GetKp();						  // These functions query the pid for interal values.
+	double GetKi();						  //  they were created mainly for the pid front-end,
+	double GetKd();						  // where it's important to know what is actually 
+	int GetMode();						  //  inside the PID.
+	int GetDirection();					  //
+
+  private:
+	void Initialize();
+
+	double dispKp;				// * we'll hold on to the tuning parameters in user-entered 
+	double dispKi;				//   format for display purposes
+	double dispKd;				//
+
+	double kp;                  // * (P)roportional Tuning Parameter
+    double ki;                  // * (I)ntegral Tuning Parameter
+    double kd;                  // * (D)erivative Tuning Parameter
+
+	int controllerDirection;
+
+    double *myInput;              // * Pointers to the Input, Output, and Setpoint variables
+    double *myOutput;             //   This creates a hard link between the variables and the 
+    double *mySetpoint;           //   PID, freeing the user from having to constantly tell us
+                                  //   what these values are.  with pointers we'll just know.
+
+	unsigned long lastTime;
+	double ITerm, lastInput;
+
+	unsigned long SampleTime;
+	double outMin, outMax;
+	bool inAuto;
+};
+#endif
+

lib/Arduino-PID-Library/README.txt

@@ -0,0 +1,11 @@
+***************************************************************
+* Arduino PID Library - Version 1.1.1
+* by Brett Beauregard <[email protected]> brettbeauregard.com
+*
+* This Library is licensed under a GPLv3 License
+***************************************************************
+
+ - For an ultra-detailed explanation of why the code is the way it is, please visit: 
+   http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-introduction/
+
+ - For function documentation see:  http://playground.arduino.cc/Code/PIDLibrary

lib/Arduino-PID-Library/keywords.txt

@@ -0,0 +1,34 @@
+#######################################
+# Syntax Coloring Map For PID Library
+#######################################
+
+#######################################
+# Datatypes (KEYWORD1)
+#######################################
+
+PID	KEYWORD1
+
+#######################################
+# Methods and Functions (KEYWORD2)
+#######################################
+
+SetMode	KEYWORD2
+Compute	KEYWORD2
+SetOutputLimits	KEYWORD2
+SetTunings	KEYWORD2
+SetControllerDirection	KEYWORD2
+SetSampleTime	KEYWORD2
+GetKp	KEYWORD2
+GetKi	KEYWORD2
+GetKd	KEYWORD2
+GetMode	KEYWORD2
+GetDirection	KEYWORD2
+
+#######################################
+# Constants (LITERAL1)
+#######################################
+
+AUTOMATIC	LITERAL1
+MANUAL	LITERAL1
+DIRECT	LITERAL1
+REVERSE	LITERAL1

lib/Arduino-PID-Library/library.json

@@ -0,0 +1,19 @@
+{
+  "name": "PID",
+  "keywords": "PID, controller, signal",
+  "description": "A PID controller seeks to keep some input variable close to a desired setpoint by adjusting an output. The way in which it does this can be 'tuned' by adjusting three parameters (P,I,D).",
+  "url": "http://playground.arduino.cc/Code/PIDLibrary",
+  "include": "PID_v1",
+  "authors":
+  [
+    {
+      "name": "Brett Beauregard"
+    }
+  ],
+  "repository":
+  {
+    "type": "git",
+    "url": "https://github.com/br3ttb/Arduino-PID-Library.git"
+  },
+  "frameworks": "arduino"
+}

lib/Arduino-PID-Library/library.properties

@@ -0,0 +1,9 @@
+name=PID
+version=1.1.1
+author=Brett Beauregard 
+maintainer=Brett Beauregard
+sentence=PID controller
+paragraph=A PID controller seeks to keep some input variable close to a desired setpoint by adjusting an output. The way in which it does this can be 'tuned' by adjusting three parameters (P,I,D).
+category=Signal Input/Output
+url=http://playground.arduino.cc/Code/PIDLibrary
+architectures=* 

lib/KBox/src/KBox.h

@@ -43,6 +43,7 @@
 #include "tasks/IMUTask.h"
 #include "tasks/NMEAReaderTask.h"
 #include "tasks/WiFiTask.h"
+#include "tasks/AutoPilotTask.h" //RIGM added
 
 /* Converters */
 #include "converters/VoltageN2kConverter.h"
@@ -51,6 +52,7 @@
 /* Pages */
 #include "pages/BatteryMonitorPage.h"
 #include "pages/StatsPage.h"
+#include "pages/NavigationPage.h" //RIGM added
 
 /* Other dependencies */
 #include <Adafruit_NeoPixel.h>