Add heater with PID
Add heating, PID Library, autotune PID TODO: check autotune PID work
This commit is contained in:
parent
d02a4f0602
commit
b7c022e2f2
|
@ -1,3 +1,6 @@
|
|||
#if !defined(ESP8266)
|
||||
#error This code is designed to run on ESP8266 and ESP8266-based boards! Please check your Tools->Board setting.
|
||||
#endif
|
||||
#include <Wire.h>
|
||||
//// Not work with Gyver Lib - EncButton
|
||||
//const int sda=D1, scl=D2;
|
||||
|
@ -24,6 +27,7 @@ hd44780_I2Cexp lcd; // declare lcd object and let it auto-configure everything.
|
|||
#define DIR_PIN D8 // thermistor pin
|
||||
#define EN_PIN D6 // thermistor pin
|
||||
|
||||
#define MOS_PIN D0
|
||||
// Make custom characters:
|
||||
byte motor_char_1[] = {
|
||||
B00111,
|
||||
|
@ -59,42 +63,43 @@ GyverNTC therm(T_PIN, 100000, 3950, 25, 8890);
|
|||
#include <EncButton2.h>
|
||||
EncButton2<EB_BTN> btn[BTN_AMOUNT];
|
||||
|
||||
//enable stepper
|
||||
//#define GS_NO_ACCEL
|
||||
// отключить модуль движения с ускорением (уменьшить вес кода)
|
||||
#include <GyverStepper2.h>
|
||||
GStepper2<STEPPER2WIRE> stepper(STEPS, STEP_PIN, DIR_PIN, EN_PIN);
|
||||
|
||||
#if !defined(ESP8266)
|
||||
#error This code is designed to run on ESP8266 and ESP8266-based boards! Please check your Tools->Board setting.
|
||||
#endif
|
||||
|
||||
// These define's must be placed at the beginning before #include "ESP8266TimerInterrupt.h"
|
||||
// _TIMERINTERRUPT_LOGLEVEL_ from 0 to 4
|
||||
// Don't define _TIMERINTERRUPT_LOGLEVEL_ > 0. Only for special ISR debugging only. Can hang the system.
|
||||
#define TIMER_INTERRUPT_DEBUG 0
|
||||
#define _TIMERINTERRUPT_LOGLEVEL_ 0
|
||||
|
||||
// Select a Timer Clock
|
||||
#define USING_TIM_DIV1 false // for shortest and most accurate timer
|
||||
#define USING_TIM_DIV16 false // for medium time and medium accurate timer
|
||||
#define USING_TIM_DIV256 true // for longest timer but least accurate. Default
|
||||
|
||||
#include "ESP8266TimerInterrupt.h"
|
||||
volatile uint32_t lastMillis = 0;
|
||||
|
||||
#define TIMER_INTERVAL_MS 1000
|
||||
|
||||
// Init ESP8266 timer 1
|
||||
ESP8266Timer ITimer;
|
||||
|
||||
|
||||
#define P 16.67
|
||||
#define I 0.75
|
||||
#define D 91.91
|
||||
#define DT 10
|
||||
//PID regulator
|
||||
#include "GyverPID.h"
|
||||
GyverPID regulator(P, I, D, DT); // коэф. П, коэф. И, коэф. Д, период дискретизации dt (мс)
|
||||
//Autotune PID
|
||||
#include "PIDtuner.h"
|
||||
PIDtuner tuner;
|
||||
|
||||
//define vars
|
||||
int max_speed = STEPS; //3600 degrees in 10 seconds = 10 rotate/seconds
|
||||
int max_speed = STEPS;
|
||||
int cursor=1; //позиция курсора
|
||||
int t_current=230;
|
||||
int t_current=25;
|
||||
// int t_current_temp=230;
|
||||
int t_set=230;
|
||||
int t_set=25;
|
||||
// int t_set_temp=230;
|
||||
int motor_speed=100;
|
||||
// int motor_speed_temp=100;
|
||||
|
@ -108,10 +113,10 @@ String motor_dir_text = "FWD";
|
|||
// String motor_dir_temp_text = "FWD";
|
||||
|
||||
int save=100; //режим работы меню. 100- режим выбора. 1,2,3 - выбранное значение
|
||||
float filT = 0; //фильтрованное значение датчика
|
||||
|
||||
long previousMillis = 0; // храним время последнего переключения светодиода
|
||||
long interval = 600; // интервал между включение/выключением светодиода (1 секунда)
|
||||
|
||||
int autotune=0;
|
||||
//===========Stepping in timer interrupts==================================
|
||||
void IRAM_ATTR TimerHandler()
|
||||
{
|
||||
|
@ -121,11 +126,12 @@ void IRAM_ATTR TimerHandler()
|
|||
}
|
||||
}
|
||||
|
||||
|
||||
//=====================================================================
|
||||
void setup(){
|
||||
Serial.begin(115200);
|
||||
//stepper.setSpeed(0);
|
||||
// Interval in microsecs
|
||||
|
||||
// Start interrupts timer. Interval in microsecs
|
||||
if (ITimer.attachInterruptInterval(100, TimerHandler))
|
||||
{
|
||||
lastMillis = millis();
|
||||
|
@ -134,6 +140,21 @@ void setup(){
|
|||
else{
|
||||
Serial.println(F("Can't set ITimer correctly. Select another freq. or interval"));
|
||||
}
|
||||
//initial screen
|
||||
init_screen();
|
||||
//init buttons
|
||||
btn[0].setPins(INPUT_PULLUP, L_BTN);
|
||||
btn[1].setPins(INPUT_PULLUP, R_BTN);
|
||||
btn[2].setPins(INPUT_PULLUP, E_BTN);
|
||||
|
||||
|
||||
regulator.setDirection(NORMAL); // направление регулирования (NORMAL/REVERSE). ПО УМОЛЧАНИЮ СТОИТ NORMAL
|
||||
regulator.setLimits(0, 255); // пределы (ставим для 8 битного ШИМ). ПО УМОЛЧАНИЮ СТОЯТ 0 И 255
|
||||
regulator.setpoint = t_set; // сообщаем регулятору температуру, которую он должен поддерживать
|
||||
}
|
||||
|
||||
void init_screen(){
|
||||
|
||||
int istatus;
|
||||
istatus = lcd.begin(20,4);
|
||||
if(istatus)
|
||||
|
@ -168,6 +189,11 @@ void setup(){
|
|||
lcd.setCursor(4,1);
|
||||
lcd.print(t_set);
|
||||
|
||||
lcd.setCursor(0,2);
|
||||
lcd.print(" PID TUNE");//0,2 - set Temperature setting
|
||||
// lcd.setCursor(4,1);
|
||||
// lcd.print(t_set);
|
||||
|
||||
lcd.setCursor(9,0);
|
||||
lcd.write(0);
|
||||
lcd.write(1);
|
||||
|
@ -201,215 +227,7 @@ void setup(){
|
|||
lcd.print("dir: FWD");//16,2 - Motor speed
|
||||
lcd.setCursor(16,2);
|
||||
lcd.print(motor_dir_text);
|
||||
|
||||
btn[0].setPins(INPUT_PULLUP, L_BTN);
|
||||
btn[1].setPins(INPUT_PULLUP, R_BTN);
|
||||
btn[2].setPins(INPUT_PULLUP, E_BTN);
|
||||
|
||||
Serial.println("+++++Menu screen ok");
|
||||
|
||||
}
|
||||
|
||||
void loop()
|
||||
{
|
||||
|
||||
//управляем мотором
|
||||
|
||||
|
||||
//get temperature with filtration
|
||||
//filT += (therm.getTemp() - filT) * 0.1;
|
||||
t_current=therm.getTemp();
|
||||
//therm.getTempAverage();
|
||||
unsigned long currentMillis = millis();
|
||||
|
||||
//проверяем не прошел ли нужный интервал, если прошел то
|
||||
if(currentMillis - previousMillis > interval) {
|
||||
// сохраняем время последнего переключения
|
||||
previousMillis = currentMillis;
|
||||
change_params(0,100,0);
|
||||
}
|
||||
|
||||
|
||||
if (motor_state == 1){
|
||||
if (motor_dir == 0){
|
||||
stepper.setSpeed(motor_speed);
|
||||
}else{
|
||||
stepper.setSpeed(-motor_speed);
|
||||
}
|
||||
}else{
|
||||
stepper.disable();
|
||||
}
|
||||
|
||||
|
||||
|
||||
if (save==100){
|
||||
lcd.noBlink();
|
||||
lcd.cursor();
|
||||
}
|
||||
for (int i = 0; i < BTN_AMOUNT; i++) btn[i].tick();
|
||||
|
||||
if (cursor==1){
|
||||
lcd.setCursor(3, 1);
|
||||
}
|
||||
if (cursor==2){
|
||||
lcd.setCursor(15, 0);
|
||||
}
|
||||
if (cursor==3){
|
||||
lcd.setCursor(15, 1);
|
||||
}
|
||||
if (cursor==4){
|
||||
lcd.setCursor(15, 2);
|
||||
}
|
||||
//listen button held
|
||||
if (btn[0].held()) {
|
||||
Serial.println("hold enter");
|
||||
//enter change mode
|
||||
if (save==100){
|
||||
Serial.println("hold enter and save==100");
|
||||
save=cursor;
|
||||
lcd.blink();
|
||||
lcd.cursor();
|
||||
}else{
|
||||
//enter save mode
|
||||
|
||||
if(save==1){
|
||||
//Serial.println("hold enter and save==1");
|
||||
//t_set=t_set_temp;
|
||||
lcd.setCursor(4,1);
|
||||
lcd.print(" ");
|
||||
lcd.setCursor(4,1);
|
||||
lcd.print(t_set);
|
||||
lcd.cursor();
|
||||
lcd.blink();
|
||||
delay(3000);
|
||||
}
|
||||
if(save==2){
|
||||
//Serial.println("hold enter and save==2");
|
||||
//motor_speed=motor_speed_temp;
|
||||
if (motor_state==1){
|
||||
if (motor_dir==0){
|
||||
stepper.setSpeed(motor_speed);
|
||||
}else{
|
||||
stepper.setSpeed(-motor_speed);
|
||||
}
|
||||
}
|
||||
|
||||
lcd.setCursor(16,0);
|
||||
lcd.print(" ");
|
||||
lcd.setCursor(16,0);
|
||||
lcd.print(motor_speed);
|
||||
lcd.cursor();
|
||||
lcd.blink();
|
||||
delay(3000);
|
||||
}
|
||||
if(save==3){
|
||||
//Serial.println("hold enter and save==3");
|
||||
//motor_state=motor_state_temp;
|
||||
//motor_state_text=motor_state_temp_text;
|
||||
if (motor_state==0){
|
||||
stepper.stop();
|
||||
stepper.disable();
|
||||
}else{
|
||||
stepper.enable();
|
||||
}
|
||||
lcd.setCursor(16,1);
|
||||
lcd.print(" ");
|
||||
lcd.setCursor(16,1);
|
||||
lcd.print(motor_state_text);
|
||||
lcd.cursor();
|
||||
lcd.blink();
|
||||
delay(3000);
|
||||
}
|
||||
if(save==4){
|
||||
//Serial.println("hold enter and save==4");
|
||||
//motor_dir=motor_dir_temp;
|
||||
//motor_dir_text=motor_dir_temp_text;
|
||||
//тормозим перед сменой направления
|
||||
stepper.brake();
|
||||
lcd.setCursor(16,2);
|
||||
lcd.print(" ");
|
||||
lcd.setCursor(16,2);
|
||||
lcd.print(motor_dir_text);
|
||||
lcd.cursor();
|
||||
lcd.blink();
|
||||
delay(3000);
|
||||
}
|
||||
lcd.noBlink();
|
||||
lcd.cursor();
|
||||
//lcd.blink();
|
||||
save=100;
|
||||
}
|
||||
}
|
||||
//listen button click
|
||||
if (btn[0].click()) {
|
||||
//Serial.println("press enter");
|
||||
if(save!=100){
|
||||
save=100;
|
||||
// t_current_temp=t_current;
|
||||
// t_set_temp=t_set;
|
||||
// motor_speed_temp=motor_speed;
|
||||
// motor_state_temp=motor_state;
|
||||
// motor_state_temp_text=motor_state_text;
|
||||
// motor_dir_temp=motor_dir;
|
||||
// motor_dir_temp_text=motor_dir_text;
|
||||
lcd.setCursor(4,0);
|
||||
lcd.print(" ");
|
||||
lcd.setCursor(4,0);
|
||||
lcd.print(t_current);
|
||||
lcd.setCursor(4,1);
|
||||
lcd.print(" ");
|
||||
lcd.setCursor(4,1);
|
||||
lcd.print(t_set);
|
||||
lcd.setCursor(16,0);
|
||||
lcd.print(" ");
|
||||
lcd.setCursor(16,0);
|
||||
lcd.print(motor_speed);
|
||||
lcd.setCursor(16,1);
|
||||
lcd.print(" ");
|
||||
lcd.setCursor(16,1);
|
||||
lcd.print(motor_state_text);
|
||||
lcd.setCursor(16,2);
|
||||
lcd.print(" ");
|
||||
lcd.setCursor(16,2);
|
||||
lcd.print(motor_dir_text);
|
||||
}
|
||||
cursor++;
|
||||
if (cursor>4){
|
||||
cursor=1;
|
||||
}
|
||||
}
|
||||
//TODO: how to know how much click done before held?
|
||||
//int clicks1 = btn[1].hasClicks();
|
||||
//int clicks2 = btn[2].hasClicks();
|
||||
if (btn[1].step(2) && save!=100) {
|
||||
//Serial.println("press right and save!=100 and HOLD");
|
||||
//Serial.println(clicks1);
|
||||
change_params(save,1,3);
|
||||
}
|
||||
if (btn[2].step(2) && save!=100) {
|
||||
//Serial.println("press right and save!=100 and HOLD");
|
||||
change_params(save,0,3);
|
||||
}
|
||||
if (btn[1].step(4) && save!=100) {
|
||||
//Serial.println("press right and save!=100 and HOLD");
|
||||
//Serial.println(clicks1);
|
||||
change_params(save,1,4);
|
||||
}
|
||||
if (btn[2].step(4) && save!=100) {
|
||||
//Serial.println("press right and save!=100 and HOLD");
|
||||
change_params(save,0,4);
|
||||
}
|
||||
if (btn[1].click() && save!=100) {
|
||||
//Serial.println("press right and save!=100");
|
||||
change_params(save,1,1);
|
||||
}
|
||||
if (btn[2].click() && save!=100) {
|
||||
//Serial.println("press left and save!=100");
|
||||
change_params(save,0,1);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void change_params(int save, int plus, int step_val){
|
||||
//change current temperature
|
||||
//save - value change params.
|
||||
|
@ -524,3 +342,238 @@ void change_params(int save, int plus, int step_val){
|
|||
Serial.println("+++++Set motor direction ok");
|
||||
}
|
||||
}
|
||||
void loop()
|
||||
{
|
||||
if (autotune==1){
|
||||
tuner.setParameters(NORMAL, t_set, 15, 5000, 0.08, 15000, 500);
|
||||
tuner.setInput(therm.getTempAverage());
|
||||
tuner.compute();
|
||||
analogWrite(MOS_PIN, tuner.getOutput());
|
||||
if (tuner.getAccuracy() > 95){
|
||||
autotune=0;
|
||||
// выводит в порт текстовые отладочные данные, включая коэффициенты
|
||||
tuner.debugText();
|
||||
lcd.setCursor(0,3);
|
||||
lcd.print(" ");//0,2 - set Temperature setting
|
||||
lcd.setCursor(0,3);
|
||||
lcd.print(tuner.getPID_p());//0,2 - set Temperature setting
|
||||
lcd.setCursor(6,3);
|
||||
lcd.print(tuner.getPID_i());//0,2 - set Temperature setting
|
||||
lcd.setCursor(12,3);
|
||||
lcd.print(tuner.getPID_d());//0,2 - set Temperature setting
|
||||
|
||||
regulator.Kp = tuner.getPID_p();
|
||||
regulator.Ki = tuner.getPID_i();
|
||||
regulator.Kd = tuner.getPID_d();
|
||||
}
|
||||
}
|
||||
//t_current=therm.getTemp();
|
||||
t_current=therm.getTempAverage();
|
||||
unsigned long currentMillis = millis();
|
||||
|
||||
//проверяем не прошел ли нужный интервал, если прошел то
|
||||
if(currentMillis - previousMillis > interval) {
|
||||
// сохраняем время последнего переключения
|
||||
previousMillis = currentMillis;
|
||||
change_params(0,100,0);
|
||||
}
|
||||
if (autotune!=1){
|
||||
regulator.setpoint = t_set;
|
||||
regulator.input = t_current; // сообщаем регулятору текущую температуру
|
||||
// getResultTimer возвращает значение для управляющего устройства
|
||||
// (после вызова можно получать это значение как regulator.output)
|
||||
// обновление происходит по встроенному таймеру на millis()
|
||||
regulator.getResult();
|
||||
analogWrite(D0, regulator.output); // отправляем на мосфет
|
||||
}
|
||||
if (motor_state == 1){
|
||||
if (motor_dir == 0){
|
||||
stepper.setSpeed(motor_speed);
|
||||
}else{
|
||||
stepper.setSpeed(-motor_speed);
|
||||
}
|
||||
}else{
|
||||
stepper.disable();
|
||||
}
|
||||
|
||||
|
||||
|
||||
if (save==100){
|
||||
lcd.noBlink();
|
||||
lcd.cursor();
|
||||
}
|
||||
for (int i = 0; i < BTN_AMOUNT; i++) btn[i].tick();
|
||||
|
||||
if (cursor==1){
|
||||
lcd.setCursor(3, 1);
|
||||
}
|
||||
if (cursor==2){
|
||||
lcd.setCursor(15, 0);
|
||||
}
|
||||
if (cursor==3){
|
||||
lcd.setCursor(15, 1);
|
||||
}
|
||||
if (cursor==4){
|
||||
lcd.setCursor(15, 2);
|
||||
}
|
||||
|
||||
if (cursor==5){
|
||||
lcd.setCursor(0, 2);
|
||||
}
|
||||
//listen button held
|
||||
if (btn[0].held()) {
|
||||
Serial.println("hold enter");
|
||||
//enter change mode
|
||||
if (save==100){
|
||||
Serial.println("hold enter and save==100");
|
||||
save=cursor;
|
||||
lcd.blink();
|
||||
lcd.cursor();
|
||||
}else{
|
||||
//enter save mode
|
||||
|
||||
if(save==1){
|
||||
//Serial.println("hold enter and save==1");
|
||||
//t_set=t_set_temp;
|
||||
lcd.setCursor(4,1);
|
||||
lcd.print(" ");
|
||||
lcd.setCursor(4,1);
|
||||
lcd.print(t_set);
|
||||
lcd.cursor();
|
||||
lcd.blink();
|
||||
delay(3000);
|
||||
}
|
||||
if(save==2){
|
||||
//Serial.println("hold enter and save==2");
|
||||
//motor_speed=motor_speed_temp;
|
||||
if (motor_state==1){
|
||||
if (motor_dir==0){
|
||||
stepper.setSpeed(motor_speed);
|
||||
}else{
|
||||
stepper.setSpeed(-motor_speed);
|
||||
}
|
||||
}
|
||||
|
||||
lcd.setCursor(16,0);
|
||||
lcd.print(" ");
|
||||
lcd.setCursor(16,0);
|
||||
lcd.print(motor_speed);
|
||||
lcd.cursor();
|
||||
lcd.blink();
|
||||
delay(3000);
|
||||
}
|
||||
if(save==3){
|
||||
//Serial.println("hold enter and save==3");
|
||||
//motor_state=motor_state_temp;
|
||||
//motor_state_text=motor_state_temp_text;
|
||||
if (motor_state==0){
|
||||
stepper.stop();
|
||||
stepper.disable();
|
||||
}else{
|
||||
stepper.enable();
|
||||
}
|
||||
lcd.setCursor(16,1);
|
||||
lcd.print(" ");
|
||||
lcd.setCursor(16,1);
|
||||
lcd.print(motor_state_text);
|
||||
lcd.cursor();
|
||||
lcd.blink();
|
||||
delay(3000);
|
||||
}
|
||||
if(save==4){
|
||||
//Serial.println("hold enter and save==4");
|
||||
//motor_dir=motor_dir_temp;
|
||||
//motor_dir_text=motor_dir_temp_text;
|
||||
//тормозим перед сменой направления
|
||||
stepper.brake();
|
||||
lcd.setCursor(16,2);
|
||||
lcd.print(" ");
|
||||
lcd.setCursor(16,2);
|
||||
lcd.print(motor_dir_text);
|
||||
lcd.cursor();
|
||||
lcd.blink();
|
||||
delay(3000);
|
||||
}
|
||||
|
||||
if (save==5){
|
||||
autotune=1;
|
||||
lcd.setCursor(0,2);
|
||||
lcd.cursor();
|
||||
lcd.blink();
|
||||
delay(3000);
|
||||
}
|
||||
lcd.noBlink();
|
||||
lcd.cursor();
|
||||
//lcd.blink();
|
||||
save=100;
|
||||
}
|
||||
}
|
||||
//listen button click
|
||||
if (btn[0].click()) {
|
||||
//Serial.println("press enter");
|
||||
if(save!=100){
|
||||
save=100;
|
||||
// t_current_temp=t_current;
|
||||
// t_set_temp=t_set;
|
||||
// motor_speed_temp=motor_speed;
|
||||
// motor_state_temp=motor_state;
|
||||
// motor_state_temp_text=motor_state_text;
|
||||
// motor_dir_temp=motor_dir;
|
||||
// motor_dir_temp_text=motor_dir_text;
|
||||
lcd.setCursor(4,0);
|
||||
lcd.print(" ");
|
||||
lcd.setCursor(4,0);
|
||||
lcd.print(t_current);
|
||||
lcd.setCursor(4,1);
|
||||
lcd.print(" ");
|
||||
lcd.setCursor(4,1);
|
||||
lcd.print(t_set);
|
||||
lcd.setCursor(16,0);
|
||||
lcd.print(" ");
|
||||
lcd.setCursor(16,0);
|
||||
lcd.print(motor_speed);
|
||||
lcd.setCursor(16,1);
|
||||
lcd.print(" ");
|
||||
lcd.setCursor(16,1);
|
||||
lcd.print(motor_state_text);
|
||||
lcd.setCursor(16,2);
|
||||
lcd.print(" ");
|
||||
lcd.setCursor(16,2);
|
||||
lcd.print(motor_dir_text);
|
||||
}
|
||||
cursor++;
|
||||
if (cursor>5){
|
||||
cursor=1;
|
||||
}
|
||||
}
|
||||
//TODO: how to know how much click done before held?
|
||||
//int clicks1 = btn[1].hasClicks();
|
||||
//int clicks2 = btn[2].hasClicks();
|
||||
if (btn[1].step(2) && save!=100) {
|
||||
//Serial.println("press right and save!=100 and HOLD");
|
||||
//Serial.println(clicks1);
|
||||
change_params(save,1,3);
|
||||
}
|
||||
if (btn[2].step(2) && save!=100) {
|
||||
//Serial.println("press right and save!=100 and HOLD");
|
||||
change_params(save,0,3);
|
||||
}
|
||||
if (btn[1].step(4) && save!=100) {
|
||||
//Serial.println("press right and save!=100 and HOLD");
|
||||
//Serial.println(clicks1);
|
||||
change_params(save,1,4);
|
||||
}
|
||||
if (btn[2].step(4) && save!=100) {
|
||||
//Serial.println("press right and save!=100 and HOLD");
|
||||
change_params(save,0,4);
|
||||
}
|
||||
if (btn[1].click() && save!=100) {
|
||||
//Serial.println("press right and save!=100");
|
||||
change_params(save,1,1);
|
||||
}
|
||||
if (btn[2].click() && save!=100) {
|
||||
//Serial.println("press left and save!=100");
|
||||
change_params(save,0,1);
|
||||
}
|
||||
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue