lastyear

src/config.h

@@ -0,0 +1,10 @@
+#pragma once
+#include <Arduino.h>
+
+#define SPEED 40 // Base speed
+#define KP 0.05 // Proportional gain
+#define KI 0.0 // Integral gain
+#define KD 0.002 // Derivative gain
+
+const uint8_t SENSORS[] = {
+  19, 18, 17, 16, 15, 14, 11, 10, 9 };

src/input.h

@@ -0,0 +1,64 @@
+#pragma once
+#include <QTRSensors.h>
+#include <EEPROM.h>
+#include <config.h>
+
+const int SENSOR_COUNT = sizeof(SENSORS) / sizeof(SENSORS[0]);
+QTRSensors qtr;
+uint16_t sensors[SENSOR_COUNT];
+
+void qtrSetup() {
+  qtr.setTypeRC();
+  qtr.setSensorPins(SENSORS, SENSOR_COUNT); }
+
+void calibrateSensors() {
+  unsigned long startTime = millis();
+
+  for (int i = 0; i < SENSOR_COUNT; i++) {
+    qtr.calibrationOn.minimum[i]  = 1023;
+    qtr.calibrationOn.maximum[i]  = 0; }
+
+  while (millis() - startTime < 5000) {
+    Serial.println(F("calibrate"));
+    qtr.calibrate();
+    delay(20); }
+
+  Serial.println(F("Saving values..."));
+  int addr = 0;
+
+  for (int i = 0; i < SENSOR_COUNT; i++) {
+    EEPROM.put(addr, qtr.calibrationOn.minimum[i]);
+    addr += sizeof(uint16_t);
+
+    EEPROM.put(addr, qtr.calibrationOn.maximum[i]);
+    addr += sizeof(uint16_t); } }
+
+void loadCalibration() {
+  qtr.calibrationOn.initialized = true;
+  qtr.calibrationOff.initialized = true;
+
+  qtr.calibrationOn.minimum = new uint16_t[SENSOR_COUNT];
+  qtr.calibrationOn.maximum = new uint16_t[SENSOR_COUNT];
+
+  int addr = 0;
+
+  for (int i = 0; i < SENSOR_COUNT; i++) {
+    EEPROM.get(addr, qtr.calibrationOn.minimum[i]);
+    addr += sizeof(uint16_t);
+
+    EEPROM.get(addr, qtr.calibrationOn.maximum[i]);
+    addr += sizeof(uint16_t); } }
+
+void printCalibration() {
+  Serial.println("============== Calibration Values ==============");
+  Serial.println("ON\tMin\tMax\tRead");
+
+  qtr.readCalibrated(sensors);
+
+  for (int i = 0; i < SENSOR_COUNT; i++) {
+    Serial.print(i); Serial.print("\t");
+    Serial.print(qtr.calibrationOn.minimum[i]); Serial.print("\t");
+    Serial.print(qtr.calibrationOn.maximum[i]); Serial.print("\t");
+    Serial.println(sensors[i]); }
+
+  Serial.println("================================================"); }

src/main.ino

@@ -0,0 +1,42 @@
+#include <Arduino.h>
+#include <uS112.h>
+#include <motor.h>
+
+void setup() {
+  Serial.begin(115200);
+  uSetup();
+  qtrSetup();
+  pidSetup();
+
+  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(30000);
+    fd(0); } }

src/motor.h

@@ -0,0 +1,28 @@
+#pragma once
+#include <input.h>
+#include <QuickPID.h>
+#include <us82_motor.h>
+float input, output, setpoint, leftmotor, rightmotor;
+
+QuickPID pid = QuickPID(&input, &output, &setpoint, KP, KI, KD, QuickPID::Action::reverse);
+
+void pidSetup() {
+  pid.SetMode(QuickPID::Control::automatic);
+  const int maxOutput = 100 + SPEED;
+  pid.SetOutputLimits(-maxOutput, maxOutput);
+  pid.SetSampleTimeUs(10000); }
+
+void track(unsigned long time) {
+  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 = SPEED + output;
+    rightmotor = 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); } }