Go Down

Topic: HDD needle PID controller 2 PIDs 1 working (Read 578 times) previous topic - next topic

rinze

Hello there,

Im trying to control an HDD needle. I made a PID current controller to control the current trough the coil of the needle. as postion sensor i''m using a "transmissive optical sensor" (tcst2103) the more licht the needle block less current flows throucht the fototransistor. passing the current through a resistor gives a voltage that can be measured.

the follwing is my problem. after i got the current PID working i wanted to add the position PID i can eddit the tunings (i know this because getkp() works). But strangly is does not giv an output. i added the code in the attachment. does anybody know the problem ?

under this follows the code

thanks in advance !

Code: [Select]
#include <PID_v1.h>

double posSetpoint = 0;
double posInput = 0;
double posOutput = 0;

double curSetpoint = 0;
double curInput = 0;
double curOutput = 0;


double positionkp = 0;
double positionki = 0;
double positionkd = 0;

double currentkp = 0;
double currentki = 0;
double currentkd = 0;


#define  currentkpAdress 0
#define  currentkiAdress 4
#define  currentkdAdress 8

#define  positionkpAdress 12
#define  positionkiAdress 16
#define  positionkdAdress 20



PID positionPID(&posInput, &posOutput, &posSetpoint, positionkp, positionki, positionkd, DIRECT);
PID  currentPID(&curInput, &curOutput, &curSetpoint,  currentkp,  currentki,  currentkd, DIRECT);

long previousMillis = 0;
long interval = 2000;

void setup()
{
  pinMode(3, OUTPUT);
  pinMode(12, OUTPUT);

  positionPID.SetSampleTime(5);
  positionPID.SetOutputLimits(-150, 150); //the values chosen so that the voicecoil will ont be overloaded with curent 203 ~= 0.185A the range is chosen below the maximum for safety
  positionPID.SetMode(AUTOMATIC);

  currentPID.SetSampleTime(1);
  currentPID.SetOutputLimits(0, 255); //the values chosen to be in the range of analogwrite wich has the exactly the same range as the current PIDcontroller
  currentPID.SetMode(AUTOMATIC);

  Serial.begin(115200);

  currentkp = EEPROMReadDouble(currentkpAdress);
  currentki = EEPROMReadDouble(currentkiAdress);
  currentkd = EEPROMReadDouble(currentkdAdress);

  positionkp = EEPROMReadDouble(positionkpAdress);
  positionki = EEPROMReadDouble(positionkiAdress);
  positionkd = EEPROMReadDouble(positionkdAdress);

  positionPID.SetTunings(positionkp, positionki, positionkd);
   currentPID.SetTunings( currentkp,  currentki,  currentkd);

  analogReference(INTERNAL);

  TCCR2B = TCCR2B & 0b11111000 | 0x01;  //this modifies the PWM frequency of the arduino
}

void loop()
{
  positionLoop(); //this loop regulates the postion of the needle, thus the setpoint for the currentPID control loop
  currentLoop(); //this loop regulates the current going through the voicecoil of the needle, thus the setpoint for the currentPID control loop
  serialcomm();

  positionPID.Compute();
  currentPID.Compute();
 
 
   //this is a interval secuence for debugging. it makes te setpoint vary per interval
  unsigned long currentMillis = millis();

  if(currentMillis - previousMillis > interval)
  {
    previousMillis = currentMillis; 

    if (posSetpoint == 500)
    {
      posSetpoint = 100;
    }
    else
    {
      posSetpoint = 500;
    }
    /*
    Serial.println("current setpoint");
    Serial.println(curSetpoint, DEC);
   
    Serial.println("current output");
    Serial.println(curOutput, DEC);
   
    Serial.println("current input");
    Serial.println(curInput, DEC);
    Serial.println("");
    */
    //---------------------------------------------------
   
    Serial.println("position setpoint");
    Serial.println(posSetpoint, DEC);
   
    Serial.println("position output");
    Serial.println(posOutput, DEC);
   
    Serial.println("position input");
    Serial.println(posInput, DEC);
    Serial.println("");
  }
}


void currentLoop()
{
  curInput = analogRead(2) -82;
  analogWrite(3, abs(curOutput));
  digitalWrite(12, HIGH);
}


void positionLoop()
{
//  posInput = analogRead(5);
  posInput = 300;


  if(posOutput < 0)
  {
   digitalWrite(12, LOW);
  }
  if(posOutput > 0)
  {
    digitalWrite(12, HIGH);
  }

  //curSetpoint = abs(posOutput);
}


this is the Serialcomm part, this allows me to eddit the parameters via the terminal. and saves them in the EEprom

Code: [Select]
#include <EEPROM.h>

int PIDidentifier = 0;

#define  noPID  0
#define posPID  1 //position PID controller
#define curPID  2 //current PID controller

#define  currentkpAdress 0
#define  currentkiAdress 4
#define  currentkdAdress 8

#define  positionkpAdress 12
#define  positionkiAdress 16
#define  positionkdAdress 20


void serialcomm()
{
  if (Serial.available() > 0)
  {
    char recstring[8] ="     ";
    char numberstring[7] = "    ";
    Serial.readBytes(recstring, 8); // p10.000
   
    for(int i; i<=4; i++)
    {
      numberstring[i] = recstring[i+1];
    }
   
    double number = atof(numberstring);
   
   
    if(recstring[0] == 'S' && recstring[1] == '-' && recstring[2] == 'p' && recstring[3] == 'o' && recstring[4] == 's')
    {
      Serial.print("setting");
      Serial.println("values in positionPID");
      PIDidentifier = posPID;
    }
    if(recstring[0] == 'S' && recstring[1] == '-' && recstring[2] == 'c' && recstring[3] == 'u' && recstring[4] == 'r')
    {
      Serial.print("setting");
      Serial.println("values in currentPID");
      PIDidentifier = curPID;
    }
    if(recstring[0] == 'p')
    {
      Serial.print("p");
      Serial.println(number, DEC);
      Serial.println("-");
      if(PIDidentifier == posPID)
      {
        positionkp = number;
      }
      if(PIDidentifier == curPID)
      {
        currentkp = number;
      }   
    }
    if(recstring[0] == 'i')
    {
      Serial.print("i");
      Serial.println(number, DEC);
      Serial.println("-");
      if(PIDidentifier == posPID)
      {
        positionki = number;
      }
      if(PIDidentifier == curPID)
      {
        currentki = number;
      }
    }
    if(recstring[0] == 'd')
    {
      Serial.print("d");
      Serial.println(number, DEC);
      Serial.println("-");
      if(PIDidentifier == posPID)
      {
        positionkd = number;
      }
      if(PIDidentifier == curPID)
      {
        currentkd = number;
      }
    }
    if(recstring[0] == 'G' && recstring[1] == '-' && recstring[2] == 'p' && recstring[3] == 'o' && recstring[4] == 's')
    {
      double poskp = positionPID.GetKp();
      double poski = positionPID.GetKi();
      double poskd = positionPID.GetKd();
     
      Serial.println("");
      Serial.println("PID value of PositionPID:");
      Serial.print("p");
      Serial.print(poskp, DEC);
      Serial.println("");
      Serial.print("i");
      Serial.print(poski, DEC);
      Serial.println("");
      Serial.print("d");
      Serial.print(poskd, DEC);
      Serial.println("");
      Serial.println("");
      Serial.println("-");
    }
    if(recstring[0] == 'G' && recstring[1] == '-' && recstring[2] == 'c' && recstring[3] == 'u' && recstring[4] == 'r')
    {
      double curkp = currentPID.GetKp();
      double curki = currentPID.GetKi();
      double curkd = currentPID.GetKd();
     
      Serial.println("");
      Serial.println("PID value of CurrentPID:");
      Serial.print("p");
      Serial.print(curkp, DEC);
      Serial.println("");
      Serial.print("i");
      Serial.print(curki, DEC);
      Serial.println("");
      Serial.print("d");
      Serial.print(curkd, DEC);
      Serial.println("");
      Serial.println("");
      Serial.println("-");
    }
    if(PIDidentifier == posPID)
    {
      positionPID.SetTunings(positionkp, positionki, positionkd);
    }
    if(PIDidentifier == curPID)
    {
      currentPID.SetTunings(currentkp, currentki, currentkd);
    }
   
    EEPROMWriteDouble(positionkpAdress, positionkp);
    EEPROMWriteDouble(positionkiAdress, positionki);
    EEPROMWriteDouble(positionkdAdress, positionkd);
   
    EEPROMWriteDouble(currentkpAdress, currentkp);
    EEPROMWriteDouble(currentkiAdress, currentki);
    EEPROMWriteDouble(currentkdAdress, currentkd);

   

  }
}

void EEPROMWriteDouble(int ee, double value)
{
  byte* p = (byte*)(void*)&value;
  for (int i = 0; i < sizeof(value); i++)
  {
    EEPROM.write(ee++, *p++);
  }
}

float EEPROMReadDouble(int ee)
{
  double value = 0.0;
  byte* p = (byte*)(void*)&value;
  for (int i = 0; i < sizeof(value); i++)
  {
    *p++ = EEPROM.read(ee++);
  }
  return value;
}

Go Up
 


Please enter a valid email to subscribe

Confirm your email address

We need to confirm your email address.
To complete the subscription, please click the link in the email we just sent you.

Thank you for subscribing!

Arduino
via Egeo 16
Torino, 10131
Italy