Skip to content

Commit

Permalink
PID code working as intended
Browse files Browse the repository at this point in the history
  • Loading branch information
tekdemo committed May 15, 2016
1 parent 7f80c23 commit e2dd85c
Show file tree
Hide file tree
Showing 4 changed files with 55 additions and 21 deletions.
4 changes: 2 additions & 2 deletions MiniPID.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -145,14 +145,14 @@ void MiniPID::setMaxIOutput(double maximum){
* set to (-maximum).
* @param output
*/
void MiniPID::setMaxOutput(double output){ setMaxOutput(-output,output);}
void MiniPID::setOutputLimits(double output){ setOutputLimits(-output,output);}

/**
* Specify a maximum output.
* @param minimum possible output value
* @param maximum possible output value
*/
void MiniPID::setMaxOutput(double minimum,double maximum){
void MiniPID::setOutputLimits(double minimum,double maximum){
if(maximum<minimum)return;
maxOutput=maximum;
minOutput=minimum;
Expand Down
4 changes: 2 additions & 2 deletions MiniPID.h
Original file line number Diff line number Diff line change
Expand Up @@ -12,8 +12,8 @@ class MiniPID{
void setPID(double p, double i, double d);
void setPID(double p, double i, double d,double f);
void setMaxIOutput(double maximum);
void setMaxOutput(double output);
void setMaxOutput(double minimum,double maximum);
void setOutputLimits(double output);
void setOutputLimits(double minimum,double maximum);
void setDirection(bool reversed);
void setSetpoint(double setpoint);
void reset();
Expand Down
62 changes: 48 additions & 14 deletions MiniPID.ino
Original file line number Diff line number Diff line change
@@ -1,22 +1,56 @@
#include "miniPID.h"
#include "Arduino.h"
#include <MiniPID.h>

#define SETPOINTPIN 0
#define FEEDBACKPIN 1
#define FEEDBACKPIN A1
#define GROUNDPIN A0
#define OUTPUTPIN 10

pid=miniPID(1,.1,.01);
unsigned long int t;
MiniPID pid(.1, 0.01, 0.00);

void setup(){
t=0;
}
void setup() {
Serial.begin(9600);
Serial.println("Configuring");

void loop(){
double output=pid.getOutput(analogRead(FEEDBACKPIN),analogRead(SETPOINTPIN););
analogWrite(OUTPUTPIN,output);
// put your setup code here, to run once:
pid.setOutputLimits(0,255);
//pid.setOutputFilter(0.01);
pid.setOutputRampRate(20);


pinMode(OUTPUTPIN,OUTPUT);
digitalWrite(OUTPUTPIN,LOW);

pinMode(GROUNDPIN,OUTPUT);
digitalWrite(GROUNDPIN,LOW);

//Wait until the next 50ms interval
while(millis()-t<50) delay(1);
t=millis();
delay(1000);
Serial.println("All set! Starting loop");
//Serial.print("Initial Voltage: ");
//Serial.println(analogRead(FEEDBACKPIN);
}

double fakesensor=0;
void loop() {

double target=768;
fakesensor-=5;
double out=pid.getOutput(fakesensor,target);
fakesensor+=out;
Serial.println((int)fakesensor);
delay(200);
/*
double target=768;
double feedback=analogRead(FEEDBACKPIN);
int out=pid.getOutput(feedback,target);
//analogWrite(OUTPUTPIN,out);
Serial.print("F: ");
Serial.println((int)feedback);
Serial.print("\t\tO: ");
Serial.println((int)out);
delay(50);
//*/
}
6 changes: 3 additions & 3 deletions README.md
Original file line number Diff line number Diff line change
Expand Up @@ -85,12 +85,12 @@ For this class of systems, it's helpful to consider the F term the primary varia

For tuning, F as the primary variable results in P, I, and D being used for minor corrections, with a much smaller system error. P and I will generally be sufficient, and can correct for non-linearities in the system such as such as drag, inertia, friction, and disturbances.

#### `setMaxOutput(double minimum,double maximum)`
#### `setMaxOutput(double output)`
#### `setOutputLimits(double minimum,double maximum)`
#### `setOutputLimits(double output)`
Optional, but highly recommended to set. The set the output limits, and ensure the controller behaves when reaching the maximum output capabilities of your physical system.

#### `setMaxIOutput(double maximum)`
Sets the maximum output generated by the I term inside the controller. This is independent of the `setMaxOutput` values. This can assist in reducing windup over large setpoint changes or stall conditions.
Sets the maximum output generated by the I term inside the controller. This is independent of the `setOutputLimits` values. This can assist in reducing windup over large setpoint changes or stall conditions.

#### `setDirection(boolean reversed)`
Reverses the output. Use this if the controller attempts to go the wrong way during operation.
Expand Down

0 comments on commit e2dd85c

Please sign in to comment.