adapted PID
This commit is contained in:
jenoack
parent
f379b9c34f
commit
3a69935127
2 changed files with 46 additions and 28 deletions
|
|
@ -1,8 +1,8 @@
|
|||
#ifndef MAIN_HPP
|
||||
#define MAIN_HPP
|
||||
|
||||
#define MACHINE_RESTART
|
||||
#define DUMMY_DATA
|
||||
//#define MACHINE_RESTART
|
||||
//#define DUMMY_DATA
|
||||
|
||||
#define EEPROM_NAME_TICK "hall_tick"
|
||||
#define EEPROM_NAME_TIMESTAMP "timestamp"
|
||||
|
|
@ -19,7 +19,7 @@
|
|||
#include "capportal.hpp"
|
||||
#include <esp32-hal-timer.h>
|
||||
|
||||
#define HAS_MQTT
|
||||
//#define HAS_MQTT
|
||||
#ifdef HAS_MQTT
|
||||
#include "mqtt.hpp"
|
||||
#include "json.hpp"
|
||||
|
|
@ -78,7 +78,7 @@ const char *Data_units[ALL_DATA_COUNT] =
|
|||
const float Data_units_factor[ALL_DATA_COUNT] =
|
||||
{
|
||||
//Hall sensors
|
||||
(60000.0/UPDATE_TURN_VALUES_EVERY_MS)/HALL_TICKS_PER_TURN, 1.0/HALL_TICKS_PER_TURN, 1.0/HALL_TICKS_PER_TURN, 1.0/HALL_TICKS_PER_TURN, 1.0/HALL_TICKS_PER_TURN, 1.0/HALL_TICKS_PER_TURN, 1.0/HALL_TICKS_PER_TURN, 1.0/HALL_TICKS_PER_TURN
|
||||
60000.0, 1.0/HALL_TICKS_PER_TURN, 1.0/HALL_TICKS_PER_TURN, 1.0/HALL_TICKS_PER_TURN, 1.0/HALL_TICKS_PER_TURN, 1.0/HALL_TICKS_PER_TURN, 1.0/HALL_TICKS_PER_TURN, 1.0/HALL_TICKS_PER_TURN
|
||||
};
|
||||
|
||||
const char ** Hall_names = Data_names;
|
||||
|
|
|
|||
|
|
@ -59,8 +59,7 @@ void setup()
|
|||
#endif
|
||||
|
||||
Store.begin();
|
||||
data_init();
|
||||
|
||||
|
||||
Serial.println("Init ETC ...");
|
||||
ESC.attach(ESC_PIN,1000,2000);
|
||||
|
||||
|
|
@ -71,6 +70,8 @@ void setup()
|
|||
ESC.write(0);
|
||||
}
|
||||
|
||||
data_init();
|
||||
|
||||
// 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
|
||||
|
|
@ -113,17 +114,17 @@ void loop()
|
|||
|
||||
count_secs(&run_time);
|
||||
speed_set();
|
||||
data_check();
|
||||
//data_check();
|
||||
data_store();
|
||||
|
||||
#ifdef HAS_MQTT
|
||||
mqtt.send_and_loop("infinity/data", json.create_json_string(AllData, ALL_DATA_COUNT, run_time));
|
||||
#endif
|
||||
|
||||
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);
|
||||
display.show_values(speed, MIN_SPEED, MAX_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);
|
||||
ESC.write(50);
|
||||
}
|
||||
|
||||
void count_secs(time_t* run_time)
|
||||
|
|
@ -199,6 +200,7 @@ void data_init()
|
|||
#endif
|
||||
last_speed = 0;
|
||||
speed = 0;
|
||||
unsigned long current_millis = millis();
|
||||
for (uint8_t i = 0; i < ALL_DATA_COUNT; i ++)
|
||||
{
|
||||
AllData[i].name = Data_names[i];
|
||||
|
|
@ -208,16 +210,21 @@ void data_init()
|
|||
AllData[i].period_ticks = 0;
|
||||
#ifdef MACHINE_RESTART
|
||||
AllData[i].ticks = 0;
|
||||
AllData[i].timestamp = 0;
|
||||
#else
|
||||
AllData[i].ticks = Store.get_int(EEPROM_NAME_TICK+String(i));
|
||||
AllData[i].timestamp = Store.get_int(EEPROM_NAME_TIMESTAMP+String(i));
|
||||
AllData[i].timestamp = Store.get_int(EEPROM_NAME_TIMESTAMP+String(i));
|
||||
if(i != 0)
|
||||
{
|
||||
AllData[i].value = (AllData[i].ticks * AllData[i].unit_factor);
|
||||
}
|
||||
#endif
|
||||
AllData[i].last_update_ms = 0;
|
||||
AllData[i].timestamp = 0;
|
||||
AllData[i].last_update_ms = current_millis;
|
||||
|
||||
}
|
||||
delay(500);
|
||||
}
|
||||
|
||||
/*
|
||||
void data_check()
|
||||
{
|
||||
unsigned long current_millis = millis();
|
||||
|
|
@ -235,23 +242,23 @@ void data_check()
|
|||
);
|
||||
|
||||
*/
|
||||
|
||||
/*
|
||||
if(current_millis - speedchanged_ms >= UPDATE_TURN_VALUES_EVERY_MS)
|
||||
{
|
||||
|
||||
|
||||
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++)
|
||||
{
|
||||
HallData[nr].value = (HallData[nr].ticks * HallData[nr].unit_factor);
|
||||
HallData[nr].period_ticks = 0;
|
||||
}
|
||||
}
|
||||
*/
|
||||
|
||||
void data_store()
|
||||
{
|
||||
|
|
@ -278,70 +285,81 @@ void IRAM_ATTR ISR_HALL1()
|
|||
HallData[hallnr].ticks++;
|
||||
HallData[hallnr].period_ticks++;
|
||||
HallData[hallnr].timestamp = run_time;
|
||||
HallData[hallnr].last_update_ms = millis();
|
||||
// each 4 ticks is one turn
|
||||
// so we calculate speed each 4 ticks... just in case we check for bigger ...
|
||||
/*
|
||||
if(HallData[hallnr].period_ticks >= HALL_TICKS_PER_TURN){
|
||||
HallData[hallnr].period_ticks = 0;
|
||||
if(HallData[hallnr].last_update_ms != 0)
|
||||
{
|
||||
HallData[hallnr].value = HallData[hallnr].unit_factor / HallData[hallnr].last_update_ms;
|
||||
}
|
||||
HallData[hallnr].last_update_ms = millis();
|
||||
}
|
||||
*/
|
||||
}
|
||||
|
||||
void IRAM_ATTR ISR_HALL2()
|
||||
{
|
||||
const uint8_t hallnr = 1;
|
||||
HallData[hallnr].ticks++;
|
||||
HallData[hallnr].period_ticks++;
|
||||
HallData[hallnr].timestamp = run_time;
|
||||
HallData[hallnr].last_update_ms = millis();
|
||||
//HallData[hallnr].value = (HallData[hallnr].ticks * HallData[hallnr].unit_factor);
|
||||
}
|
||||
|
||||
void IRAM_ATTR ISR_HALL3()
|
||||
{
|
||||
const uint8_t hallnr = 2;
|
||||
HallData[hallnr].ticks++;
|
||||
HallData[hallnr].period_ticks++;
|
||||
HallData[hallnr].timestamp = run_time;
|
||||
HallData[hallnr].last_update_ms = millis();
|
||||
//HallData[hallnr].value = (HallData[hallnr].ticks * HallData[hallnr].unit_factor);
|
||||
}
|
||||
|
||||
void IRAM_ATTR ISR_HALL4()
|
||||
{
|
||||
const uint8_t hallnr = 3;
|
||||
HallData[hallnr].ticks++;
|
||||
HallData[hallnr].period_ticks++;
|
||||
HallData[hallnr].timestamp = run_time;
|
||||
HallData[hallnr].last_update_ms = millis();
|
||||
//HallData[hallnr].value = (HallData[hallnr].ticks * HallData[hallnr].unit_factor);
|
||||
}
|
||||
|
||||
void IRAM_ATTR ISR_HALL5()
|
||||
{
|
||||
const uint8_t hallnr = 4;
|
||||
HallData[hallnr].ticks++;
|
||||
HallData[hallnr].period_ticks++;
|
||||
HallData[hallnr].timestamp = run_time;
|
||||
HallData[hallnr].last_update_ms = millis();
|
||||
//HallData[hallnr].value = (HallData[hallnr].ticks * HallData[hallnr].unit_factor);
|
||||
}
|
||||
|
||||
void IRAM_ATTR ISR_HALL6()
|
||||
{
|
||||
const uint8_t hallnr = 5;
|
||||
HallData[hallnr].ticks++;
|
||||
HallData[hallnr].period_ticks++;
|
||||
HallData[hallnr].timestamp = run_time;
|
||||
HallData[hallnr].last_update_ms = millis();
|
||||
//HallData[hallnr].value = (HallData[hallnr].ticks * HallData[hallnr].unit_factor);
|
||||
}
|
||||
|
||||
void IRAM_ATTR ISR_HALL7()
|
||||
{
|
||||
const uint8_t hallnr = 6;
|
||||
HallData[hallnr].ticks++;
|
||||
HallData[hallnr].period_ticks++;
|
||||
HallData[hallnr].timestamp = run_time;
|
||||
HallData[hallnr].last_update_ms = millis();
|
||||
//HallData[hallnr].value = (HallData[hallnr].ticks * HallData[hallnr].unit_factor);
|
||||
}
|
||||
|
||||
void IRAM_ATTR ISR_HALL8()
|
||||
{
|
||||
const uint8_t hallnr = 7;
|
||||
HallData[hallnr].ticks++;
|
||||
HallData[hallnr].period_ticks++;
|
||||
HallData[hallnr].timestamp = run_time;
|
||||
HallData[hallnr].last_update_ms = millis();
|
||||
//HallData[hallnr].value = (HallData[hallnr].ticks * HallData[hallnr].unit_factor);
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -356,15 +374,15 @@ void IRAM_ATTR ISR_onTimer()
|
|||
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);
|
||||
esc_output = MIN_ESC_SPEED + constrain(output, 0, MAX_ESC_SPEED - MIN_ESC_SPEED);
|
||||
//Serial.printf("PID ... (goal: %f, current: %06.3f, err: %06.3f, integ: %06.3f, derive: %06.3f) 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);
|
||||
//ESC.write(50);
|
||||
}
|
||||
|
||||
//this code is just for generating dummy data - in normal use not needed
|
||||
|
|
|
|||
Loading…
Add table
Reference in a new issue