Pixycam Follower Code

#include <PIDLoop.h>
#include <Pixy2I2C.h>

// Variablen für die Distanzmessung
float knownWidth = 12.0;      // Tatsächliche Breite des Objekts
float focalLength = 60.0;     // Brennweite der PixyCam
float allowedDistance = 10;   // Maximal zulässige Distanz zum Objekt

Pixy2I2C pixy;
PIDLoop panLoop(180, 0, 180, true);  // Initialisierung des PID-Regelkreises für den Pan-Winkel
PIDLoop tiltLoop(180, 0, 180, true); // Initialisierung des PID-Regelkreises für den Tilt-Winkel

typedef struct
{
  int motor_rightspeed;
  int motor_leftspeed;
} MS;

int OutputMspeedR = 6;
int OutputMotorRa = 4;
int OutputMotorRb = 5;
int OutputMspeedL = 9;
int OutputMotorLa = 8;
int OutputMotorLb = 7;

void setup()
{
  // Motoren initialisieren
  pinMode(OutputMspeedL, OUTPUT);
  pinMode(OutputMspeedR, OUTPUT);
  pinMode(OutputMotorLa, OUTPUT);
  pinMode(OutputMotorRa, OUTPUT);
  pinMode(OutputMotorLb, OUTPUT);
  pinMode(OutputMotorRb, OUTPUT);
  // Initialize motor direction to forward
  setMotors("Vorne");
  analogWrite(OutputMspeedR, 0); // Ensure motors are initially stopped
  analogWrite(OutputMspeedL, 0); // Ensure motors are initially stopped
  Serial.begin(9600);
  Serial.print("Setted pins...\n");
  pixy.init();                                   // Initialisierung der Pixy2
  pixy.changeProg("color_connected_components"); // Änderung des Pixy2-Modus auf "color_connected_components"
}

void loop()
{
  static int i = 0;
  int32_t panOffset, tiltOffset;

  // Aktive Blöcke von Pixy2 erhalten
  pixy.ccc.getBlocks();
  // Wenn ein oder mehrere Blöcke erfast worden.
  if (pixy.ccc.numBlocks)
  {
    i++;
    panOffset = (int32_t)pixy.frameWidth / 2 - (int32_t)pixy.ccc.blocks[0].m_x;
    tiltOffset = (int32_t)pixy.ccc.blocks[0].m_y - (int32_t)pixy.frameHeight / 2;
    tiltOffset = -tiltOffset;
    int blockWidth = pixy.ccc.blocks[0].m_width;
    int blocksHeight = pixy.ccc.blocks[0].m_height;

    float distance = (knownWidth * focalLength) / blockWidth;

    panLoop.update(panOffset);
    tiltLoop.update(tiltOffset);

    pixy.setServos(panLoop.m_command, tiltLoop.m_command);

    if (distance < allowedDistance)
    {
      analogWrite(OutputMspeedR, 140);
      analogWrite(OutputMspeedL, 140);
      setMotors("Hinten");
      return;
    }
    panLoop.update(panOffset);
    tiltLoop.update(tiltOffset);
    MS MotorSpeed = adjustMotorSpeed();

    // Set motor speeds
    analogWrite(OutputMspeedL, MotorSpeed.motor_leftspeed);
    analogWrite(OutputMspeedR, MotorSpeed.motor_rightspeed);
    setMotors("Vorne");
  }
  else
  {
    analogWrite(OutputMspeedL, 0);
    analogWrite(OutputMspeedR, 0);
    pixy.setServos(panLoop.m_command, tiltLoop.m_command);
  }
}

MS adjustMotorSpeed()
{
  // Definition der Konstanten für die Pan/Tilt-Werte und Motorgeschwindigkeiten
  const int minValue = 0;
  const int maxValue = 1000;
  const int minSpeed = 40;    // Minimale Motorgeschwindigkeit
  const int maxSpeed = 180;   // Maximale Motorgeschwindigkeit

  // Aktuelle Befehle von den PID-Regelkreisen abrufen
  int currentCommand = panLoop.m_command;
  int currentTilt = tiltLoop.m_command;

  // Servos der Pixy2-Kamera mit den aktuellen Winkeln einstellen
  int motor_rightspeed = map(currentCommand, minValue, maxValue, minSpeed, maxSpeed);
  int motor_leftspeed = map(currentCommand, minValue, maxValue, maxSpeed, minSpeed);

  // Speichern und zurückgeben der berechneten Motorgeschwindigkeiten
  MS speed;
  setMotors("Vorne");

  Serial.print("Linke Geschwindigkeit: ");
  Serial.println(motor_leftspeed);
  Serial.print("Rechte Geschwindigkeit: ");
  Serial.println(motor_rightspeed);

  speed.motor_rightspeed = motor_rightspeed;
  speed.motor_leftspeed = motor_leftspeed;
  return speed;
}

void setMotors(const char *richtung)
{
  // Setze beide Motoren so dass sie nach Vorne fahren
  if (strcmp(richtung, "Vorne") == 0)
  {
    digitalWrite(OutputMotorLa, HIGH);
    digitalWrite(OutputMotorLb, LOW);
    digitalWrite(OutputMotorRa, HIGH);
    digitalWrite(OutputMotorRb, LOW);
  }
  // Setze beide Motoren so dass sie nach Hinten fahren
  if (strcmp(richtung, "Hinten") == 0)
  {
    digitalWrite(OutputMotorLa, LOW);
    digitalWrite(OutputMotorLb, HIGH);
    digitalWrite(OutputMotorRa, LOW);
    digitalWrite(OutputMotorRb, HIGH);
  }
}