Programme zusammenfügen

3. fertiges Programm:

Nun verbinden wir das Beispielprogramm ccc_pantilt und die eben geschriebenen Motorenprogramme. 

Das Programm für die Lenkung soll nur laufen, wenn ein Objekt erkannt wird. Es muss also innerhalb der if-Anweisung if (pixy.ccc.numBlocks) {...} stehen. Die Ansteuerung, in der der Gleichstrommotor läuft, muss auch innerhalb dieses Anweisungsblocks stehen. 

Die Ansteuerung, in der der Gleichstrommotor stehen bleibt, muss hingegen in der else {..} Anweisung stehen. 

Es ergibt sich das fertige Programm, mit dem der T1 einem Objekt autonom hinterherfahren kann:

//
// begin license header
//
// This file is part of Pixy CMUcam5 or "Pixy" for short
//
// All Pixy source code is provided under the terms of the
// GNU General Public License v2 (http://www.gnu.org/licenses/gpl-2.0.html).
// Those wishing to use Pixy source code, software and/or
// technologies under different licensing terms should contact us at
// cmucam@cs.cmu.edu. Such licensing terms are available for
// all portions of the Pixy codebase presented here.
//
// end license header
//
// verändertes Programm
//
#include <Pixy2.h>
#include <PIDLoop.h>//muss eingebunden werden
#include <ESP32Servo.h>


Pixy2 pixy;
PIDLoop panLoop(400, 0, 400, true);
PIDLoop tiltLoop(500, 0, 500, true);
Servo Servolenkung;//muss definiert werden

void setup()
{
  Serial.begin(115200);
  Serial.print("Starting...\n");
 
  // We need to initialize the pixy object 
  pixy.init();
  // Use color connected components program for the pan tilt to track 
  pixy.changeProg("color_connected_components");


  //Servo für die Lenkung bereit machen
  Servolenkung.setPeriodHertz(50);
  Servolenkung.attach(14, 500, 2500);
  
  
  //Pins für die Antriebsteuerung
  pinMode(12, OUTPUT);
  pinMode(13, OUTPUT);

  digitalWrite(13, LOW);
}

void loop()
{  
  static int i = 0;
  int j;
  char buf[64]; 
  int32_t panOffset, tiltOffset;
  
  // get active blocks from Pixy
  pixy.ccc.getBlocks();
  
  if (pixy.ccc.numBlocks)
  {        
    i++;
    
    if (i%60==0)
      Serial.println(i);   
    
    // calculate pan and tilt "errors" with respect to first object (blocks[0]), 
    // which is the biggest object (they are sorted by size).  
    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;  
  
    // update loops
    panLoop.update(panOffset);
    tiltLoop.update(tiltOffset);
  
    // set pan and tilt servos  
    pixy.setServos(panLoop.m_command, tiltLoop.m_command);

   
    //Räder sollen fahren:
    digitalWrite(12, HIGH);


    //Abschnitt der Servos:
    if(panLoop.m_command == 500)
     {
      Servolenkung.write(90);
     }
     else if((panLoop.m_command > 500) && (panLoop.m_command <= 1000))
     {
      Servolenkung.write((map(panLoop.m_command, 501, 1000, 91, 115)));
     }
     else if((panLoop.m_command < 500) && (panLoop.m_command >= 0))
     {
      Servolenkung.write((map(panLoop.m_command, 0, 499, 50, 89)));
     }
    
   
#if 0 // for debugging
    sprintf(buf, "%ld %ld %ld %ld", rotateLoop.m_command, translateLoop.m_command, left, right);
    Serial.println(buf);   
#endif

  }  
  else // no object detected, go into reset state
  {
    panLoop.reset();
    tiltLoop.reset();
    //pixy.setServos(panLoop.m_command, tiltLoop.m_command);
    

    // Räder sollen nicht fahren:
    digitalWrite(12, LOW);
  }
}