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 !
#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
#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;
}