71 lines
1.8 KiB
C++
71 lines
1.8 KiB
C++
#include <Arduino.h>
|
|
#include "IndSensorMap.hpp"
|
|
#include "PseudoSensorControl.hpp"
|
|
|
|
float refs[4] = {14,14,14,14};
|
|
|
|
Constants repelling = {10000, 0, 50000};
|
|
Constants attracting = {10000, 0, 50000};
|
|
|
|
K_MAP consts = {repelling, attracting};
|
|
|
|
#define slewRateLimit 100 // max PWM change per control cycle (determined by 1 second / sampling rate)
|
|
// this was implemented by Claude and we can see if it helps.
|
|
// Set it at or above 255 to make it have no effect.
|
|
|
|
// Might be useful for things like jitter or lag.
|
|
#define sampling_rate 1000 // Hz
|
|
|
|
// EMA filter alpha value (all sensors use same alpha)
|
|
#define alphaVal 0.3f
|
|
|
|
|
|
// ABOVE THIS LINE IS TUNING VALUES ONLY, BELOW IS ACTUAL CODE.
|
|
|
|
unsigned long tprior;
|
|
unsigned int tDiffMicros;
|
|
|
|
PseudoSensorController controller(indL, indR, indF, indB, consts, refs, slewRateLimit);
|
|
|
|
const int dt_micros = 1e6/sampling_rate;
|
|
|
|
#define LEV_ON
|
|
|
|
int ON = 0;
|
|
|
|
void setup() {
|
|
Serial.begin(57600);
|
|
|
|
indL.alpha = alphaVal;
|
|
indR.alpha = alphaVal;
|
|
indF.alpha = alphaVal;
|
|
indB.alpha = alphaVal;
|
|
|
|
tprior = micros();
|
|
for (PinPair& mc : pinMap) {
|
|
pinMode(mc.dir, OUTPUT);
|
|
pinMode(mc.pwm, OUTPUT);
|
|
}
|
|
}
|
|
|
|
void loop() {
|
|
if (Serial.available() > 0) {
|
|
// this might need to be changed if we have trouble getting serial to read.
|
|
char c = Serial.read();
|
|
while(Serial.available()) Serial.read(); // flush remaining
|
|
|
|
controller.outputOn = (c != '0');
|
|
}
|
|
|
|
tDiffMicros = micros() - tprior;
|
|
|
|
if (tDiffMicros >= dt_micros){
|
|
controller.update();
|
|
controller.report();
|
|
controller.sendOutputs();
|
|
// this and the previous line can be switched if you want the PWMs to display 0 when controller off.
|
|
|
|
tprior = micros(); // maybe we have to move this line to before the update commands?
|
|
// since the floating point arithmetic may take a while...
|
|
}
|
|
} |