The Adafruit_INA219 is included.
As requested the sketch. I commented some code out of the sketch to make it run beyond the setup (otherwise the Guru error would not show and we see only a lock since no data coming to the serial monitor).
Also the inits of the INA219 and the MPU6050 are commented out.
/*
A high speed balancing robot, running on an ESP32.
Chip Model: ESP32-D0WD-V3
Chip revision: 3
Board: ESP32 Dev Module
FCC ID: 2BB77-ESP32-32X
Use board library ESP32 v2.0.17
*/
#include <Arduino.h>
#include <WiFi.h>
#include <MPU6050.h>
#include <Streaming.h>
#include <PID.h>
#include <fastStepper.h>
#include <par.h>
#include <Preferences.h> // for storing settings
#include <Adafruit_INA219.h>
#include <ESPAsyncWebServer.h>
#include <WebSocketsServer.h>
#include <FS.h>
#include <LittleFS.h>
#include <ESPmDNS.h>
#include "defines.h"
// ----- Type definitions
typedef union
{
struct
{
float val; // Float (4 bytes) comes first, as otherwise padding will be applied
uint8_t cmd;
uint8_t checksum;
};
uint8_t array[6];
} command;
// ----- Function prototypes
bool connectWifi();
void parseSerial();
void parseCommand(char *data, uint8_t length);
void calculateGyroOffset(uint8_t nSample);
void webSocketEvent(uint8_t num, WStype_t type, uint8_t *payload, size_t length);
void readSensor();
void initSensor(uint8_t n);
void sendConfigurationData(uint8_t num);
bool InitPIDs();
void IRAM_ATTR motLeftTimerFunction();
void IRAM_ATTR motRightTimerFunction();
// ----- Definitions and variables
// -- Server
const char *http_username = "admin";
const char *http_password = "admin";
AsyncWebServer server(80);
WebSocketsServer wsServer = WebSocketsServer(81);
// -- EEPROM
Preferences preferences;
// -- Stepper motors
fastStepper motLeft(PIN_MOTOR_LEFT_STEP, PIN_MOTOR_LEFT_DIR, 0, REVERSE_LEFT_MOTOR, motLeftTimerFunction);
fastStepper motRight(PIN_MOTOR_RIGHT_STEP, PIN_MOTOR_RIGHT_DIR, 1, REVERSE_RIGHT_MOTOR, motRightTimerFunction);
uint8_t motorCurrent = 35;
float maxStepSpeed = MAX_STEP_SPEED;
float absSpeedChange = 0;
// -- PID control
// go (default)
PID pidAngle(cPD, dT, PID_ANGLE_MAX, -PID_ANGLE_MAX);
PID pidPos(cPD, dT, PID_POS_MAX, -PID_POS_MAX);
PID pidSpeed(cPID, dT, PID_SPEED_MAX, -PID_SPEED_MAX);
// stay
PID pidAngleStay(cPD, dT, PID_ANGLE_MAX, -PID_ANGLE_MAX);
PID pidPosStay(cPD, dT, PID_POS_MAX, -PID_POS_MAX);
// to save intial values
PID intialPidAngle(cPD, dT, PID_ANGLE_MAX, -PID_ANGLE_MAX);
PID intialPidPos(cPD, dT, PID_POS_MAX, -PID_POS_MAX);
uint8_t controlMode = 1; // 0 = only angle, 1 = angle+position, 2 = angle+speed
// -- IMU
MPU6050 imu;
// current and voltage
Adafruit_INA219 ina219(0x40);
int16_t gyroOffset[3];
float accAngle = 0;
float filterAngle = 0;
float accAngle_X = 0;
float filterAngle_X = 0;
float angleOffset = 0.0;
float gyroFilterConstant = GYRO_FILTER_CONSTANT;
float gyroGain = GYRO_GAIN;
// -- Others
float steerFilterConstant = 0.9;
float speedFilterConstant = 0.9;
// -- WiFi
const char host[] = "BalancingRobot";
// Noise source (for system identification)
boolean noiseSourceEnable = 0;
float noiseSourceAmplitude = 1;
float steerInput, speedInput;
uint32_t lastCntInput = 0;
boolean enableControl = 0;
boolean inOTA = false;
uint8_t pidTypeApply = 1; // PID to use 0=auto, 1= go PIDs, 2= stay PIDs
float batVoltageArray[16];
float batVoltage = 0;
float batVoltageLowest = 26;
float current_mA = 0;
float power_mW = 0;
float mA_h_count = 0;
uint32_t energySave_ready = 0;
boolean calibrationAutoDone = false;
// ----- Interrupt functions -----
portMUX_TYPE timerMux = portMUX_INITIALIZER_UNLOCKED;
void IRAM_ATTR motLeftTimerFunction()
{
portENTER_CRITICAL_ISR(&timerMux);
motLeft.timerFunction();
portEXIT_CRITICAL_ISR(&timerMux);
}
void IRAM_ATTR motRightTimerFunction()
{
portENTER_CRITICAL_ISR(&timerMux);
motRight.timerFunction();
portEXIT_CRITICAL_ISR(&timerMux);
}
void sendData(uint8_t *b, uint8_t l)
{
wsServer.sendBIN(0, b, l);
}
void wirelessTask(void *parameters)
{
while (1)
{
//IBus.loop();
wsServer.loop();
//ArduinoOTA.handle();
delay(2);
}
}
// compare PIDs
bool InitPIDs()
{
if (pidAngle.K == intialPidAngle.K && pidAngle.Ti == intialPidAngle.Ti && pidAngle.Td == intialPidAngle.Td && pidAngle.N == intialPidAngle.N)
{
if (pidPos.K == intialPidPos.K && pidPos.Ti == intialPidPos.Ti && pidPos.Td == intialPidPos.Td && pidPos.N == intialPidPos.N)
return true;
}
return false;
}
// ----- Main code
void setup()
{
Serial.begin(115200);
preferences.begin("settings", false); // false = RW-mode
// preferences.clear(); // Remove all preferences under the opened namespace
pinMode(PIN_MOTOR_ENABLE, OUTPUT);
digitalWrite(PIN_MOTOR_ENABLE, 1); // Disable steppers during startup
pinMode(PIN_LED, OUTPUT);
digitalWrite(PIN_LED, 0);
motLeft.init();
motRight.init();
motLeft.microStep = MICRO_STEP;
motRight.microStep = MICRO_STEP;
// Connect to Wifi
Serial.println("Init WiFi...");
boolean wifiConnected = connectWifi();
// Start DNS server
if (MDNS.begin(host))
{
Serial.print("MDNS responder started, name: ");
Serial.println(host);
}
else
{
Serial.println("Could not start MDNS responder");
}
// LittleFS setup
if (!LittleFS.begin(FORMAT_LITTLEFS_IF_FAILED))
{
Serial.println("LittleFS mount failed");
return;
}
else
{
Serial.println("LittleFS mount success !");
}
// Test file
// File file = LittleFS.open("/index.htm", "r");
// if (!file)
// {
// Serial.println("Failed to open file '/index.htm' for reading");
// return;
// }
// Serial.println("File Content:");
// while (file.available()) {
// Serial.write(file.read());
// }
// file.close();
// Serial.println();
// Define a route to serve the HTML page
server.on("/", HTTP_GET, [](AsyncWebServerRequest* request) {
request->send(200, "text/html", "<html><body><h1>Hello, ESP32!</h1></body></html>");
});
server.on("/", HTTP_GET, [](AsyncWebServerRequest *request) {
request->send(LittleFS, "/index.htm");
});
server.serveStatic("/", LittleFS, "/");
server.onNotFound([](AsyncWebServerRequest *request) {
request->send(404, "text/plain", "FileNotFound");
});
// Start the server
server.begin();
wsServer.begin();
wsServer.onEvent(webSocketEvent);
MDNS.addService("http", "tcp", 80);
MDNS.addService("ws", "tcp", 81);
// Gyro setup
delay(200);
Wire.begin(21, 22, 400000);
// ============== find all i2c adresses
Serial.println("===============================");
Serial.println(F("Finding all i2c adresses !"));
byte error, address;
int nDevices;
nDevices = 0;
for (address = 1; address < 127; address++)
{
// The i2c_scanner uses the return value of
// the Write.endTransmisstion to see if
// a device did acknowledge to the address.
Wire.beginTransmission(address);
error = Wire.endTransmission();
if (error == 0)
{
Serial.println("-");
Serial.print("I2C device found at address 0x");
if (address < 16)
Serial.print("0");
Serial.print(address, HEX);
Serial.println(" !");
nDevices++;
}
else if (error == 4)
{
Serial.println("-");
Serial.print("Unknow error at address 0x");
if (address < 16)
Serial.print("0");
Serial.println(address, HEX);
}
} // for loop
if (nDevices == 0)
{
Serial.println("No I2C devices found");
}
Serial.println("===============================");
// ============== i2c test end.
Serial.println("Init Gyro...");
// imu.initialize();
Serial.println("Calculate Gyro range and store offsets...");
// imu.setFullScaleGyroRange(MPU6050_GYRO_FS_500);
// Calculate and store gyro offsets
delay(50);
// sensor start
Serial.println("Init ina219...");
// ina219.begin();
// Init EEPROM, if not done before
Serial.println("Init EEPROM...");
if (preferences.getUInt("pref_version", 0) != PREF_VERSION)
{
preferences.clear(); // Remove all preferences under the opened namespace
preferences.putUInt("pref_version", PREF_VERSION);
Serial << "EEPROM init complete, all preferences deleted, new pref_version: " << PREF_VERSION << "\n";
}
// Read gyro offsets
Serial.println("Gyro calibration values...");
Serial << "Gyro calibration values: ";
for (uint8_t i = 0; i < 3; i++)
{
char buf[16];
sprintf(buf, "gyro_offset_%u", i);
gyroOffset[i] = preferences.getShort(buf, 0);
Serial << gyroOffset[i] << "\t";
}
Serial << endl;
// Read angle offset
angleOffset = preferences.getFloat("angle_offset", 0.0);
// Perform initial gyro measurements
Serial.println("Initial Gyro measurements...");
// initSensor(50);
// disable robot
enableControl = 0;
inOTA = true;
// motLeft.speed = 0;
// motRight.speed = 0;
digitalWrite(PIN_MOTOR_ENABLE, 1);
// PID (go)
Serial.println("Init PID...");
pidAngle.setParameters(KP_ANGLE_GO, KI_ANGLE_GO, KD_ANGLE_GO, KN_ANGLE_GO);
pidPos.setParameters(KP_POS_GO, KI_POS_GO, KD_POS_GO, KN_POS_GO);
pidSpeed.setParameters(KP_SPEED_GO, KI_SPEED_GO, KD_SPEED_GO, KN_SPEED_GO);
// PID (stay)
pidAngleStay.setParameters(KP_ANGLE_STAY, KI_ANGLE_STAY, KD_ANGLE_STAY, KN_ANGLE_STAY);
pidPosStay.setParameters(KP_POS_STAY, KI_POS_STAY, KD_POS_STAY, KN_POS_STAY);
intialPidAngle = pidAngle; // copy intial PIDs
intialPidPos = pidPos;
// Run wireless related tasks on core 0
xTaskCreatePinnedToCore(
wirelessTask, /* Function to implement the task */
"wirelessTask", /* Name of the task */
10000, /* Stack size in words */
NULL, /* Task input parameter */
0, /* Priority of the task */
NULL, /* Task handle. */
0); /* Core where the task should run */
// ready beep
Serial.println("Init Stepper motors...");
dacWrite(PIN_MOTOR_CURRENT, 7);
digitalWrite(PIN_MOTOR_ENABLE, 0); // enable
motLeft.speed = -1000;
motRight.speed = -1000;
Serial.println("Init Stepper motors: speed update");
// motLeft.update();
// motRight.update();
// delay(360);
// Serial.println("Init Stepper motors: speed set");
// motLeft.speed = 650;
// motRight.speed = 650;
// Serial.println("Init Stepper motors: speed update");
// motLeft.update();
// motRight.update();
// delay(170);
// digitalWrite(PIN_MOTOR_ENABLE, 1); // disable
// dacWrite(PIN_MOTOR_CURRENT, motorCurrent);
Serial.println("End of setup!");
}
void loop()
{
static unsigned long tLast = 0;
float pidAngleOutput = 0;
float avgMotSpeed;
float steer = 0;
static float avgSteer;
static float avgSpeed;
static float avgMotSpeedSum = 0;
int32_t avgMotStep;
float pidPosOutput = 0, pidSpeedOutput = 0;
static uint8_t k = 0;
static uint32_t lastInputTime = 0;
uint32_t tNowMs;
float noiseValue = 0;
static uint32_t sendLaslWs = 0;
static uint32_t maxCurHold = 0;
static uint32_t measurment_last = 0;
static boolean measurment_switch = true; // just to minimize i2c use time
static uint32_t maxSpeedCount = 0;
static float absSpeed = 0;
unsigned long tNow = micros();
tNowMs = millis();
// test
Serial.println("looping...");
// check voltage every xx ms.
if (millis() - measurment_last > 100)
{
measurment_last = millis();
if (measurment_switch)
{ // true every 200ms
for (int i = 0; i != 15; i++)
{ // move
batVoltageArray[i] = batVoltageArray[i + 1];
}
batVoltageArray[15] = ina219.getBusVoltage_V() + (ina219.getShuntVoltage_mV() / 1000) + 0.1; // write to last
float batVoltageTemp = 0;
for (int i = 0; i != 15; i++)
{ // add all
batVoltageTemp += batVoltageArray[i];
}
if (batVoltageArray[0] != 0)
{ // if array full, calculate avg
batVoltage = batVoltageTemp / 15; // avg
if (batVoltage < batVoltageLowest) // updt lowest voltage
{
batVoltageLowest = batVoltage;
}
}
}
if (!measurment_switch)
{
current_mA = (current_mA + ina219.getCurrent_mA()) / 2;
power_mW = (power_mW + ina219.getPower_mW()) / 2;
if (current_mA < 0)
{
mA_h_count += -((current_mA / 3600) / 5);
}
}
measurment_switch = !measurment_switch; // invert
}
// blink led depending on battery level
if (batVoltageLowest < DEAD_BATTERY_VOLT - 0.2)
{ // dead battery
digitalWrite(PIN_LED, millis() >> 6 & 1);
enableControl = 0;
motLeft.speed = 0.003; // very slow led blinking, dead battery
motRight.speed = 0.003;
digitalWrite(PIN_MOTOR_ENABLE, 1); // disable
}
else if (batVoltageLowest < DEAD_BATTERY_VOLT)
{
digitalWrite(PIN_LED, millis() >> 8 & 1);
}
else if (batVoltageLowest < DEAD_BATTERY_VOLT + 0.5)
{
digitalWrite(PIN_LED, millis() >> 10 & 1);
} // 1 Hz blinking
else
{
digitalWrite(PIN_LED, 0);
}
if (tNow - tLast > dT_MICROSECONDS && !inOTA)
{
readSensor();
if (enableControl)
{
// to 0, if not updated x ms, in case ws control suddently goes offline with not 0 as the last value
if (lastCntInput < millis() - 1300)
{
speedInput = 0;
steerInput = 0;
}
avgSpeed = speedFilterConstant * avgSpeed + (1 - speedFilterConstant) * speedInput / 12;
avgSteer = steerFilterConstant * avgSteer + (1 - steerFilterConstant) * steerInput / 2.0;
// turn off on dead battery
if (batVoltageLowest < DEAD_BATTERY_VOLT)
{
avgSpeed = 0;
avgSteer = 0;
}
if (abs(avgSpeed) < 0.2)
{
// speedInput = 0;
}
else
{
lastInputTime = tNowMs;
if (controlMode == 1)
{
controlMode = 2;
motLeft.setStep(0);
motRight.setStep(0);
pidSpeed.reset();
}
}
steer = avgSteer;
if (tNowMs - lastInputTime > 300 && controlMode == 2)
{
controlMode = 1;
motLeft.setStep(0);
motRight.setStep(0);
pidPos.reset();
}
if (controlMode == 0)
{ // angle
pidAngle.setpoint = avgSpeed * 2;
}
else if (controlMode == 1)
{ // angle+position
avgMotStep = (motLeft.getStep() + motRight.getStep()) / 2;
pidPos.setpoint = avgSpeed;
pidPos.input = -((float)avgMotStep) / 1000.0;
pidPosOutput = pidPos.calculate();
pidAngle.setpoint = pidPosOutput;
}
else if (controlMode == 2)
{ // angle+speed
pidSpeed.setpoint = avgSpeed; // pid set target
pidSpeed.input = -avgMotSpeedSum / 35.0; // pid speed now (input)
pidSpeedOutput = pidSpeed.calculate(); // pid out
pidAngle.setpoint = pidSpeedOutput; // apply to angle
}
pidAngle.input = filterAngle;
pidAngleOutput = pidAngle.calculate();
if (noiseSourceEnable)
{
noiseValue = noiseSourceAmplitude * ((random(1000) / 1000.0) - 0.5);
pidAngleOutput += noiseValue;
}
avgMotSpeedSum += pidAngleOutput / 2;
if (avgMotSpeedSum > maxStepSpeed)
{
avgMotSpeedSum = maxStepSpeed;
}
else if (avgMotSpeedSum < -maxStepSpeed)
{
avgMotSpeedSum = -maxStepSpeed;
}
avgMotSpeed = avgMotSpeedSum;
float absSpeed_Before = absSpeed;
absSpeed = abs(avgMotSpeed);
absSpeedChange = abs(absSpeed_Before - absSpeed);
// default control
motLeft.speed = avgMotSpeed + steer;
motRight.speed = avgMotSpeed - steer;
// count how many cycles robot is stable & not moving.
if (absSpeed < 20 && abs(filterAngle) < 5 && controlMode == 1 && abs(avgSteer) < 0.01) // true if robot stable
{
energySave_ready++;
}
else
{
energySave_ready = 0;
}
//=============================================
// count how many cycles motors at max speed without input.
if (maxStepSpeed == absSpeed && abs(avgSpeed) < 0.2)
{
maxSpeedCount++;
}
else
{
maxSpeedCount = 0;
}
//=============================================
//==================current control
int setCur = 0;
if (absSpeed < 15)
{
setCur = map(absSpeed, 0, 15, 42, 70);
setCur = constrain(setCur, 42, 70);
}
// lower current when robot stays for a long period of time
if (energySave_ready >= 2500 && absSpeed < 5 && abs(filterAngle) < 4)
{
setCur = map(absSpeed, 0, 3, 28, 42);
}
if (absSpeed >= 15)
{
setCur = map(absSpeed, 15, 120, 70, 110);
setCur = constrain(setCur, 70, 110);
}
// add current on angle
if (setCur < 100)
{
int addCur = map(abs(filterAngle), 0, 8, 0, 25);
addCur = constrain(addCur, 0, 25);
setCur = setCur + addCur;
}
// on fast change max current for atleast XX ms
if (absSpeedChange > 2 || millis() - maxCurHold < 250)
{
setCur = 180;
if (absSpeedChange > 2)
{
maxCurHold = millis();
}
}
motorCurrent = setCur;
dacWrite(PIN_MOTOR_CURRENT, setCur);
//====
// apply spetial stay PIDs if robot stable
if ((energySave_ready == 400 && abs(filterAngle) < 1.3) && InitPIDs() && pidTypeApply == 0)
{
pidAngle = pidAngleStay; // apply stay (no swing pids), good for staying, bad for going
pidPos = pidPosStay; // apply stay (no swing pids), good for staying, bad for going
// Serial.println("Stay PIDs applied automatically");
}
if ((abs(filterAngle) >= 2.2 || energySave_ready < 400) && !InitPIDs() && pidTypeApply == 0) // robot going, so revert PIDs
{
pidAngle = intialPidAngle; // revert to initial pids
pidPos = intialPidPos;
// Serial.println("Go PIDs applied automatically");
}
if (pidTypeApply == 2) // only stay PIDs
{
pidTypeApply = 20; // it means applied
pidAngle = pidAngleStay; // apply stay (no swing pids), good for staying, bad for going
pidPos = pidPosStay; // apply stay (no swing pids), good for staying, bad for going
// Serial.println("Stay PIDs applied manually");
}
if (pidTypeApply == 1) // only go PIDs
{
pidTypeApply = 10; // applied
pidAngle = intialPidAngle; // revert to initial pids
pidPos = intialPidPos;
// Serial.println("Go PIDs applied manually");
}
//======================
// auto micro calibratation (once per boot when robot stable)
if (abs(filterAngle) >= 1.8 && energySave_ready == 4000 && !calibrationAutoDone)
{ // auto calibration
angleOffset = angleOffset + (filterAngle * 0.5);
preferences.putFloat("angle_offset", angleOffset);
calibrationAutoDone = true;
}
//======
// Disable control if robot is almost horizontal. Re-enable if upright.
if (abs(filterAngle) > 50 || maxSpeedCount > 300)
{ // (if motors at max spped for a long time, something happened, disable robot)
enableControl = 0;
motLeft.speed = 0.01; // slow led blinking, ready state
motRight.speed = 0.01;
digitalWrite(PIN_MOTOR_ENABLE, 1); // Inverted action on enable pin
}
}
else
{
if (abs(filterAngle) < 0.5 && abs(filterAngle_X) < 13 && batVoltageLowest > DEAD_BATTERY_VOLT)
{ // (re-)enable and reset stuff
enableControl = 1;
controlMode = 1;
avgMotSpeedSum = 0;
motLeft.setStep(0);
motRight.setStep(0);
pidAngle.reset();
pidPos.reset();
pidSpeed.reset();
digitalWrite(PIN_MOTOR_ENABLE, 0); // Inverted action on enable pin
// delay(1);
}
}
motLeft.update();
motRight.update();
parseSerial();
// Serial << micros()-tNow << "\t";
tLast = tNow;
// Run other tasks
// eyes of robot?
// internet requests?
// Serial << micros()-tNow << endl;
}
}
bool connectWifi()
{
if (WiFi.status() == WL_CONNECTED) return false;
//Manual Wifi
Serial.printf("Connecting to WiFi %s/%s", WIFI_SSID.c_str(), WIFI_PASS.c_str());
WiFi.disconnect();
WiFi.mode(WIFI_STA);
WiFi.hostname(WIFI_HOSTNAME);
WiFi.begin(WIFI_SSID.c_str(), WIFI_PASS.c_str());
int i = 0;
while ((WiFi.status() != WL_CONNECTED) || (i > 5) )
{
delay(500);
i += 1;
Serial.print(".");
}
if (WiFi.status() == WL_CONNECTED)
{
Serial.printf("Connected to WiFi '%s'\n", WIFI_SSID.c_str());
Serial.printf("Connected, IP address: %s/%s\n", WiFi.localIP().toString().c_str(), WiFi.subnetMask().toString().c_str()); //Get ip and subnet mask
Serial.printf("Connected, MAC address: %s\n", WiFi.macAddress().c_str()); //Get the local mac address
return true;
}
else
{
Serial.printf("Could not connected to WiFi '%s'\n", WIFI_SSID.c_str());
return false;
}
}
void parseSerial()
{
static char serialBuf[10];
static uint8_t pos = 0;
char currentChar;
while (Serial.available())
{
currentChar = Serial.read();
serialBuf[pos++] = currentChar;
if (currentChar == 'x')
{
parseCommand(serialBuf, pos);
pos = 0;
}
}
}
void parseCommand(char *data, uint8_t length)
{
float val2;
if ((data[length - 1] == 'x') && length >= 3)
{
switch (data[0])
{
//=== other
case 'c':
{ // Change controller parameter
uint8_t controllerNumber = data[1] - '0';
char cmd2 = data[2];
float val = atof(data + 3);
// Make a temporary pid object, in which parameters are updated
PID pidTemp = pidAngle;
switch (controllerNumber)
{
case 1:
pidTemp = pidAngle;
break;
case 2:
pidTemp = pidPos;
break;
case 3:
pidTemp = pidSpeed;
break;
}
switch (cmd2)
{
case 'p':
pidTemp.K = val;
break;
case 'i':
pidTemp.Ti = val;
break;
case 'd':
pidTemp.Td = val;
break;
case 'n':
pidTemp.N = val;
break;
case 't':
pidTemp.controllerType = (uint8_t)val;
break;
case 'm':
pidTemp.maxOutput = val;
break;
case 'o':
pidTemp.minOutput = -val;
break;
}
pidTemp.updateParameters();
// Store temporary pid object in correct pid object
switch (controllerNumber)
{
case 1:
pidAngle = pidTemp;
break;
case 2:
pidPos = pidTemp;
break;
case 3:
pidSpeed = pidTemp;
break;
}
Serial << controllerNumber << "\t" << pidTemp.K << "\t" << pidTemp.Ti << "\t" << pidTemp.Td << "\t" << pidTemp.N << "\t" << pidTemp.controllerType << endl;
break;
}
case 'y': // PID to use 0=auto 1= go PIDs 2= stay PIDs
pidTypeApply = atoi(&data[1]);
Serial << pidTypeApply << endl;
break;
case 'a': // Change angle offset
angleOffset = atof(data + 1);
Serial << angleOffset << endl;
break;
case 'f':
gyroFilterConstant = atof(data + 1);
Serial << gyroFilterConstant << endl;
break;
case 'v':
motorCurrent = atof(data + 1);
Serial << motorCurrent << endl;
dacWrite(PIN_MOTOR_CURRENT, motorCurrent);
break;
case 'm':
val2 = atof(data + 1);
Serial << val2 << endl;
controlMode = val2;
break;
case 'u':
//
break;
case 'g':
gyroGain = atof(data + 1);
break;
case 'p':
{
switch (data[1])
{
case 'n': // Noise source enable
noiseSourceEnable = atoi(data + 2);
break;
case 'a': // Noise source amplitude
noiseSourceAmplitude = atof(data + 2);
break;
}
break;
}
case 'j':
gyroGain = atof(data + 1);
break;
case 'k':
{
uint8_t cmd2 = atoi(data + 1);
if (cmd2 == 1)
{ // calibrate gyro
calculateGyroOffset(100);
}
else if (cmd2 == 2)
{ // calibrate acc
Serial << "Updating angle offset from " << angleOffset;
// angleOffset = filterAngle;
angleOffset = angleOffset + (filterAngle * 0.5);
Serial << " to " << angleOffset << endl;
preferences.putFloat("angle_offset", angleOffset);
}
break;
}
case 'l':
maxStepSpeed = atof(&data[1]);
break;
case 'n':
gyroFilterConstant = atof(&data[1]);
break;
}
}
}
void calculateGyroOffset(uint8_t nSample)
{
int32_t sumX = 0, sumY = 0, sumZ = 0;
int16_t x, y, z;
for (uint8_t i = 0; i < nSample; i++)
{
imu.getRotation(&x, &y, &z);
sumX += x;
sumY += y;
sumZ += z;
delay(5);
}
gyroOffset[0] = sumX / nSample;
gyroOffset[1] = sumY / nSample;
gyroOffset[2] = sumZ / nSample;
for (uint8_t i = 0; i < 3; i++)
{
char buf[16];
sprintf(buf, "gyro_offset_%u", i);
preferences.putShort(buf, gyroOffset[i]);
}
Serial << "New gyro calibration values: " << gyroOffset[0] << "\t" << gyroOffset[1] << "\t" << gyroOffset[2] << endl;
}
void readSensor()
{
int16_t ax, ay, az, gx, gy, gz;
float deltaGyroAngle;
float deltaGyroAngle_X;
imu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
accAngle = atan2f((float)ay, (float)az) * 180.0 / M_PI - angleOffset;
deltaGyroAngle = ((float)((gx - gyroOffset[0])) / GYRO_SENSITIVITY) * dT * gyroGain;
filterAngle = gyroFilterConstant * (filterAngle + deltaGyroAngle) + (1 - gyroFilterConstant) * (accAngle);
// Serial << ay/1000.0 << "\t" << az/1000.0 << "\t" << accAngle << "\t" << filterAngle << endl;
accAngle_X = atan2f((float)ax, (float)az) * 180.0 / M_PI - 5.68;
deltaGyroAngle_X = -((float)((gx - gyroOffset[1])) / GYRO_SENSITIVITY) * dT * 1;
filterAngle_X = 0.98 * (filterAngle_X + deltaGyroAngle_X) + (1 - 0.98) * (accAngle_X);
// Serial << ax/1000.0 << "\t" << az/1000.0 << "\t" << accAngle_X << "\t" << filterAngle_X << endl;
}
void initSensor(uint8_t n)
{
float gyroFilterConstantBackup = gyroFilterConstant;
gyroFilterConstant = 0.8;
for (uint8_t i = 0; i < n; i++)
{
readSensor();
}
gyroFilterConstant = gyroFilterConstantBackup;
}
void webSocketEvent(uint8_t num, WStype_t type, uint8_t *payload, size_t length)
{
Serial.println("webSocketEvent");
switch (type)
{
case WStype_DISCONNECTED:
Serial.printf("[%u] Disconnected!\n", num);
break;
case WStype_CONNECTED:
{
IPAddress ip = wsServer.remoteIP(num);
Serial.printf("[%u] Connected from %d.%d.%d.%d url: %s\n", num, ip[0], ip[1], ip[2], ip[3], payload);
sendConfigurationData(num);
}
break;
case WStype_TEXT:
Serial.printf("[%u] get Text: %s\n", num, payload);
parseCommand((char *)payload, length);
break;
case WStype_BIN:
{
// Serial.printf("[%u] get binary length: %u\n", num, length);
if (length == 6)
{
cmd c;
memcpy(c.arr, payload, 6);
// Serial << "Binary: " << c.grp << "\t" << c.cmd << "\t" << c.val << "\t" << sizeof(cmd) << endl;
if (c.grp < parList::groupCounter)
{
// if (c.grp==0 && c.cmd<100) {
// pidParList.set(c.cmd,c.val);
// // pidPar[c.cmd].setFloat(c.val);
// }
// if (c.cmd==253) {
// pidParList.sendList(&wsServer);
// }
// if (c.cmd==254) {
// pidParList.read();
// }
// if (c.cmd==255) {
// pidParList.write();
// }
}
else if (c.grp == 100)
{
if (c.cmd == 0)
{
speedInput = c.val;
}
else if (c.cmd == 1)
{
steerInput = -c.val;
}
}
else if (c.grp == 105)
{
lastCntInput = millis();
String msg = "TELEMETRY: ";
msg += String(batVoltageLowest);
msg += "V (L) | ";
msg += String(batVoltage);
msg += "V (N) | ";
msg += String(current_mA);
msg += "mA | ";
msg += String(power_mW / 1000);
msg += "W | Ang: ";
msg += String(filterAngle);
if (enableControl == 1) // enabled
{
msg += " | Speed: ";
// msg += String(-(motRight.speed + motLeft.speed) / 2);
msg += " | Step: ";
// msg += String(motRight.getStep());
msg += ":";
// msg += String(motLeft.getStep());
if (InitPIDs())
{
msg += " [GO PIDs]";
}
else
{
msg += " [STAY PIDs]";
}
if (energySave_ready)
{
msg += " ES: ";
msg += String(energySave_ready);
}
}
else // disabled
{
msg += "<font color=\"#0caded\"> [LAYING DOWN]</font>";
}
if (calibrationAutoDone)
{
msg += " [CLBR DONE]";
}
if (batVoltageLowest < DEAD_BATTERY_VOLT) // dead battery
{
msg += "<font color=\"#fe3437\"> [LOW BATTERY]</font>";
}
if (current_mA < 0) // charging
{
msg += "<font color=\"#08e18e\"> [CHARGING]</font>";
msg += " Capacity: ";
msg += String(mA_h_count);
msg += "mA-h";
}
wsServer.sendTXT(0, msg);
}
}
break;
}
case WStype_ERROR:
case WStype_FRAGMENT_TEXT_START:
case WStype_FRAGMENT_BIN_START:
case WStype_FRAGMENT:
case WStype_FRAGMENT_FIN:
break;
}
}
void sendConfigurationData(uint8_t num)
{
// save configured data
char wBuf[63];
char buf[63];
sprintf(wBuf, "c%dp%.4f", 1, pidAngle.K);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "c%di%.4f", 1, pidAngle.Ti);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "c%dd%.4f", 1, pidAngle.Td);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "c%dn%.4f", 1, pidAngle.N);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "c%dr%.4f", 1, pidAngle.R);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "c%dm%.4f", 1, pidAngle.maxOutput);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "c%do%.4f", 1, -pidAngle.minOutput);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "c%dp%.4f", 2, pidPos.K);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "c%di%.4f", 2, pidPos.Ti);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "c%dd%.4f", 2, pidPos.Td);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "c%dn%.4f", 2, pidPos.N);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "c%dr%.4f", 2, pidPos.R);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "c%dm%.4f", 2, pidPos.maxOutput);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "c%do%.4f", 2, -pidPos.minOutput);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "c%dp%.4f", 3, pidSpeed.K);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "c%di%.4f", 3, pidSpeed.Ti);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "c%dd%.4f", 3, pidSpeed.Td);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "c%dn%.4f", 3, pidSpeed.N);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "c%dr%.4f", 3, pidSpeed.R);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "c%dm%.4f", 3, pidSpeed.maxOutput);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "c%do%.4f", 3, -pidSpeed.minOutput);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "h%.4f", speedFilterConstant);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "i%.4f", steerFilterConstant);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "v%d", motorCurrent);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "j%.4f", gyroGain);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "n%.4f", gyroFilterConstant);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "a%.4f", angleOffset);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "l%.4f", maxStepSpeed);
wsServer.sendTXT(num, wBuf);
}