public
anonymous / gist:4699857
Created

  • Download Gist
gistfile1.cpp
C++
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169
/******************************************************************
* PID Simple Example (Augmented with Processing.org Communication)
* Version 0.3
* by Brett Beauregard
* License: Creative-Commons Attribution Share-Alike
* April 2011
******************************************************************/
 
#include <PID_v1.h>
#include <MegaEncoderCounter.h>
#include <AFMotor.h>
 
MegaEncoderCounter megaEncoderCounter(4); // Initializes the Mega Encoder Counter in the 4X Count Mode
AF_DCMotor motor(1);
 
//Define Variables we'll be connecting to
double Setpoint, Input, Output;
int inputPin=0, outputPin=3;
 
 
unsigned long count;
unsigned long rgcount;
int qualityTestHigh[32];
int qualityTestLow[32];
 
 
 
//Specify the links and initial tuning parameters
PID myPID(&Input, &Output, &Setpoint,0.5,5,1, DIRECT);
 
 
unsigned long serialTime; //this will help us know when to talk with processing
 
void setup()
{
//initialize the serial link with processing
Serial.begin(9600);
//initialize the variables we're linked toh
Input =megaEncoderCounter.XAxisGetCount();
Setpoint = 100000;
 
//turn the PID on
myPID.SetMode(AUTOMATIC);
}
 
void loop()
{
//pid-related code
Input =megaEncoderCounter.XAxisGetCount();
myPID.Compute();
motor.setSpeed(Output);
motor.run(FORWARD);
//send-receive with processing if it's time
if(millis()>serialTime)
{
SerialReceive();
SerialSend();
serialTime+=500;
}
}
 
 
/********************************************
* Serial Communication functions / helpers
********************************************/
 
 
union { // This Data structure lets
byte asBytes[24]; // us take the byte array
float asFloat[6]; // sent from processing and
} // easily convert it to a
foo; // float array
 
 
 
// getting float values from processing into the arduino
// was no small task. the way this program does it is
// as follows:
// * a float takes up 4 bytes. in processing, convert
// the array of floats we want to send, into an array
// of bytes.
// * send the bytes to the arduino
// * use a data structure known as a union to convert
// the array of bytes back into an array of floats
 
// the bytes coming from the arduino follow the following
// format:
// 0: 0=Manual, 1=Auto, else = ? error ?
// 1: 0=Direct, 1=Reverse, else = ? error ?
// 2-5: float setpoint
// 6-9: float input
// 10-13: float output
// 14-17: float P_Param
// 18-21: float I_Param
// 22-245: float D_Param
void SerialReceive()
{
 
// read the bytes sent from Processing
int index=0;
byte Auto_Man = -1;
byte Direct_Reverse = -1;
while(Serial.available()&&index<26)
{
if(index==0) Auto_Man = Serial.read();
else if(index==1) Direct_Reverse = Serial.read();
else foo.asBytes[index-2] = Serial.read();
index++;
}
// if the information we got was in the correct format,
// read it into the system
if(index==26 && (Auto_Man==0 || Auto_Man==1)&& (Direct_Reverse==0 || Direct_Reverse==1))
{
Setpoint=double(foo.asFloat[0]);
//Input=double(foo.asFloat[1]); // * the user has the ability to send the
// value of "Input" in most cases (as
// in this one) this is not needed.
if(Auto_Man==0) // * only change the output if we are in
{ // manual mode. otherwise we'll get an
Output=double(foo.asFloat[2]); // output blip, then the controller will
} // overwrite.
double p, i, d; // * read in and set the controller tunings
p = double(foo.asFloat[3]); //
i = double(foo.asFloat[4]); //
d = double(foo.asFloat[5]); //
myPID.SetTunings(p, i, d); //
if(Auto_Man==0) myPID.SetMode(MANUAL);// * set the controller mode
else myPID.SetMode(AUTOMATIC); //
if(Direct_Reverse==0) myPID.SetControllerDirection(DIRECT);// * set the controller Direction
else myPID.SetControllerDirection(REVERSE); //
}
Serial.flush(); // * clear any random data from the serial buffer
}
 
// unlike our tiny microprocessor, the processing ap
// has no problem converting strings into floats, so
// we can just send strings. much easier than getting
// floats from processing to here no?
void SerialSend()
{
Serial.print("PID ");
Serial.print(Setpoint);
Serial.print(" ");
Serial.print(Input);
Serial.print(" ");
Serial.print(Output);
Serial.print(" ");
Serial.print(myPID.GetKp());
Serial.print(" ");
Serial.print(myPID.GetKi());
Serial.print(" ");
Serial.print(myPID.GetKd());
Serial.print(" ");
if(myPID.GetMode()==AUTOMATIC) Serial.print("Automatic");
else Serial.print("Manual");
Serial.print(" ");
if(myPID.GetDirection()==DIRECT) Serial.println("Direct");
else Serial.println("Reverse");
}

Please sign in to comment on this gist.

Something went wrong with that request. Please try again.