See implemented code below.
It seems like the example code is stuck in the 'stabilizing' fase and as such keeps executing case _myPID.autoTune->AUTOTUNE:
Am i doing something wrong?
`/******************************************************************************
AutoTune Filter DIRECT Example
Circuit: https://github.com/Dlloydev/QuickPID/wiki/AutoTune_RC_Filter
******************************************************************************/
#include "QuickPID.h"
#include <EEPROM.h>
int motor_links = 3;
int motor_rechts = 5;
int motor_enable = 4;
int sensorpiny = A5;
// Tilt setting values
float Voutmin1;
float Voutplus1;
float Vout0G1;
float Voutmin2;
float Voutplus2;
float Vout0G2;
float sensitivityX;
float sensitivityY;
byte n = 4; // ADC OVERAMPLING SETTING ADC 10+4 bits
float sensorx_v = 0, sensory_v = 0;
float sensorx = 0.0, sensory = 0.0;
float filt = 0.3; // filter
const uint32_t sampleTimeUs = 100000; // 10ms
const byte inputPin = 0;
const byte outputPin = 3;
const int outputMax = 255;
const int outputMin = 0;
bool printOrPlotter = 1; // on(1) monitor, off(0) plotter
float POn = 1.0; // proportional on Error to Measurement ratio (0.0-1.0), default = 1.0
float DOn = 0.0; // derivative on Error to Measurement ratio (0.0-1.0), default = 0.0
byte outputStep = 5;
byte hysteresis = 1;
int setpoint = 300; // 1/3 of range for symetrical waveform
int output = 85; // 1/3 of range for symetrical waveform
float Input, Output, Setpoint=220.0;
float Kp = 0, Ki = 0, Kd = 0;
bool pidLoop = false;
QuickPID _myPID = QuickPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, POn, DOn, QuickPID::DIRECT);
void setup() {
Serial.begin(115200);
Serial.println("started");
pinMode(sensorpiny, INPUT);
pinMode (motor_links, OUTPUT);
pinMode (motor_rechts, OUTPUT);
pinMode (motor_enable, OUTPUT);
sensor_readsetup();
if (constrain(output, outputMin, outputMax - outputStep - 5) < output) {
Serial.println(F("AutoTune test exceeds outMax limit. Check output, hysteresis and outputStep values"));
while (1);
}
// Select one, reference: https://github.com/Dlloydev/QuickPID/wiki
//_myPID.AutoTune(tuningMethod::ZIEGLER_NICHOLS_PI);
_myPID.AutoTune(tuningMethod::ZIEGLER_NICHOLS_PID);
//_myPID.AutoTune(tuningMethod::TYREUS_LUYBEN_PI);
//_myPID.AutoTune(tuningMethod::TYREUS_LUYBEN_PID);
//_myPID.AutoTune(tuningMethod::CIANCONE_MARLIN_PI);
//_myPID.AutoTune(tuningMethod::CIANCONE_MARLIN_PID);
//_myPID.AutoTune(tuningMethod::AMIGOF_PID);
//_myPID.AutoTune(tuningMethod::PESSEN_INTEGRAL_PID);
//_myPID.AutoTune(tuningMethod::SOME_OVERSHOOT_PID);
//_myPID.AutoTune(tuningMethod::NO_OVERSHOOT_PID);
_myPID.autoTune->autoTuneConfig(outputStep, hysteresis, setpoint, output, QuickPID::DIRECT, printOrPlotter, sampleTimeUs);
}
void loop() {
if (_myPID.autoTune) // Avoid dereferencing nullptr after _myPID.clearAutoTune()
{
switch (_myPID.autoTune->autoTuneLoop()) {
case _myPID.autoTune->AUTOTUNE:
float raw = sensor_read(sensorpiny);
sensory_v = (filt * raw) + ((1.0 - filt) * sensory_v);
// sensory = ((sensory_v - Vout0G1) / sensitivityY) * 5;
Input = round (sensory_v*100.0);
// Serial.print ("Setpoint=");
// Serial.print (Setpoint);
// Serial.print (" || Input =");
// Serial.print (Input);
// Serial.print (" || Output=");
// Serial.println (Output);
if (Input < Setpoint) {
analogWrite(motor_rechts, Output);
analogWrite(motor_links, 0);
digitalWrite(motor_enable,HIGH);
}
else if (Input > Setpoint ) {
analogWrite(motor_rechts, 0);
analogWrite(motor_links, Output);
digitalWrite(motor_enable,HIGH);
}
else { analogWrite(motor_rechts, 0);
analogWrite(motor_links, 0);
digitalWrite(motor_enable,LOW);}
break;
case _myPID.autoTune->TUNINGS:
Serial.println("TUNING");
_myPID.autoTune->setAutoTuneConstants(&Kp, &Ki, &Kd); // set new tunings
_myPID.SetMode(QuickPID::AUTOMATIC); // setup PID
_myPID.SetSampleTimeUs(sampleTimeUs);
_myPID.SetTunings(Kp, Ki, Kd, POn, DOn); // apply new tunings to PID
Setpoint = 300.0;
break;
case _myPID.autoTune->CLR:
if (!pidLoop) {
_myPID.clearAutoTune(); // releases memory used by AutoTune object
pidLoop = true;
}
break;
}
}
if (pidLoop) {
if (printOrPlotter == 0) { // plotter
Serial.print("Setpoint:"); Serial.print(Setpoint); Serial.print(",");
Serial.print("Input:"); Serial.print(Input); Serial.print(",");
Serial.print("Output:"); Serial.print(Output); Serial.println(",");
}
float raw = sensor_read(sensorpiny);
sensory_v = (filt * raw) + ((1.0 - filt) * sensory_v);
// sensory = ((sensory_v - Vout0G1) / sensitivityY) * 5;
Input = sensory_v;
_myPID.Compute();
if (Input < Setpoint) {
analogWrite(motor_rechts, Output);
analogWrite(motor_links, 0);
digitalWrite(motor_enable,HIGH);
}
else if (Input > Setpoint ) {
analogWrite(motor_rechts, 0);
analogWrite(motor_links, Output);
digitalWrite(motor_enable,HIGH);
}
else { analogWrite(motor_rechts, 0);
analogWrite(motor_links, 0);
digitalWrite(motor_enable,LOW);}
}
}
float avg(int inputVal) {
static int arrDat[16];
static int pos;
static long sum;
pos++;
if (pos >= 16) pos = 0;
sum = sum - arrDat[pos] + inputVal;
arrDat[pos] = inputVal;
return (float)sum / 16.0;
}
// =============================================== FUNCTIONS ====================================================================
void loadsettings() { // load saved calibrate settings on start up
Voutmin1 = ( word(EEPROM.read(1), EEPROM.read(2))) / 1000.0;
Vout0G1 = ( word(EEPROM.read(3), EEPROM.read(4))) / 1000.0;
Voutplus1 = ( word(EEPROM.read(5), EEPROM.read(6))) / 1000.0;
Voutmin2 = (word(EEPROM.read(7), EEPROM.read(8))) / 1000.0;
Vout0G2 = (word(EEPROM.read(9), EEPROM.read(10))) / 1000.0;
Voutplus2 = (word(EEPROM.read(11), EEPROM.read(12))) / 1000.0;
// tilt sensor sensivity berekening.
sensitivityY = (Voutplus1 - Voutmin1) / 2;
sensitivityX = (Voutplus2 - Voutmin2) / 2;
}
float fSamples;
int samples;
void sensor_readsetup() { // ADc sensor setup
fSamples = pow(4, (float) n);
samples = (int)(fSamples + 0.5); // solves : when n = 3 get get pow(4,n) of 63
}
float sensor_read(byte input) { // Read sensor
// Decimation for Arduino ADC A0, averaged then decimated.
long dv = 0;
long derp = 0;
derp = millis();
for (byte avg = 0; avg < 2; avg++)
for (int j = 0; j < samples; j++) {
dv += analogRead(input); // analogRead(A0);
}
dv = (dv / 2); // average calc.
dv = dv >> n; // decimated
float Volt = dv * (5.0 / 16384.0); // conversion to voltage
return Volt;
}
`