PIDThrottleMyPIDswitch.pde
PIDThrottleMyPIDswitch.pde
////////////////////////////////////////////////
/*
PID Throttle Governor
M.G.McKinley
6/1/2009
http://mckgyver.pbworks.com
*********** THE BURRITO-WARE LICENSE *************
As long as you retain this notice you can do whatever you
want with this program. If we meet some day, and you think
my work is cool/helpful you can can get me a buritto in return.
Compiler = Arduino v15
Project microcontroller = Atmel 168 RBBB Arduino
Servo = Generic 100g 15in*oz
Encoder = 2ch rotary encoder (custom for this case, but anything quadrature will work)
Tachometer = Photogate + 8 hole encoder disc see schematic for optical tachometer http://mckgyver.pbworks.com/Arduino_Tachometer
Functionality test, code needs to be optimized
This is the software side of an Arduino powered governor for an internal combustion engine. Closed loop control is accomplished
by measuring engine speed (with an optical tachometer) and adjusting throttle body position with an RC servo. The governor utilizes
a simple PID algorythm to calculate throttle changes, and incorperates a number of safeguards to avoid damaging the engine.
*/
////////////////////////////////////////////////////////////////////////////////////////////////
int DataFast = LOW; //High for time, RPM
int PrintRate =10; // # of cycles before printing data
byte loopCount=0;
long PIDtime=0;
int ThrottleMin =0; //in degrees, determines servo open loop idle point
int ThrottleMax =101; //in degrees, sets servo throttle max
int RPMmin=0;
int RPMmax=1600; //Highest RPM allowed for closed loop setpoint
int PresetSetpointPin = 7; //IO pin for preset button
int OpenLoopPresetSetpoint = 60; // degrees
int ClosedLoopPresetSetpoint = 1300; //RPM setting
// smooth setpoint adjustment (not used)
int AdjustingSetpoint = 0;
int SetpointInitial;
int SetpointFinal;
int dSetpoint;
// diesel protection
int KillRPM = 1800; //sets max RPM threshold (auto kill if exceeded)
int RunAway=0;//0.4;
int RunAwayThreshold=ClosedLoopPresetSetpoint+100; // detect rpm surge after rapid unloading
int Reacting=0;
//PID
float Setpoint= 0; //Initial Closed loop setpoint
float Input=0;
float Output=0;
float OutputOld=0;
float Kp=0.09;//.09;
float Ki=0.000025;//0.00005;//0.00001;
float Integral=0;
float Kd=0; // Turn derivative action off
float Error=0;
float PreviousError=0;
float Derivative=0;
// Servo
#include <Servo.h>
Servo myservo; // create servo object to control a servo
int Throttle=ThrottleMin; //Set servo to 0 on reboot
int ThrottlePin = 10; //Pin to attach servo
//Pot
//The governor is designed to use a rotary encoder as primary adjustment device
//Potentiometer is implemented in code as redundant system.
int potpin = 0; // analog pin used to connect the setpoint adjustment potientiometer
int KnobVal; // variable to read the value from the analog pin
int AnalogSelect = 6; //pin to enable potpin as an input
int AnalogSelectCount=0;
//Tach Variables
int TicsPerRev = 8; // define number of tics per rev of code wheel
float rpm = 0.0; // I prefer float for rpm
volatile int rpmcount =0; // volitile because the variable is manipulated during the interrupt
unsigned long timeold = 0; // used to calculate d_t= millis()-timeold;
int d_t;
int statusPin = 13; // LED connected to digital pin (changes state with each tic of code wheel)
volatile byte status= LOW; // set initial state of status LED
//Kill Switch
int KillSwitch = 5;
int ESTOP=HIGH;
//Smooth Tach
#define NUMREADINGS 10
int readings[NUMREADINGS]; // the readings from the analog input
int index = 0; // the index of the current reading
int total = 0; // the running total
int RPMaverage = 0; // the average
//Encoder
int encoderPinA = 3; //Interrupt 1
int encoderPinB = 4;
//Open Closed Loop Switch
int ModeSwitchPin = 8;
int ModeSwitchCount = 0;
int ControlMode = 1; //1 = Open Loop, 0 = closed loop
int ClosedLoopLED = 11;
int OpenLoopLED = 12;
////////////////////////////////////////////////////////////////////////////////////////////////
void setup()
{
Serial.begin(9600);
//Encoder
pinMode (encoderPinB,INPUT);
//Kill Switch
pinMode (KillSwitch,INPUT);
//Analog Input Select
pinMode (AnalogSelect,INPUT);
//Open Closed Loop Select
pinMode (ModeSwitchPin,INPUT);
pinMode (ClosedLoopLED,OUTPUT);
pinMode (OpenLoopLED,OUTPUT);
digitalWrite(ClosedLoopLED, LOW);
digitalWrite(OpenLoopLED, HIGH);
//ModeSwitchCount=10;
//ModeSwitch();
pinMode (PresetSetpointPin,INPUT);
//Servo
myservo.attach(9); // attaches the servo on pin 9 to the servo object
pinMode(ThrottlePin, OUTPUT); // sets the pin as output
//Tach Status
pinMode(statusPin, OUTPUT); //Use statusPin to flash along with interrupts
//Smooth RPM
for (int i = 0; i < NUMREADINGS; i++)
readings[i] = 0; // initialize all the readings to 0
Throttle_Reset();
//Interrupt 0 is digital pin 2, so that is where the IR detector is connected
//Triggers on FALLING (change from HIGH to LOW)
attachInterrupt(1, set_fun, FALLING);
attachInterrupt(0, rpm_fun, FALLING);
}
////////////////////////////////////////////////////////////////////////////////////////////////
void loop()
{
//Don't process interrupts during calculations
detachInterrupt(0);
detachInterrupt(1);
d_t=millis()-timeold;
//KillSwitch
//Ideally the kill switch should be on an interrupt, but Ive already used bith of them
if (digitalRead(KillSwitch) == LOW)
{
Throttle_Reset();
}
//Update RPM every 1/10 second
if (d_t >= 100)
{
//Stall Failsafe
if (RPMaverage <1 && Setpoint > 10 && ControlMode == 0)
{
Throttle_Reset();
}
if (RPMaverage == 0 && ControlMode == 0)
{
ControlMode=1;
digitalWrite(ClosedLoopLED, LOW);
digitalWrite(OpenLoopLED, HIGH);
ModeSwitchCount=-12;
}
if (digitalRead(ModeSwitchPin) == HIGH)
{
ESTOP=LOW;
}
if (ESTOP == HIGH) //Wont let governor return to closed loop without reset
{
ControlMode=1;
digitalWrite(ClosedLoopLED, LOW);
digitalWrite(OpenLoopLED, HIGH);
ModeSwitchCount=-12;
}
//Mode Check
ModeSwitch();
//Preset Setpoint
if ((digitalRead(PresetSetpointPin) == LOW && ESTOP == LOW) )//| AdjustingSetpoint == 1)
{
PresetSetpointAdjust();
}
CalculateRPM();
if (RPMaverage > KillRPM)
{
Throttle_Reset();
}
timeold = millis();
Input=RPMaverage;
if (ControlMode == 0) //Closed Loop mode
{
//PID////////////////////////////////////////////////////////////////////////////////////////////////
Error=Setpoint-Input;
if (Throttle < ThrottleMax && Throttle > ThrottleMin) //Anti Windup
{
Integral = Integral + Error*d_t;
}
Derivative = (Error - PreviousError)/d_t;
Output = Kp*Error + Ki*Integral + Kd*Derivative;
Output=constrain(Output,ThrottleMin,ThrottleMax);
Throttle = Output; // ready for switch to open loop mode
}
else //Open Loop mode
{
Output=Throttle;
Integral=Output/Ki;
if (digitalRead(ModeSwitchPin) == HIGH)
{
Setpoint=RPMaverage; // ready for switch to closed loop mode
}
}
if (RPMaverage > RunAwayThreshold)
{
RunAwayAdjust();
}
if (Reacting >0)
{
RunAwayAdjust();
}
myservo.write(Output); // sets the servo position according to the scaled value
//analogWrite(ThrottlePin,map(Output,ThrottleMin,ThrottleMax,0,255));
OutputOld=Output;
d_t=0; //reset d_t
if (loopCount >= PrintRate)
{
PrintData();
}
loopCount++;
rpmcount = 0; //reset rpmcount
}
//Restart the interrupt processing
attachInterrupt(1, set_fun, FALLING);
attachInterrupt(0, rpm_fun, FALLING);
}
////////////////////////////////////////////////////////////////////////////////////////////////
void rpm_fun()
{
//This interrupt is run at each codewheel tic
detachInterrupt(0); //im not sure if this is necessary here
rpmcount++; //update rpmcount
//Toggle status LED
if (status == LOW) {
status = HIGH;
} else {
status = LOW;
}
digitalWrite(statusPin, status);
attachInterrupt(0, rpm_fun, FALLING);
}
////////////////////////////////////////////////////////////////////////////////////////////////
void set_fun()
{
if (ControlMode == 1)
{
if (digitalRead(encoderPinB) == LOW)
{
Throttle = Throttle - 2;
Throttle = constrain(Throttle,ThrottleMin,ThrottleMax);
}
else
{
Throttle = Throttle + 2;
Throttle = constrain(Throttle,ThrottleMin,ThrottleMax);
}
}
else
{
if (digitalRead(encoderPinB) == LOW)
{
Setpoint=Setpoint - 25;
Setpoint=constrain(Setpoint,RPMmin,RPMmax);
}
else
{
Setpoint=Setpoint + 25;
Setpoint=constrain(Setpoint,RPMmin,RPMmax);
}
}
}
////////////////////////////////////////////////////////////////////////////////////////////////
void Throttle_Reset()
{
myservo.write(0); // home servo
ControlMode=1;
digitalWrite(ClosedLoopLED, LOW);
digitalWrite(OpenLoopLED, HIGH);
Output=0;
Throttle=0;
Setpoint=0;
ESTOP=HIGH;
delay(2000);
}
////////////////////////////////////////////////////////////////////////////////////////////////
void ModeSwitch()
{
if (digitalRead(ModeSwitchPin) == LOW)
{
ModeSwitchCount++;
if(ModeSwitchCount >12)
{
ModeSwitchCount = 12;
}
}
else
{
ModeSwitchCount--;
if(ModeSwitchCount < -12)
{
ModeSwitchCount = -12;
}
}
if(ModeSwitchCount == 10)
{
ControlMode = 0;
digitalWrite(ClosedLoopLED, HIGH);
digitalWrite(OpenLoopLED, LOW);
}
if(ModeSwitchCount == -10)
{
ControlMode = 1;
digitalWrite(ClosedLoopLED, LOW);
digitalWrite(OpenLoopLED, HIGH);
}
}
////////////////////////////////////////////////////////////////////////////////////////////////
void CalculateRPM()
{
rpm = float(60.0*1000.0)/float((d_t))*float(rpmcount)/TicsPerRev;
total -= readings[index]; // subtract the last reading
readings[index] = rpm; // read from the sensor
total += readings[index]; // add the reading to the total
index = (index + 1); // advance to the next index
if (index >= NUMREADINGS) // if we're at the end of the array...
{
index = 0; // ...wrap around to the beginning
}
RPMaverage = total / NUMREADINGS; // calculate the average
}
////////////////////////////////////////////////////////////////////////////////////////////////
void PrintData()
{
if (DataFast == HIGH)
{
Serial.print(" time(s) ");
PIDtime = millis()/1000;
Serial.print(PIDtime);
Serial.print(" RPM ");
Serial.println(RPMaverage);
Serial.print(" Deriv ");
Serial.println(Derivative);
loopCount=0;
}
else
{
Serial.print("SetPt ");
Serial.print(Setpoint);
Serial.print(" Output ");
Serial.print(Output);
Serial.print(" Mode ");
Serial.print(ControlMode);
Serial.print(" time(s) ");
PIDtime = millis()/1000;
Serial.print(PIDtime);
Serial.print(" Integral ");
Serial.print(Integral);
Serial.print(" RPM ");
Serial.println(RPMaverage);
Serial.print(" Deriv ");
Serial.println(Derivative);
loopCount=0;
}
}
////////////////////////////////////////////////////////////////////////////////////////////////
void PresetSetpointAdjust()
{
if (ControlMode == 1)
{
Throttle = OpenLoopPresetSetpoint;
}
else
{
Integral=Output/Ki;
Setpoint=ClosedLoopPresetSetpoint; // ready for switch to closed loop mode
}
}
////////////////////////////////////////////////////////////////////////////////////////////////
void RunAwayAdjust()
{
if (Reacting==0)
{
ControlMode = 1;
digitalWrite(ClosedLoopLED, LOW);
digitalWrite(OpenLoopLED, HIGH);
Throttle = OpenLoopPresetSetpoint;//*.75;
Reacting = 1;
}
if (Reacting == 1)
{
Reacting = 0;
ControlMode = 0;
digitalWrite(ClosedLoopLED, HIGH);
digitalWrite(OpenLoopLED, LOW);
}
}
Comments (0)
You don't have permission to comment on this page.