Open Source Lab

void setup() { Serial.begin(9600); }
void loop() {
  for (int i = 0; i < 8; i++) {
    Serial.print(analogRead(i));
    Serial.print("\t");
  }
  Serial.println();
  delay(100);
}

/*
8A Sensor Robot Firmware
Logic: Immediate start, 400 threshold, PID control.
*/

#ifndef CLEAR_BIT
#define CLEAR_BIT(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
#endif
#ifndef SET_BIT
#define SET_BIT(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))
#endif

// Pin Definitions
#define MOTOR_A_PWM 9
#define MOTOR_A_IN1 8
#define MOTOR_A_IN2 7
#define MOTOR_B_PWM 3
#define MOTOR_B_IN1 5
#define MOTOR_B_IN2 4

// Global Configuration
int finalError = 0;
bool darkLineMode = 1;         
unsigned int sensorCount = 8;      

// PID Components
int propPart, derPart, intPart, lastReading, driveCorrection;
double currentDeviation;
int pwrLeft, pwrRight;
int topSpeed = 175;
int velocity = 30;
int weightFactors[8] = { 4, 3, 2, 1, -1, -2, -3, -4 };
int sensorsTriggered;

// PID Gains
float gainP = 0.13;
float gainD = 2.4;
float gainI = 0;

int lineDetected = 1;

// Calibration Bounds
int floorLevels[8] = {0, 0, 0, 0, 0, 0, 0, 0}; 
int peakLevels[8] = {1023, 1023, 1023, 1023, 1023, 1023, 1023, 1023}; 
int triggerPoint[8], valBuffer[8], digitalState[8];

void setup() {
  SET_BIT(ADCSRA, ADPS2);
  CLEAR_BIT(ADCSRA, ADPS1);
  CLEAR_BIT(ADCSRA, ADPS0);

  Serial.begin(9600);

  pinMode(MOTOR_A_IN1, OUTPUT);
  pinMode(MOTOR_A_IN2, OUTPUT);
  pinMode(MOTOR_B_IN1, OUTPUT);
  pinMode(MOTOR_B_IN2, OUTPUT);
  pinMode(MOTOR_A_PWM, OUTPUT);
  pinMode(MOTOR_B_PWM, OUTPUT);
  
  pinMode(13, OUTPUT); // Status LED

  pinMode(5, OUTPUT); 
  digitalWrite(5, HIGH); // Driver Enable
}

void loop() {
  while (1) {
    updateSensorData();
    
    if (velocity < topSpeed) velocity++;
    
    if (lineDetected == 1) { 
      applyPIDControl();
      digitalWrite(13, HIGH);
    } 
    else {
      digitalWrite(13, LOW);
      int pivotPower = 150;

      // Search pattern until line is re-acquired
      while (lineDetected == 0) {
        if (finalError < 0) {
          moveMotorA(-pivotPower);
          moveMotorB(pivotPower);
        } else {
          moveMotorA(pivotPower);
          moveMotorB(-pivotPower);
        }
        updateSensorData(); 
      }
    }
  }
}

void executeBrake(int direction) {
  if (direction <= 0) {
    moveMotorA(180);
    moveMotorB(-180);
  }
  delay(15); 
}

void applyPIDControl() {
  currentDeviation = 0;
  sensorsTriggered = 0;

  for (int i = 0; i < 8; i++) {
    currentDeviation += weightFactors[i] * digitalState[i] * valBuffer[i];
    sensorsTriggered += digitalState[i];
  }
  
  if (sensorsTriggered != 0) {
    currentDeviation = currentDeviation / sensorsTriggered;
    finalError = currentDeviation;
  }

  propPart = currentDeviation;
  intPart = intPart + currentDeviation;
  derPart = currentDeviation - lastReading;

  driveCorrection = (gainP * propPart) + (gainI * intPart) + (gainD * derPart);
  lastReading = currentDeviation;

  pwrLeft = velocity - driveCorrection;
  pwrRight = velocity + driveCorrection;

  // Constrain limits for motor stability
  if (pwrLeft > 255) pwrLeft = 255;
  if (pwrLeft < -255) pwrLeft = -255;
  if (pwrRight > 255) pwrRight = 255;
  if (pwrRight < -255) pwrRight = -255;

  moveMotorA(pwrRight);
  moveMotorB(pwrLeft);
}

void updateSensorData() {
  lineDetected = 0;

  for (int i = 0; i < 8; i++) {
    if (darkLineMode) {
      valBuffer[i] = map(analogRead(i), floorLevels[i], peakLevels[i], 0, 1000);
    } else {
      valBuffer[i] = map(analogRead(i), floorLevels[i], peakLevels[i], 1000, 0);
    }
    
    valBuffer[i] = constrain(valBuffer[i], 0, 1000);
    
    // Threshold comparison (Fixed at 400)
    digitalState[i] = valBuffer[i] > 400;

    if (digitalState[i]) lineDetected = 1;
  }
}

void moveMotorA(int speedValue) {
  speedValue = constrain(speedValue, -255, 255);
  if (speedValue > 0) {
    digitalWrite(MOTOR_A_IN1, 0);
    digitalWrite(MOTOR_A_IN2, 1);
    analogWrite(MOTOR_A_PWM, speedValue);
  } else if (speedValue < 0) {
    digitalWrite(MOTOR_A_IN1, 1);
    digitalWrite(MOTOR_A_IN2, 0);
    analogWrite(MOTOR_A_PWM, abs(speedValue));
  } else {
    digitalWrite(MOTOR_A_IN1, 1);
    digitalWrite(MOTOR_A_IN2, 1);
    analogWrite(MOTOR_A_PWM, 0);
  }
}

void moveMotorB(int speedValue) {
  speedValue = constrain(speedValue, -255, 255);
  if (speedValue > 0) {
    digitalWrite(MOTOR_B_IN1, 0);
    digitalWrite(MOTOR_B_IN2, 1);
    analogWrite(MOTOR_B_PWM, speedValue);
  } else if (speedValue < 0) {
    digitalWrite(MOTOR_B_IN1, 1);
    digitalWrite(MOTOR_B_IN2, 0);
    analogWrite(MOTOR_B_PWM, abs(speedValue));
  } else {
    digitalWrite(MOTOR_B_IN1, 1);
    digitalWrite(MOTOR_B_IN2, 1);
    analogWrite(MOTOR_B_PWM, 0);
  }
}

// Unused reference calibration
void performManualCalib() {
  for (int i = 0; i < 8; i++) {
    floorLevels[i] = analogRead(i);
    peakLevels[i] = analogRead(i);
  }

  for (int i = 0; i < 10000; i++) {
    moveMotorA(70);
    moveMotorB(-70);

    for (int j = 0; j < 8; j++) {
      int readVal = analogRead(j);
      if (readVal < floorLevels[j]) floorLevels[j] = readVal;
      if (readVal > peakLevels[j]) peakLevels[j] = readVal;
    }
  }
  moveMotorA(0);
  moveMotorB(0);
}