First PID version
This commit is contained in:
parent
d113c1b1f7
commit
f379b9c34f
3 changed files with 68 additions and 43 deletions
|
|
@ -2,7 +2,7 @@
|
|||
#define MAIN_HPP
|
||||
|
||||
#define MACHINE_RESTART
|
||||
//#define DUMMY_DATA
|
||||
#define DUMMY_DATA
|
||||
|
||||
#define EEPROM_NAME_TICK "hall_tick"
|
||||
#define EEPROM_NAME_TIMESTAMP "timestamp"
|
||||
|
|
@ -17,6 +17,7 @@
|
|||
#include "eeprom.hpp"
|
||||
#include "oled.hpp"
|
||||
#include "capportal.hpp"
|
||||
#include <esp32-hal-timer.h>
|
||||
|
||||
#define HAS_MQTT
|
||||
#ifdef HAS_MQTT
|
||||
|
|
@ -98,6 +99,8 @@ const uint8_t HALL6_PIN = 26;
|
|||
const uint8_t HALL7_PIN = 27;
|
||||
const uint8_t HALL8_PIN = 14;
|
||||
|
||||
const uint16_t MIN_SPEED = 600;
|
||||
const uint16_t MAX_SPEED = 1200;
|
||||
const uint8_t MAX_ESC_SPEED = 60;
|
||||
const uint8_t MIN_ESC_SPEED = 35;
|
||||
const uint16_t MAX_POTI_VALUE = 4095;
|
||||
|
|
|
|||
|
|
@ -46,7 +46,7 @@ emyPxgcYxn/eR44/KJ4EBs+lVDR3veyJm+kXQ99b21/+jh5Xos1AnX5iItreGCc=
|
|||
#define MQTT_TRY_TO_CONNECT_MS 2000
|
||||
#define MQTT_CHANNEL "infinitymachine"
|
||||
#define MQTT_BUFFER_SIZE 1024
|
||||
#define SEND_MQTT_EVERY_MS 1000
|
||||
#define SEND_MQTT_EVERY_MS 10000
|
||||
|
||||
class Mymqtt
|
||||
{
|
||||
|
|
|
|||
|
|
@ -6,6 +6,20 @@ int last_speed;
|
|||
|
||||
time_t run_time = 0;
|
||||
time_t start_time = 0;
|
||||
hw_timer_t * timer = NULL;
|
||||
|
||||
volatile double integ = 0.0;
|
||||
volatile double derivative = 0.0;
|
||||
volatile double esc_output = 0.0;
|
||||
volatile double last_error = 0.0;
|
||||
int esc_value = 0;
|
||||
volatile double goal_speed = 1000; //1.0 * speed;
|
||||
volatile double current_speed = 0;
|
||||
volatile double err = 0;
|
||||
volatile double output = 0.0;
|
||||
const double kp = 0.01;
|
||||
const double ki = 0.001;
|
||||
const double kd = 0.0005;
|
||||
|
||||
void ISR_HALL1();
|
||||
void ISR_HALL2();
|
||||
|
|
@ -15,6 +29,7 @@ void ISR_HALL5();
|
|||
void ISR_HALL6();
|
||||
void ISR_HALL7();
|
||||
void ISR_HALL8();
|
||||
void ISR_onTimer();
|
||||
|
||||
void data_generate();
|
||||
void data_store();
|
||||
|
|
@ -56,6 +71,12 @@ void setup()
|
|||
ESC.write(0);
|
||||
}
|
||||
|
||||
// Initialise timer
|
||||
timer = timerBegin(0, 80, true); // Timer 0, Prescaler 80, zähle im Up-Modus
|
||||
timerAttachInterrupt(timer, &ISR_onTimer, true); // Setze den Interrupt-Handler und aktiviere den Interrupt auf steigende Flanke
|
||||
timerAlarmWrite(timer, 1000000, true); // Setze den Alarm auf 1/4 Sekunde und aktiviere ihn
|
||||
timerAlarmEnable(timer); // Aktiviere den Alarm
|
||||
|
||||
pinMode(39, INPUT_PULLDOWN);
|
||||
pinMode(HALL1_PIN, INPUT);
|
||||
pinMode(HALL2_PIN, INPUT);
|
||||
|
|
@ -66,7 +87,6 @@ void setup()
|
|||
pinMode(HALL7_PIN, INPUT);
|
||||
pinMode(HALL8_PIN, INPUT);
|
||||
|
||||
|
||||
//Serial.println("Init light sensor interrupt ...");
|
||||
//attachInterrupt(LS1_PIN, ISR_LS1, RISING);
|
||||
|
||||
|
|
@ -102,6 +122,8 @@ void loop()
|
|||
|
||||
display.show_values(speed, MIN_ESC_SPEED, MAX_ESC_SPEED, HallData, HALL_SENSORS_COUNT, run_time, capportal.localIP());
|
||||
|
||||
// Serial.printf("PID ... (goal: %06.3f, current: %06.3f, err: %06.3f, integ: %06.3f, derive: %06.3f, output=%06.3f), esc_output=%06.3f \n", goal_speed, current_speed, err, integ, derivative, output, esc_output);
|
||||
|
||||
}
|
||||
|
||||
void count_secs(time_t* run_time)
|
||||
|
|
@ -153,31 +175,12 @@ void speed_set()
|
|||
}
|
||||
sumDrehregler = sumDrehregler - maxDrehregler - minDrehregler;
|
||||
sumDrehregler = sumDrehregler/4;
|
||||
speed = map(sumDrehregler, 0, MAX_POTI_VALUE, MIN_ESC_SPEED, MAX_ESC_SPEED); // Der "MAP-" Befehl wandelt den Messwert aus der Variablen "Drehregler" um, damit er am ESC verarbeitet werden kann. Der Zahlenbereich 0 bis 1023 wird dabei in einen Zahlenwert zwischen 0 und 180 umgewandelt.
|
||||
/*
|
||||
if(abs(Drehregler - _Drehregler) < ((_Drehregler * JITTER_POTI_PERCENT)/100) )
|
||||
{
|
||||
_count_meas++;
|
||||
#ifdef DUMMY_DATA
|
||||
speed = 1000;
|
||||
#else
|
||||
speed = map(sumDrehregler, 0, MAX_POTI_VALUE, MIN_SPEED, MAX_SPEED); // Der "MAP-" Befehl wandelt den Messwert aus der Variablen "Drehregler" um, damit er am ESC verarbeitet werden kann. Der Zahlenbereich 0 bis 1023 wird dabei in einen Zahlenwert zwischen 0 und 180 umgewandelt.
|
||||
#endif
|
||||
}
|
||||
else
|
||||
{
|
||||
_count_meas = 0;
|
||||
}
|
||||
_Drehregler = Drehregler;
|
||||
|
||||
if(_count_meas >= 4)
|
||||
{
|
||||
_count_meas >= 0;
|
||||
speed = map(Drehregler, 0, MAX_POTI_VALUE, MIN_ESC_SPEED, MAX_ESC_SPEED); // Der "MAP-" Befehl wandelt den Messwert aus der Variablen "Drehregler" um, damit er am ESC verarbeitet werden kann. Der Zahlenbereich 0 bis 1023 wird dabei in einen Zahlenwert zwischen 0 und 180 umgewandelt.
|
||||
}
|
||||
*/
|
||||
}
|
||||
|
||||
if(MIN_ESC_SPEED < speed)
|
||||
ESC.write(speed); // Der endgültige Wert für den ESC wird an den ESC gesendet. Der ESC nimmt das Signal an dieser Stelle auf und steuert den Motor entsprechend der gesendeten Werte an.
|
||||
else
|
||||
ESC.write(0);
|
||||
|
||||
}
|
||||
|
||||
void data_init()
|
||||
|
|
@ -217,9 +220,9 @@ void data_init()
|
|||
|
||||
void data_check()
|
||||
{
|
||||
static unsigned long speedchanged_ms ;
|
||||
unsigned long current_millis = millis();
|
||||
|
||||
static unsigned long speedchanged_ms = current_millis ;
|
||||
/*
|
||||
Serial.printf("H1(pin %d): %d, H2(pin %d): %d, H3(pin %d): %d, H4(pin %d): %d, H5(pin %d) : %d, H6(pin %d) : %d, H7(pin %d) : %d, H8(pin %d) : %d.\n",
|
||||
HALL1_PIN, digitalRead(HALL1_PIN),
|
||||
HALL2_PIN, digitalRead(HALL2_PIN),
|
||||
|
|
@ -231,22 +234,17 @@ void data_check()
|
|||
HALL8_PIN, digitalRead(HALL8_PIN)
|
||||
);
|
||||
|
||||
*/
|
||||
if(current_millis - speedchanged_ms >= UPDATE_TURN_VALUES_EVERY_MS)
|
||||
{
|
||||
|
||||
|
||||
if(speed != last_speed)
|
||||
{
|
||||
last_speed = speed;
|
||||
speedchanged_ms = current_millis;
|
||||
HallData[0].period_ticks = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
if(current_millis - speedchanged_ms > UPDATE_TURN_VALUES_EVERY_MS)
|
||||
{
|
||||
speedchanged_ms = current_millis;
|
||||
HallData[0].value = (HallData[0].period_ticks * HallData[0].unit_factor);
|
||||
HallData[0].period_ticks = 0;
|
||||
Serial.printf("current_millis: %lu goal speed: %lu calc speed: %f (%lu ticks in %lu ms) \n", current_millis, speed, HallData[0].value, current_millis - speedchanged_ms);
|
||||
speedchanged_ms = current_millis;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
for(uint8_t nr = 1; nr < HALL_SENSORS_COUNT; nr++)
|
||||
{
|
||||
|
|
@ -346,13 +344,37 @@ void IRAM_ATTR ISR_HALL8()
|
|||
HallData[hallnr].last_update_ms = millis();
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
void IRAM_ATTR ISR_onTimer()
|
||||
{
|
||||
current_speed = HallData[0].value;
|
||||
err = goal_speed - current_speed;
|
||||
integ = integ + err;
|
||||
derivative = (err - last_error);
|
||||
output = kp * err + ki * integ + kd * derivative;
|
||||
|
||||
//Beschränkung der Ausgabe
|
||||
esc_output = constrain(output, MIN_ESC_SPEED, MAX_ESC_SPEED);
|
||||
//Serial.printf("PID ... (goal: %f, current: %06.3f, err: %06.3f, integ: %06.3f, derive: 06.3%f) output=%06.3f \n", goal_speed, current_speed, err, integ, derivative, output);
|
||||
|
||||
//Aktualisierung der letzten Fehler- und Zeitwerte
|
||||
last_error = err;
|
||||
|
||||
//Ansteuerung des Outputs
|
||||
|
||||
ESC.write(esc_output);
|
||||
}
|
||||
|
||||
//this code is just for generating dummy data - in normal use not needed
|
||||
void data_generate()
|
||||
{
|
||||
#ifdef DUMMY_DATA
|
||||
|
||||
static unsigned long _last_updated_ms = 0;
|
||||
unsigned long _wait_ms = map(speed, MIN_ESC_SPEED, MAX_ESC_SPEED, 60000/500/4, 60000/1500/4);
|
||||
speed = 1000 + random(-250,150);
|
||||
unsigned long _wait_ms = map( speed, MIN_SPEED, MAX_SPEED, 60000/MIN_SPEED/4, 60000/MAX_SPEED/4);
|
||||
//Serial.printf("Speed is : %lu and _wait_ms is %lu\n", speed, _wait_ms);
|
||||
|
||||
if(_last_updated_ms == 0)
|
||||
|
|
|
|||
Loading…
Add table
Reference in a new issue