| 
  • If you are citizen of an European Union member nation, you may not use this service unless you are at least 16 years old.

  • Social distancing? Try a better way to work remotely on your online files. Dokkio, a new product from PBworks, can help your team find, organize, and collaborate on your Drive, Gmail, Dropbox, Box, and Slack files. Sign up for free.

View
 

Arduino_PID

Page history last edited by mckgyver@... 10 years, 10 months ago

 

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.