feat: implement autoboost functionality and enhance motor control logic
This commit is contained in:
+14
-2
@@ -5,11 +5,23 @@
|
||||
// Speed, Kp, Ki, Kd
|
||||
struct Tunings {
|
||||
const Tuning slow = { 40, 0.05, 0.0, 0.002 };
|
||||
const Tuning main = { 60, 0.09, 0.0, 0.003 }; };
|
||||
// const Tuning main = { 60, 0.07, 0.0, 0.002 }; }; // working, but understeering
|
||||
// const Tuning main = { 60, 0.12, 0.0, 0.0022 }; }; // working, WITH autotuning
|
||||
// const Tuning main = { 40, 0.1, 0.0, 0.001 }; }; // first round good
|
||||
const Tuning main = { 67, 0.1, 0.0, 0.004 }; };
|
||||
// const Tuning main = { 60, 0.09, 0.0, 0.002 }; }; // randomly going off track, maybe oversteering?
|
||||
|
||||
const uint8_t SENSORS[] = {
|
||||
19, 18, 17, 16, 15, 14, 11, 10, 9 };
|
||||
// 0 to disable
|
||||
const uint8_t IR_PIN = 20;
|
||||
// will get inaccurate under 5cm
|
||||
const int IR_THRESHOLD = 10;
|
||||
const int IR_THRESHOLD = 0;
|
||||
|
||||
// Autoboost
|
||||
// #define ENABLE_AUTOBOOST
|
||||
const int TURN_THRESHOLD = 150;
|
||||
const int TURN_HYSTERESIS = 30;
|
||||
const float BOOST_FACTOR = 1.5;
|
||||
const int LEAD_IN_MS = 80;
|
||||
#define AUTOBOOT_MAGIC 0xAB
|
||||
+29
-7
@@ -13,18 +13,36 @@ void setup() {
|
||||
avoidanceSetup();
|
||||
|
||||
if(SW()) {
|
||||
Serial.println(F("EMERGENCY MODE RESETTING EEPROM"));
|
||||
unsigned long pressStart = millis();
|
||||
unsigned long lastPrintSec = 0;
|
||||
|
||||
for (int i = 0; i < (int)EEPROM.length(); i++) {
|
||||
EEPROM.write(i, 0xFF); }
|
||||
while(SW()) {
|
||||
unsigned long s = (millis() - pressStart) / 1000;
|
||||
|
||||
Serial.println(F("DONE")); }
|
||||
if(s != lastPrintSec) {
|
||||
lastPrintSec = s;
|
||||
Serial.println(F("Activating emergency EEPROM reset")); }
|
||||
|
||||
if(millis() - pressStart >= 3000) {
|
||||
for(int i = 0; i < (int)EEPROM.length(); i++)
|
||||
EEPROM.write(i, 0xFF);
|
||||
|
||||
Serial.println(F("Full EEPROM reset done"));
|
||||
|
||||
while(SW());
|
||||
|
||||
break; } }
|
||||
|
||||
if(millis() - pressStart < 3000) {
|
||||
autoboost.invalidate();
|
||||
Serial.println(F("Autoboost data reset")); } }
|
||||
|
||||
Serial.println(F("Loading calibration values"));
|
||||
loadCalibration();
|
||||
Serial.println(F("Calibration values loaded"));
|
||||
printCalibration();
|
||||
printDistance(); }
|
||||
printDistance();
|
||||
autoboost.begin(); }
|
||||
|
||||
void loop() {
|
||||
while (true) {
|
||||
@@ -46,6 +64,7 @@ void loop() {
|
||||
break; }
|
||||
|
||||
long startTime = millis();
|
||||
autoboost.begin();
|
||||
|
||||
while(SW());
|
||||
|
||||
@@ -70,7 +89,7 @@ void loop() {
|
||||
state = END; } }
|
||||
|
||||
// printDistance();
|
||||
scale = min(1.0, 0.5 + (float)getDistance() / 50.0 / 2.0);
|
||||
// scale = min(1.0, 0.5 + (float)getDistance() / 50.0 / 2.0);
|
||||
|
||||
switch (state) {
|
||||
case START:
|
||||
@@ -89,7 +108,9 @@ void loop() {
|
||||
state = AVOIDANCE;
|
||||
timer.reset(); }
|
||||
|
||||
if (timer.elapsed(30000)) {
|
||||
if (timer.elapsed(30000) || SW()) {
|
||||
while(SW());
|
||||
|
||||
state = END;
|
||||
timer.reset(); }
|
||||
|
||||
@@ -127,4 +148,5 @@ void loop() {
|
||||
default:
|
||||
break; } }
|
||||
|
||||
autoboost.save();
|
||||
fd(0); } }
|
||||
|
||||
@@ -0,0 +1,141 @@
|
||||
#pragma once
|
||||
#include <EEPROM.h>
|
||||
#include <config.h>
|
||||
|
||||
#define MAX_EVENTS 32
|
||||
|
||||
struct __attribute__((packed)) AutoboostData {
|
||||
uint8_t magic;
|
||||
uint8_t eventCount;
|
||||
uint16_t deltas[MAX_EVENTS]; };
|
||||
|
||||
struct Autoboost {
|
||||
enum Mode { RECORD, RUN, IDLE } mode = IDLE;
|
||||
AutoboostData data;
|
||||
|
||||
uint32_t lapStartMs = 0;
|
||||
float lastError = 0;
|
||||
uint32_t lastTickMicros = 0;
|
||||
bool inTurn = false;
|
||||
uint16_t lastEventMs = 0;
|
||||
|
||||
uint8_t entryBounce = 0;
|
||||
uint8_t exitBounce = 0;
|
||||
static const uint8_t ENTRY_DEBOUNCE = 5;
|
||||
static const uint8_t EXIT_DEBOUNCE = 10;
|
||||
|
||||
// run-mode stream walker
|
||||
uint16_t nextStart = 0;
|
||||
uint16_t nextEnd = 0;
|
||||
uint16_t eventIdx = 0;
|
||||
|
||||
void begin() {
|
||||
static const int addr = 4 * (sizeof(SENSORS) / sizeof(SENSORS[0]));
|
||||
|
||||
lapStartMs = millis();
|
||||
eventIdx = 0;
|
||||
nextStart = nextEnd = 0;
|
||||
|
||||
EEPROM.get(addr, data);
|
||||
|
||||
if (data.magic == AUTOBOOT_MAGIC && data.eventCount > 0) {
|
||||
mode = RUN;
|
||||
|
||||
if (data.eventCount >= 2) {
|
||||
nextStart = data.deltas[0];
|
||||
nextEnd = nextStart + data.deltas[1]; }
|
||||
|
||||
Serial.print(F("[AB] RUN "));
|
||||
Serial.print(data.eventCount / 2); Serial.print(F("T("));
|
||||
Serial.print(data.eventCount); Serial.println(F("e):"));
|
||||
|
||||
uint16_t accum = 0;
|
||||
|
||||
for (uint16_t i = 0; i + 1 < data.eventCount; i += 2) {
|
||||
uint16_t s = accum + data.deltas[i];
|
||||
uint16_t e = s + data.deltas[i + 1];
|
||||
Serial.print(F(" ")); Serial.print(i / 2 + 1);
|
||||
Serial.print(F(": ")); Serial.print(s);
|
||||
Serial.print(F(" -> ")); Serial.print(e);
|
||||
Serial.print(F(" (")); Serial.print(e - s); Serial.println(F(")"));
|
||||
accum = e; } }
|
||||
else {
|
||||
uint16_t maxEv = min((uint16_t)MAX_EVENTS,
|
||||
(uint16_t)((EEPROM.length() - addr - 2) / 2));
|
||||
Serial.print(F("[AB] REC 0/")); Serial.print(maxEv); Serial.println(F("e"));
|
||||
mode = RECORD;
|
||||
data.magic = AUTOBOOT_MAGIC;
|
||||
data.eventCount = 0; } }
|
||||
|
||||
void tick(float error) {
|
||||
if (mode != RECORD || data.eventCount >= MAX_EVENTS) return;
|
||||
|
||||
uint32_t now = micros();
|
||||
|
||||
if (now - lastTickMicros < 5000) return;
|
||||
|
||||
lastTickMicros = now;
|
||||
|
||||
uint16_t elapsed = millis() - lapStartMs;
|
||||
|
||||
if (!inTurn) {
|
||||
if (abs(error) > TURN_THRESHOLD) {
|
||||
if (++entryBounce >= ENTRY_DEBOUNCE) {
|
||||
inTurn = true;
|
||||
entryBounce = 0;
|
||||
data.deltas[data.eventCount++] = elapsed - lastEventMs;
|
||||
lastEventMs = elapsed; } }
|
||||
else
|
||||
entryBounce = 0; }
|
||||
else {
|
||||
if (abs(error) < TURN_THRESHOLD - TURN_HYSTERESIS) {
|
||||
if (++exitBounce >= EXIT_DEBOUNCE) {
|
||||
inTurn = false;
|
||||
exitBounce = 0;
|
||||
data.deltas[data.eventCount++] = elapsed - lastEventMs;
|
||||
lastEventMs = elapsed; } }
|
||||
else
|
||||
exitBounce = 0; }
|
||||
|
||||
lastError = error; }
|
||||
|
||||
float boost() {
|
||||
if (mode != RUN || data.eventCount < 2) return 1.0;
|
||||
|
||||
uint16_t elapsed = millis() - lapStartMs;
|
||||
|
||||
while (eventIdx + 2 < data.eventCount &&
|
||||
elapsed > nextEnd + LEAD_IN_MS) {
|
||||
nextStart = nextEnd + data.deltas[eventIdx + 2];
|
||||
nextEnd = nextStart + data.deltas[eventIdx + 3];
|
||||
eventIdx += 2; }
|
||||
|
||||
if (elapsed < nextStart - LEAD_IN_MS) return BOOST_FACTOR;
|
||||
|
||||
if (elapsed < nextStart) {
|
||||
float t = (float)(elapsed - (nextStart - LEAD_IN_MS)) / LEAD_IN_MS;
|
||||
return BOOST_FACTOR - (BOOST_FACTOR - 1.0) * t; }
|
||||
|
||||
if (elapsed <= nextEnd) return 1.0;
|
||||
|
||||
if (elapsed < nextEnd + LEAD_IN_MS) {
|
||||
float t = (float)(elapsed - nextEnd) / LEAD_IN_MS;
|
||||
return 1.0 + (BOOST_FACTOR - 1.0) * t; }
|
||||
|
||||
return BOOST_FACTOR; }
|
||||
|
||||
void save() {
|
||||
if (mode != RECORD) return;
|
||||
|
||||
static const int addr = 4 * (sizeof(SENSORS) / sizeof(SENSORS[0]));
|
||||
EEPROM.put(addr, data.magic);
|
||||
EEPROM.put(addr + 1, data.eventCount);
|
||||
|
||||
for (uint16_t i = 0; i < data.eventCount; i++)
|
||||
EEPROM.put(addr + 2 + i * 2, data.deltas[i]); }
|
||||
|
||||
void invalidate() {
|
||||
static const int addr = 4 * (sizeof(SENSORS) / sizeof(SENSORS[0]));
|
||||
EEPROM.write(addr, 0xFF); } };
|
||||
|
||||
Autoboost autoboost;
|
||||
+16
-9
@@ -1,10 +1,11 @@
|
||||
#pragma once
|
||||
#include <modules/input.h>
|
||||
#include <modules/autotuning.h>
|
||||
#include <modules/autoboost.h>
|
||||
#include <QuickPID.h>
|
||||
#include <us82_motor.h>
|
||||
#include <lib/filter.h>
|
||||
float input, output, setpoint, leftmotor, rightmotor, currSpeed, scale = 1.001;
|
||||
float input, output, setpoint, leftmotor, rightmotor, currSpeed, scale = 1.001, lastInput = 0;
|
||||
|
||||
QuickPID pid = QuickPID(&input, &output, &setpoint, 0, 0, 0, QuickPID::Action::reverse);
|
||||
|
||||
@@ -17,6 +18,7 @@ void pidSetup(Tuning tuning) {
|
||||
Serial.print("Updating speed: ");
|
||||
Serial.println(tuning.speed);
|
||||
|
||||
// scaleFilter.filter(0.2);
|
||||
currSpeed = tuning.speed;
|
||||
pid.SetMode(QuickPID::Control::automatic);
|
||||
pid.SetTunings(tuning.kp, tuning.ki, tuning.kd);
|
||||
@@ -33,21 +35,26 @@ void track(Tuning tuning) {
|
||||
// static uint32_t lastAdapt = 0;
|
||||
pidSetup(tuning);
|
||||
|
||||
// tuner.tick(line());
|
||||
// input = tuner.filteredError;
|
||||
line();
|
||||
autoboost.tick(input);
|
||||
pid.Compute();
|
||||
|
||||
leftmotor = tuning.speed + output;
|
||||
rightmotor = tuning.speed - output;
|
||||
|
||||
// Serial.print("Input: "); Serial.print(input); Serial.print("\tOutput: "); Serial.print(output);
|
||||
// Serial.print("\tLeft Motor: "); Serial.print(leftmotor); Serial.print("\tRight Motor: "); Serial.println(rightmotor);
|
||||
|
||||
// if scale is not controlled, disable the filter
|
||||
float smoothScale = scale < 1.001 ? scaleFilter.filter(scale) : 1.001;
|
||||
#ifdef ENABLE_AUTOBOOST
|
||||
float boostScale = autoboost.boost();
|
||||
float combined = scale * boostScale;
|
||||
#else
|
||||
float combined = scale;
|
||||
#endif
|
||||
float smoothScale = scaleFilter.filter(combined);
|
||||
|
||||
fd2(constrain(leftmotor * smoothScale, -100, 100), constrain(rightmotor * smoothScale, -100, 100));
|
||||
leftmotor = tuning.speed * smoothScale + output;
|
||||
rightmotor = tuning.speed * smoothScale - output;
|
||||
// printf("Boost speed: %.3f\n", tuning.speed * (double)boostScale);
|
||||
|
||||
fd2(constrain(leftmotor, -100, 100), constrain(rightmotor, -100, 100));
|
||||
|
||||
// if (millis() - lastAdapt > 100) {
|
||||
// float kp = pid.GetKp(), kd = pid.GetKd();
|
||||
|
||||
Reference in New Issue
Block a user