dmgnr/lastyear
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: add IR sensor support

Browse Source
This commit is contained in:
dmgnr
2026-05-22 08:00:54 +07:00 Unverified
parent bf14bfcf30
commit d99dba0118
12 changed files with 201 additions and 79 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/config.h
+5 -3
View File
@@ -1,10 +1,12 @@
#pragma once
#include <Arduino.h>
#include <struct.h>
#include <lib/struct.h>
// Speed, Kp, Ki, Kd
const Config configSlow = { 40, 0.05, 0.0, 0.002 };
const Config configMain = { 80, 0.05, 0.0, 0.002 };
const Config configMain = { 60, 0.07, 0.0, 0.002 };
const uint8_t SENSORS[] = {
19, 18, 17, 16, 15, 14, 11, 10, 9 };
19, 18, 17, 16, 15, 14, 11, 10, 9 };
// Can be commented out
#define IR_PIN 20
src/lib/filter.h
+29
View File
@@ -0,0 +1,29 @@
#pragma once
#include <Arduino.h>
class PT1 {
public:
PT1(float tau = 0.02f) : tau(tau), y(0), initialized(false), lastMicros(0) {}
float filter(float x) {
unsigned long now = micros();
float dt;
if (!initialized) {
initialized = true;
y = x;
lastMicros = now;
return y; }
dt = (now - lastMicros) * 1e-6f;
lastMicros = now;
float a = dt / (tau + dt);
y += a * (x - y);
return y; }
private:
float tau; // smaller = faster response
float y;
bool initialized;
unsigned long lastMicros; };
src/lib/state.h
+17
View File
@@ -0,0 +1,17 @@
#pragma once
#include <Arduino.h>
enum LoopState {
START,
TRACK,
AVOIDANCE,
END };
struct StateTimer {
uint32_t started;
void reset() {
started = millis(); }
bool elapsed(uint32_t ms) {
return millis() - started >= ms; } };
src/struct.h → src/lib/struct.h
View File
src/main.cpp
+75
View File
@@ -0,0 +1,75 @@
#include <Arduino.h>
#include <uS112.h>
#include <modules/motor.h>
#include <lib/state.h>
void setup() {
Serial.begin(115200);
uSetup();
qtrSetup();
avoidanceSetup();
if(SW()) {
Serial.println(F("EMERGENCY MODE RESETTING EEPROM"));
for (int i = 0; i < (int)EEPROM.length(); i++) {
EEPROM.write(i, 0xFF); }
Serial.println(F("DONE")); }
Serial.println(F("Loading calibration values"));
loadCalibration();
Serial.println(F("Calibration values loaded"));
printCalibration();
printDistance(); }
void loop() {
while(!SW());
long startTime = millis();
while(SW());
if (millis() - startTime > 500) {
Serial.println(F("Calibrating sensors..."));
calibrateSensors(); printCalibration();
Serial.println(F("Calibration done")); }
else {
delay(1000);
StateTimer timer;
enum LoopState state = START;
timer.reset();
while(state != END) {
switch(state) {
case START:
track(configSlow);
if (timer.elapsed(200)) {
state = TRACK;
timer.reset(); }
break;
case TRACK:
track(configMain);
// state = AVOIDANCE;
// timer.reset();
if (timer.elapsed(5000)) {
state = END;
timer.reset(); }
break;
case AVOIDANCE:
// track(configSlow);
state = TRACK;
// timer.reset();
break;
default:
break; } }
fd(0); } }
src/main.ino
-42
View File
@@ -1,42 +0,0 @@
#include <Arduino.h>
#include <uS112.h>
#include <motor.h>
void setup() {
Serial.begin(115200);
uSetup();
qtrSetup();
if(SW()) {
Serial.println(F("EMERGENCY MODE RESETTING EEPROM"));
for (int i = 0; i < EEPROM.length(); i++) {
EEPROM.write(i, 0xFF); }
Serial.println(F("DONE")); }
Serial.println(F("Loading calibration values"));
loadCalibration();
Serial.println(F("Calibration values loaded"));
printCalibration(); }
void loop() {
while(!SW());
long startTime = millis();
while(SW());
if (millis() - startTime > 500) {
Serial.println(F("Calibrating sensors..."));
calibrateSensors(); printCalibration();
Serial.println(F("Calibration done")); }
else {
delay(1000);
// TODO: replace with line tracking code
// fd(20);
// delay(20000);
track(configSlow, 500);
track(configMain, 30000);
fd(0); } }
src/modules/avoidance.h
+29
View File
@@ -0,0 +1,29 @@
#pragma once
#include <lib/filter.h>
#include <config.h>
#ifdef IR_PIN
PT1 irFilter(0.05f);
float getDistanceCM() {
int raw = analogRead(IR_PIN);
if(raw < 15) return 999;
return 2100.0 / (raw - 10.0); }
float getDistance() {
return irFilter.filter(getDistanceCM()); }
void avoidanceSetup() {
pinMode(IR_PIN, INPUT); }
void printDistance() {
uint8_t distance = getDistance();
Serial.print(F("IR Raw: ")); Serial.print(analogRead(IR_PIN));
Serial.print(F("\tDistance: ")); Serial.println(distance); }
#else
void avoidanceSetup() {}
void printDistance() {
Serial.print(F("Avoidance sensor is not configured")); }
#endif
src/input.h → src/modules/input.h
+3 -3
View File
@@ -1,7 +1,7 @@
#pragma once
#include <QTRSensors.h>
#include <EEPROM.h>
#include <config.h>
#include <modules/avoidance.h>
const int SENSOR_COUNT = sizeof(SENSORS) / sizeof(SENSORS[0]);
QTRSensors qtr;
@@ -50,7 +50,7 @@ void loadCalibration() {
addr += sizeof(uint16_t); } }
void printCalibration() {
Serial.println("============== Calibration Values ==============");
Serial.println("====== Calibration Values ======");
Serial.println("ON\tMin\tMax\tRead");
qtr.readCalibrated(sensors);
@@ -61,4 +61,4 @@ void printCalibration() {
Serial.print(qtr.calibrationOn.maximum[i]); Serial.print("\t");
Serial.println(sensors[i]); }
Serial.println("================================================"); }
Serial.println("================================"); }
src/modules/motor.h
+41
View File
@@ -0,0 +1,41 @@
#pragma once
#include <modules/input.h>
#include <QuickPID.h>
#include <us82_motor.h>
float input, output, setpoint, leftmotor, rightmotor, currSpeed;
QuickPID pid = QuickPID(&input, &output, &setpoint, 0, 0, 0, QuickPID::Action::reverse);
void pidSetup(Config config) {
if(config.speed == currSpeed)
return;
currSpeed = config.speed;
pid.SetMode(QuickPID::Control::automatic);
pid.SetTunings(config.kp, config.ki, config.kd);
const int maxOutput = 100 + config.speed;
pid.SetOutputLimits(-maxOutput, maxOutput);
pid.SetSampleTimeUs(10000); }
float line() {
return input = (qtr.readLineBlack(sensors) - ((SENSOR_COUNT - 1) * 500.0)) / (SENSOR_COUNT - 1); }
void track(Config config) {
pidSetup(config);
line();
pid.Compute();
leftmotor = config.speed + output;
rightmotor = config.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);
fd2(leftmotor, rightmotor); }
void track(Config config, unsigned long time) {
const unsigned long startTime = millis();
while (millis() - startTime < (unsigned long)time)
track(config); }
src/motor.h
-30
View File
@@ -1,30 +0,0 @@
#pragma once
#include <input.h>
#include <QuickPID.h>
#include <us82_motor.h>
float input, output, setpoint, leftmotor, rightmotor;
QuickPID pid = QuickPID(&input, &output, &setpoint, 0, 0, 0, QuickPID::Action::reverse);
void pidSetup(Config config) {
pid.SetMode(QuickPID::Control::automatic);
pid.SetTunings(config.kp, config.ki, config.kd);
const int maxOutput = 100 + config.speed;
pid.SetOutputLimits(-maxOutput, maxOutput);
pid.SetSampleTimeUs(10000); }
void track(Config config, unsigned long time) {
pidSetup(config);
const unsigned long startTime = millis();
while (millis() - startTime < (unsigned long)time) {
input = (qtr.readLineBlack(sensors) - ((SENSOR_COUNT - 1) * 500.0)) / (SENSOR_COUNT - 1);
pid.Compute();
leftmotor = config.speed + output;
rightmotor = config.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);
fd2(leftmotor, rightmotor); } }