Commit 18f48b4b authored by Wouter Klop's avatar Wouter Klop

Initial work on fully customisable, non-volatile, user changeable parameters!

parent 39b8ea5b
......@@ -54,7 +54,7 @@ void FlySkyIBus::loop(void)
{
state = GET_LENGTH;
while(Serial.available() > 0) {
char t = Serial.read();
Serial.read();
}
}
break;
......
......@@ -13,25 +13,43 @@ To view a copy of this license, visit http://creativecommons.org/licenses/by-sa/
#include <Arduino.h>
#include <streaming.h>
PID::PID(uint8_t controllerType, float dT, float max, float min)
PID::PID(void) {
_dT = 0.01;
controllerType = cPID;
maxOutput = 1;
minOutput = 1;
init();
}
PID::PID(uint8_t cType, float dT, float max, float min)
{
_dT = dT;
_lastError = 0;
_sumError = 0;
_lastDterm = 0;
maxOutput = max;
minOutput = min;
this->controllerType = controllerType;
controllerType = cType;
intDeadband = 0; // Integrator dead band width
resetInt = false; // Reset integrator if true when error is within dead band
init();
}
v[0] = 0; // Output filter variables
void PID::init() {
_lastError = 0;
_sumError = 0;
_lastDterm = 0;
_lastSetpoint = 0;
intDeadband = 0;
resetInt = false;
setpoint = 0;
_lastInput = 0;
R = 0;
v[0] = 0; // Output filter state variables
v[1] = 0;
}
float PID::calculate()
{
float error = setpoint - input;
......@@ -41,7 +59,7 @@ float PID::calculate()
float output = _Pterm;
if (controllerType&cPI) {
float errorDeadzone;
float errorDeadzone = 0;
if (intDeadband > 0) {
if (abs(error) < intDeadband) {
errorDeadzone = 0;
......@@ -65,10 +83,11 @@ float PID::calculate()
if (controllerType&cPD) {
// _Dterm = _lastDterm*_df1 + (error-_lastError)*_df2;
_Dterm = _lastDterm*_df1 - (input-_lastInput)*_df2;
_Dterm = _lastDterm*_df1 + (setpoint-_lastSetpoint)*R*_df2 - (input-_lastInput)*_df2;
_lastInput = input;
_lastDterm = _Dterm;
_lastError = error;
_lastSetpoint = setpoint;
output += _Dterm;
}
......
......@@ -23,6 +23,7 @@ To view a copy of this license, visit http://creativecommons.org/licenses/by-sa/
class PID {
public:
PID(void);
PID(uint8_t controllerType, float dT, float max, float min);
void setParameters(float nK, float nTi, float nTd, float nN);
......@@ -37,18 +38,19 @@ class PID {
float minOutput, maxOutput;
uint8_t controllerType;
float K, Ti, Td, N;
float K, Ti, Td, N, R;
bool resetInt;
private:
void init();
float outputFilter(float x);
float _dT;
float _lastDterm;
float _sumError;
float _Pterm, _Iterm, _Dterm;
float _lastError, _lastInput;
float _lastError, _lastInput, _lastSetpoint;
float _if, _df1, _df2;
float v[3];
......
#include <par.h>
#include <EEPROM.h>
/*
How to pass objects to be modified?
For PID, this is simple: pass array of PID objects, or a single PID object.
How to define a list of parameters to be stored?
For example, for PID this would be: type (uint8_t), P/I/D/N/min/max(/R) (float)
Every parameters should have a command (integer / character).
Proposal: use parameter objects.
Properties: pointer, type/tag, command, EEPROM address (maybe use static counter)
Functions: read/write EEPROM, receive command, send command
Sending a command is easy: get the value that the pointer corresponds to,
compose a message with a header, maybe add checksum, and return an array and length.
Receiving is a bit harder: how to (efficiently) find which parameter/pointer corresponds to the command?
Easiest method would be to have an array of parameters, which can be indexed.
Makes sending a bit harder: how to send a single command. However, when something is sent,
all parameters are sent.
In this way, can a single function be used for reading/writing from/to EEPROM?
Why not: as long as all variable types are covered, it should work.
However, a (group of) parameter(s) might need an update function.
Maybe add both command parameters to parameter definition?
To do:
Check if address counter is smaller than EEPROM size
*/
// float a,b,c;
// uint8_t d;
// parameter pars[4] = {
// {(uint8_t*) &a, t_f, 0, EEPROM_ADR_PID + 0},
// {(uint8_t*) &b, t_f, 1, EEPROM_ADR_PID + 1},
// {(uint8_t*) &c, t_f, 2, EEPROM_ADR_PID + 2},
// {(uint8_t*) &d, t_u8, 3, EEPROM_ADR_PID + 3}
// };
int parameter::addressCounter = 300;
uint8_t parameter::cmdCounter = 0;
parameter::parameter(uint8_t* _p, uint8_t _tag, uint8_t _cmd, int _address) {
p = _p;
tag = _tag;
cmd = _cmd;
address = _address;
}
parameter::parameter(uint8_t* _p, uint8_t _tag) {
p = _p;
tag = _tag;
cmd = cmdCounter++;
address = addressCounter;
addressCounter += tagSize[tag];
}
parameter::parameter(uint8_t* _p) {
p = (uint8_t*) _p;
tag = t_u8;
cmd = cmdCounter++;
address = addressCounter;
addressCounter += tagSize[tag];
}
parameter::parameter(float* _p) {
p = (uint8_t*) _p;
tag = t_f;
cmd = cmdCounter++;
address = addressCounter;
addressCounter += tagSize[tag];
}
void parameter::read(void) {
switch(tag) {
case t_u8:
*p = EEPROM.read(address);
break;
}
}
void parameter::write(void) {
switch(tag) {
case t_u8:
EEPROM.write(address, *p);
break;
}
}
// void readPIDParameters(PID* pid) {
//
// parameter pars[4] = {
// {(uint8_t*) &pid[0].K, t_f, 0, EEPROM_ADR_PID + 0},
// {(uint8_t*) &pid[0].Ti, t_f, 1, EEPROM_ADR_PID + 1},
// {(uint8_t*) &pid[1].N, t_f, 2, EEPROM_ADR_PID + 2},
// {(uint8_t*) &pid[1].controllerType, t_u8, 3, EEPROM_ADR_PID + 3}
// };
//
// // pars[0].f = &pid[0].K;
// // If parameters have never been written to EEPROM, don't read parameters (EEPROM will contain gibberish)
// if (EEPROM.read(EEPROM_ADR_PID_WRITTEN)==EEPROM_WRITTEN) {
// for (uint8_t i=0; i<3; i++) {
// pid[i].K = EEPROM.readFloat(EEPROM_ADR_PID + i*30 + 4*0);
// pid[i].Ti = EEPROM.readFloat(EEPROM_ADR_PID + i*30 + 4*1);
// pid[i].Td = EEPROM.readFloat(EEPROM_ADR_PID + i*30 + 4*2);
// pid[i].N = EEPROM.readFloat(EEPROM_ADR_PID + i*30 + 4*3);
// pid[i].R = EEPROM.readFloat(EEPROM_ADR_PID + i*30 + 4*4);
// pid[i].minOutput = EEPROM.readFloat(EEPROM_ADR_PID + i*30 + 4*5);
// pid[i].maxOutput = EEPROM.readFloat(EEPROM_ADR_PID + i*30 + 4*6);
// pid[i].controllerType = EEPROM.read(EEPROM_ADR_PID + i*30 + 4*7);
// pid[i].updateParameters();
// }
// }
// }
//
// void writePIDParameters(PID* pid) {
// for (uint8_t i=0; i<3; i++) {
// EEPROM.writeFloat(EEPROM_ADR_PID + i*30 + 4*0, pid[i].K);
// EEPROM.writeFloat(EEPROM_ADR_PID + i*30 + 4*1, pid[i].Ti);
// EEPROM.writeFloat(EEPROM_ADR_PID + i*30 + 4*2, pid[i].Td);
// EEPROM.writeFloat(EEPROM_ADR_PID + i*30 + 4*3, pid[i].N);
// EEPROM.writeFloat(EEPROM_ADR_PID + i*30 + 4*4, pid[i].R);
// EEPROM.writeFloat(EEPROM_ADR_PID + i*30 + 4*5, pid[i].minOutput);
// EEPROM.writeFloat(EEPROM_ADR_PID + i*30 + 4*6, pid[i].maxOutput);
// EEPROM.write(EEPROM_ADR_PID + i*30 + 4*7, pid[i].controllerType);
// EEPROM.write(EEPROM_ADR_PID_WRITTEN, EEPROM_WRITTEN);
// EEPROM.commit();
// }
// }
//
// void readParameter(parameter p) {
// switch (p.tag) {
// case t_f:
// 1;
// break;
// case t_u8:
// 2;
// break;
// }
// }
#ifndef uniPar_H
#define uniPar_H
#include<Arduino.h>
#include<PID.h>
#define EEPROM_ADR_PID_WRITTEN 200
#define EEPROM_ADR_PID 201
#define EEPROM_WRITTEN 123 // Flag to check if EEPROM has been initialised
// #define PAR_READ 0
// #define PAR_WRITE 1
//
// void readPIDParameters(PID* pid);
// void writePIDParameters(PID* pid);
enum tag {t_u8,t_u16,t_u32,t_i8,t_i16,t_i32,t_f,t_d};
const uint8_t tagSize[] = {1,2,4,1,2,4,4,8};
class parameter {
public:
uint8_t* p;
int address;
uint8_t cmd;
uint8_t tag;
static int addressCounter;
static uint8_t cmdCounter;
parameter(uint8_t* _p, uint8_t _tag, uint8_t _cmd, int _address);
parameter(uint8_t* _p, uint8_t _tag);
parameter(uint8_t* _p);
parameter(float* _p);
void read(void);
void write(void);
};
// typedef struct {
// // union {
// // uint8_t * u8;
// // uint16_t * u16;
// // uint32_t * u32;
// // int8_t * i8;
// // int16_t * i16;
// // int32_t * i32;
// // float * f;
// // double * d;
// // };
// uint8_t *p;
// uint8_t tag;
// uint8_t cmdNo;
// uint16_t address;
// // void getFun() {};
// } parameter;
#endif
......@@ -16,4 +16,4 @@ monitor_port = COM3
monitor_speed = 115200
; upload_port = COM3
; upload_port = balancingrobot.local
upload_port = 192.168.178.35
upload_port = 192.168.178.33
......@@ -31,6 +31,7 @@ Also, you have to publish all modifications.
#include <SPIFFS.h>
#include <SPIFFSEditor.h>
#include <fastStepper.h>
#include <par.h>
// ----- Type definitions
typedef union {
......@@ -71,7 +72,7 @@ AsyncWebServer httpServer(80);
WebSocketsServer wsServer = WebSocketsServer(81);
// -- EEPROM
#define EEPROM_SIZE 256
#define EEPROM_SIZE 1024
#define EEPROM_ADR_INIT 0
#define EEPROM_ADR_GYRO_OFFSET 50
#define EEPROM_ADR_ANGLE_OFFSET 60
......@@ -96,6 +97,12 @@ float maxStepSpeed = 3000;
#define dT_MICROSECONDS 5000
#define dT dT_MICROSECONDS/1000000.0
#define PID_ANGLE 0
#define PID_POS 1
#define PID_SPEED 2
PID pid[3];
#define PID_ANGLE_MAX 20
PID pidAngle(cPD, dT, PID_ANGLE_MAX, -PID_ANGLE_MAX);
#define PID_POS_MAX 35
......@@ -145,14 +152,10 @@ void setMotorCurrent() {
dacWrite(motorCurrentPin, motorCurrent);
}
uint8_t x = 0;
void wirelessTask(void * parameters) {
while (1) {
IBus.loop();
wsServer.loop(); // Shouldn't this run on core 0?
// x++;
// Serial.println(x);
IBus.loop();
wsServer.loop(); // Shouldn't this run on core 0?
delay(1);
}
}
......@@ -276,6 +279,8 @@ void setup() {
if (MDNS.begin(host)) {
Serial.print("MDNS responder started, name: ");
Serial.println(host);
} else {
Serial.println("Could not start MDNS responder");
}
httpServer.on("/", HTTP_GET, [](AsyncWebServerRequest *request){
......@@ -305,15 +310,20 @@ void setup() {
// }
dacWrite(motorCurrentPin, motorCurrent);
pidAngle.setParameters(0.65,0,0.075,15);
pidAngle.setpoint = 0;
pidAngle.setParameters(0.65,0,0.075,15);
pidPos.setParameters(1,0,1.2,20);
pidPos.setpoint = 0;
pidSpeed.setParameters(6,5,0,20);
pidSpeed.setpoint = 0;
float a = 1.2, b = 3.341;
uint8_t c = 123, d = 245;
parameter p[] = {{&a}, {&b}, {&c}, {&d}};
for (uint8_t i=0; i<4; i++) {
Serial << i << "\t"<< p[i].tag << "\t" << (*p[i].p) << "\t" << p[i].cmd << "\t" << p[i].address << endl;
}
// Run wirless related tasks on core 0
xTaskCreatePinnedToCore(
wirelessTask, /* Function to implement the task */
"wirelessTask", /* Name of the task */
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment