Created
February 3, 2013 16:54
-
-
Save anonymous/4702547 to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
/****************************************************************** | |
* 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.01,0.1,0, 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(); | |
if (Input>10000000){ | |
Input=0;} | |
Setpoint = 100000; | |
//turn the PID on | |
myPID.SetMode(AUTOMATIC); | |
myPID.SetOutputLimits(-255, 255); | |
myPID.SetSampleTime(1); | |
} | |
void loop() | |
{ | |
//pid-related code | |
Input =megaEncoderCounter.XAxisGetCount(); | |
if (Input>10000000){ | |
Input=0;} | |
myPID.Compute(); | |
if(Output >= 0){ | |
motor.setSpeed(Output); | |
motor.run(FORWARD); | |
} | |
else{ | |
motor.setSpeed(abs(Output)); | |
motor.run(BACKWARD); | |
} | |
//send-receive with processing if it's time | |
if(millis()>serialTime) | |
{ | |
SerialReceive(); | |
SerialSend(); | |
serialTime+=100; | |
} | |
} | |
/******************************************** | |
* 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.println(Input); | |
// Serial.print(" "); | |
// Serial.println(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"); | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment