Control PI + rampa

  /*

* 25 september
* PID control
*/

#include <TimerOne.h>
#include <PID_v1.h>

#define a 5
#define encoder0PinA 2
#define encoder0PinB 3
volatile long encoder0Pos=0; //alias pulses

double setPoint=60; //rpm max 40 for current constraints

double rpm;
float Dangle;
double mvolt;

const int enPinmot1 = 5; // H-Bridge enable pin
const int in1Pinmot1 = 7; // H-Bridge input pins
const int in2Pinmot1 = 4;

//tuning parameters
// 131:1
/*
const double Kp=0.6;
const double Kd=0;
const double Ki=1.5;
*/
const double Kp=0.6;
const double Kd=0;
const double Ki=2.0;

//manual tuning:
//1) increase K_p until oscillation
//2) increase K_d until oscillation disappears
//3) increase K_i until the steady state error goes to zero

//Load a new PID controller
PID myPID(&rpm, &mvolt, &setPoint, Kp, Ki, Kd, DIRECT);
const long sampleRate = 50;
// frequency of the digital controller

unsigned long currentTime;
unsigned long loopTime;
long loopPulses;

long isr_time=50000;//us

const int pulse_per_rev = 8384;

void setup()
{
  pinMode(in1Pinmot1, OUTPUT);
  pinMode(in2Pinmot1, OUTPUT);
  //pinMode(in1Pinmot2, OUTPUT);
  //pinMode(in2Pinmot2, OUTPUT);

  pinMode(encoder0PinA, INPUT);
  pinMode(encoder0PinB, INPUT);

  attachInterrupt(0, doEncoderA, CHANGE);
  attachInterrupt(1, doEncoderB, CHANGE);

    //turn the PID on
   myPID.SetMode(AUTOMATIC);
   myPID.SetSampleTime(sampleRate);

   //myPID.SetOutputLimits(0,12.34);
  myPID.SetOutputLimits(0,10);
  //here call the variables for optical encoder???

  Timer1.initialize(isr_time);
  Timer1.attachInterrupt(timerISR);

  loopPulses=encoder0Pos;

  Serial.begin(115200);


}

double setPoint1=30;
double setPoint2=60;
double v;
double t=0.0;

void loop()
{
  if (v<setPoint2) {
    v= setPoint1 + a*t;
    t+= 1;
    setPoint= v;
    delay (1);
    }
  /*
    currentTime=millis();
  //200 Hz
  if (currentTime>=(loopTime+500)){
    Serial.println(pulses);
   loopTime=currentTime;//refresh looptime
  }// end current time
  */
  //start loop block (without ISR)
  /*
  currentTime=millis();
  // Hz
  if (currentTime>=(loopTime+1000)){
      Dangle=encoder0Pos-loopPulses;
      Serial.println(Dangle);
      loopPulses=encoder0Pos;// refresh pulses
      loopTime=currentTime;//refresh looptime
    }//end current time
  //end loop block
    */

  myPID.Compute(); // this function updates the control action.

    int mvoltbit=map(mvolt,0,10,0,255);

  analogWrite(enPinmot1, mvoltbit);
//analogWrite(enPinmot2, speed);

digitalWrite(in1Pinmot1,LOW);
digitalWrite(in2Pinmot1,HIGH);

}//end loop

void doEncoderA(){
  if (digitalRead(encoder0PinA)==HIGH){
    if (digitalRead(encoder0PinB)==LOW){
      encoder0Pos++;
    }
    else{
      encoder0Pos--; //CCW
    }
  }
  else
  {
    if (digitalRead(encoder0PinB)==HIGH){
      encoder0Pos++;
    }
    else{
      encoder0Pos--;
    }
  }
  //Serial.println(encoder0Pos);
}

void doEncoderB(){
   if (digitalRead(encoder0PinB)==HIGH){
      if (digitalRead(encoder0PinA)==HIGH){
        encoder0Pos++;
        }
      else{
        encoder0Pos--;
        }
    }
    else{
       if (digitalRead(encoder0PinA)==LOW){
           encoder0Pos++;
         }
         else{
           encoder0Pos--;
         }
     }
      //Serial.println(encoder0Pos);
   }

void timerISR(){
    Dangle=(encoder0Pos-loopPulses)* (2*PI/ pulse_per_rev);//rad
    //float Dangle=(pulses-loopPulses);
    rpm=(Dangle/isr_time)*(30000000.0/PI);
    //Serial.println(Dangle);

     Serial.print(rpm);

  Serial.print("\t");
  Serial.print(0);
  Serial.print("\t");
  Serial.print(0);
  Serial.print("\t");
  Serial.print(0);
  Serial.print("\t");
  Serial.print(0);
  Serial.print("\t");
  Serial.println(0);



   loopPulses=encoder0Pos;// refresh pulses
  }