Pages: [1]   Go Down
Author Topic: HDD needle PID controller 2 PIDs 1 working  (Read 482 times)
0 Members and 1 Guest are viewing this topic.
Offline Offline
Newbie
*
Karma: 0
Posts: 1
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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:
#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:
#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;
}
Logged

Pages: [1]   Go Up
Jump to: