Zumo32U4/Zumo32U4/Hauptprojekt/Hauptprojekt.ino

//Verfassser: Felix Stange 4056379 MET2016
//Datum: 19.07.2017 erstellt, 20.10.2017 zuletzt geändert
//Projekt: Der Zumo fährt automatisch zwischen 2 Linien ohne diese zu überfahren mithilfe der 
//Liniensensoren (3), ähnlich einer Spurhalteautomatik (dunkler Belag und helle Streifen). 
//Außerdem werden Kollisionen verhindert durch Nutzung der vorderen Abstandssensoren. Kommt es 
//dennoch zu einer Kollision, wird diese durch die Beschleunigungssensoren (LSM303) erkannt. 
//Für den Überholvorgang werden die seitlichen Abstandssensoren und der Drehbewegungssensor (L3G) 
//genutzt. Mithilfe der Quadraturencoder in den Motoren können Wegstrecken vermessen werden. 
//Der Zumo kommuniziert über ein Bluetooth-Modul (HC-05) mit anderen Geräten. Die 
//Kommunikation erfolgt seriell ('SERIAL' für USB, 'SERIAL1' für Bluetooth). 
//Das LCD kann bei Bluetoothnutzung nicht verwendet werden. 

#include <Wire.h>
#include <Zumo32U4.h>

Zumo32U4ProximitySensors  proxSensors;  //Abstandssensoren
Zumo32U4LineSensors       lineSensors;  //Liniensensoren
Zumo32U4Motors            motors;
Zumo32U4ButtonA           buttonA;
Zumo32U4Encoders          encoders;     //Motorencoder
LSM303                    compass;      //Beschleunigungssensor x-Achse
L3G                       gyro;         //Drehbewegungssensor z-Achse

uint16_t  lineValue[3];                 //von links (0) nach rechts (2)
uint16_t  lineOffset[3]; 
uint16_t  lineRaw[3];

uint8_t   proxValue[4];                 //von links (0) nach rechts (3)

int16_t   encoderCounts[2];             //von links (0) nach rechts (1)
int16_t   driveRotation[2];

int32_t   rotationAngle = 0;            //Drehwinkel
int32_t   turnAngle = 0; 
int16_t   turnRate;
int16_t   gyroOffset;
uint16_t  gyroLastUpdate;

int32_t   moveDisplay = 0;              //Beschleunigung
int32_t   moveDistance = 0;   
int16_t   moveRate;
int16_t   compassOffset;                
uint16_t  compassLastUpdate;

uint8_t   randy;                        //Richtungsauswahl Abbiegen geradeaus => 0
bool      leftCode;                     //                          links => 1
bool      rightCode;                    //                          rechts => 2

int       maxSpeed = 200;               //Maximale Geschwindigkeit (möglich 400)
char      dir = '0';                    //Fahrtrichtung, Ereignis
char      report[200];                  //Ausgabe

/*-----------------------------------------------------------------*/

//Setup Liniensensoren
void LineSetup()                                            
{
  ledYellow(1);
  lineSensors.initThreeSensors();     
  
  //Kalibrierung
  uint32_t total[3] = {0, 0, 0};  
  for(uint8_t i = 0; i < 120; i++)                                
  {
    if (i > 30 && i <= 90) motors.setSpeeds(maxSpeed, -maxSpeed);
    else motors.setSpeeds(-maxSpeed, maxSpeed);
    lineSensors.read(lineOffset);  
    total[0] += lineOffset[0];
    total[1] += lineOffset[1];
    total[2] += lineOffset[2];
    lineSensors.calibrate();                                      
  }
  ledYellow(0);
  motors.setSpeeds(0, 0);
  lineOffset[0] = total[0] / 120;
  lineOffset[1] = total[1] / 120;
  lineOffset[2] = total[2] / 120;
}

//Setup Abstandssensoren
void ProxSetup()
{
  proxSensors.initThreeSensors();  
}

//Setup Drehbewegungssensor
void GyroSetup()                                            
{                                                                 
  ledYellow(1);
  gyro.init();

  gyro.writeReg(L3G::CTRL1, 0b11111111);                          //Ausgaberate 800Hz, Tiefpassfilter 100Hz
  gyro.writeReg(L3G::CTRL4, 0b00100000);                          //2000dps Auflösung
  gyro.writeReg(L3G::CTRL5, 0b00000000);                          //Hochpassfilter ausgeschaltet

  //Kalibrierung
  int32_t total = 0;                                              
  for (uint16_t i = 0; i < 1024; i++)
  {
    while(!gyro.readReg(L3G::STATUS_REG) & 0x08);
    gyro.read();
    total += gyro.g.z;
  }
  gyroOffset = total / 1024;

  gyroLastUpdate = micros();
  ledYellow(0);
}

//Setup Beschleunigungssensor
void CompassSetup()                                            
{
  ledYellow(1);
  compass.init();
  compass.enableDefault();  

  //Kalibrierung
  int32_t total = 0;                                              
  for (uint16_t i = 0; i < 1024; i++)
  {
    compass.read();
    total += compass.a.x;
  }
  compassOffset = total / 1024;

  compassLastUpdate = micros();  
  ledYellow(0);
}

/*-----------------------------------------------------------------*/

void setup() 
{
  //Initialisierung der Bluetoothverbindung
  Serial1.begin(9600);                                              
  if(Serial1.available()) Serial1.println("bluetooth available");  

  //Initialisierung und Kalibrierung der Sensoren
  Serial1.println("sensor calibration, dont touch");   
  Wire.begin();                       
  delay(500);
  ProxSetup();
  LineSetup();                                                
  GyroSetup();
  CompassSetup();
  
  //Zumo bereit zu starten
  Serial1.println("Zumo ready to start, press A");
  ledGreen(1);
  buttonA.waitForButton();
  randomSeed(millis());
  delay(500);
}

/*-----------------------------------------------------------------*/

//Update Abstandssensoren
void ProxUpdate()
{
  proxSensors.read();
  proxValue[0] = proxSensors.countsLeftWithLeftLeds();
  proxValue[1] = proxSensors.countsFrontWithLeftLeds();
  proxValue[2] = proxSensors.countsFrontWithRightLeds();  
  proxValue[3] = proxSensors.countsRightWithRightLeds(); 
}

//Updaten Liniensensoren
void LineUpdate()
{
  lineSensors.read(lineRaw);
  lineValue[0] = lineRaw[0] - lineOffset[0];
  lineValue[1] = lineRaw[1] - lineOffset[1];
  lineValue[2] = lineRaw[2] - lineOffset[2];
}

//Update Drehbewegungssensor
void GyroUpdate()                                                 
{
  gyro.read();
  turnRate = gyro.g.z - gyroOffset;
  uint16_t m = micros();
  uint16_t dt = m - gyroLastUpdate;
  gyroLastUpdate = m;
  int32_t d = (int32_t)turnRate * dt;
  turnAngle += (int64_t)d * 14680064 / 17578125;
  rotationAngle = (((int32_t)turnAngle >> 16) * 360) >> 16;
}

// Update Beschleunigungssensor
void CompassUpdate()
{
  compass.read();
  moveRate = compass.a.x - compassOffset;
  uint16_t m = micros();
  uint16_t dt = m - compassLastUpdate;
  compassLastUpdate = m;
  int32_t d = (int32_t)moveRate * dt;
  moveDistance += (int64_t)d * dt >> 14;
  moveDisplay = moveDistance * 1000 / 9,81;
}

//Update Encoder
void EncoderUpdate()
{
  encoderCounts[0] = encoders.getCountsLeft();
  driveRotation[0] = encoderCounts[0] / 910;                                //12cpr (Motorwelle) * 75,81:1 (Getriebe) = 909,7cpr (Antriebszahnrad)
  encoderCounts[1] = encoders.getCountsRight();
  driveRotation[1] = encoderCounts[1] / 910;
}

/*-----------------------------------------------------------------*/

 //Funktion Kollisionserkennung
void CollisionDetection()                                            
{
  dir = 'X';
  Serial1.println("impact detected");
  ledRed(1);

  motors.setSpeeds(0, 0);
  delay(500);
  motors.setSpeeds(-maxSpeed/2, -maxSpeed/2);
  delay(1000); 
}

//Funktion Hindernisumfahrung
void ObstacleAvoidance()                                              
{
  dir = 'U';
  Serial1.println("obstacle avoidance");  
  ledYellow(1);

  //Schritt 1: links bis über Mittellinie fahren                            
  rotationAngle = 0;                                                      
  GyroUpdate();                                                 
  while(lineValue[2] > 1000)              
  {
    LineUpdate();
    motors.setSpeeds(maxSpeed/2, maxSpeed); 
  }
  motors.setSpeeds(maxSpeed, maxSpeed);   

  //Schritt 2: rechts fahren bis Fahrzeug gerade steht ohne dabei weitere Linien zu überfahren                            
  while(rotationAngle != 0)                                               
  {                                                                   
    GyroUpdate();
    motors.setSpeeds(maxSpeed, maxSpeed/2); 
  }

  //Gegenverkehr beachten
  proxSensors.read();                                                 
  proxValue[1] = proxSensors.countsFrontWithLeftLeds();
  proxValue[2] = proxSensors.countsFrontWithRightLeds();  
  if(proxValue[1] < 4 || proxValue[2] < 4)
  {
    //Schritt 3: geradeaus fahren bis Hindernis passiert
    maxSpeed = 400;
    motors.setSpeeds(maxSpeed, maxSpeed);                             
    delay(1000);
    proxSensors.read();
    proxValue[3] = proxSensors.countsRightWithRightLeds();  
    while(proxValue[3] > 4)
    {
      motors.setSpeeds(maxSpeed, maxSpeed);  
      proxSensors.read();
      proxValue[3] = proxSensors.countsRightWithRightLeds();  
      TrackKeeper();
    }
    maxSpeed = 200;
  }
  
  //Schritt 4: rechts bis über Mittellinie fahren                           
  rotationAngle = 0;                                                      
  GyroUpdate();                                                 
  while(lineValue[0] > 1000)     
  {
    LineUpdate();
    motors.setSpeeds(maxSpeed, maxSpeed/2);  
  }
  motors.setSpeeds(maxSpeed, maxSpeed);   

  //Schritt 5: links fahren bis Fahrzeug gerade steht ohne dabei weitere Linien zu überfahren
  motors.setSpeeds(maxSpeed/2, maxSpeed);                             
  while(rotationAngle != 0)                                               
  {                                                                   
    GyroUpdate();
    TrackKeeper();
  }  
}

//Funktion Spurhalten
void TrackKeeper()                                                   
{

  //linke Linie erreicht, nach rechts fahren
  if(lineValue[0] < 1000)                      
  {
    motors.setSpeeds(maxSpeed, maxSpeed/2);
    ledYellow(1);
    delay(100);    
    dir = 'R';
  }

  //rechte Linie erreicht, nach links fahren
  else if(lineValue[2] < 1000)                 
  {
    motors.setSpeeds(maxSpeed/2, maxSpeed);  
    ledYellow(1); 
    delay(100); 
    dir = 'L';     
  }

  //Linie überfahren, zurücksetzen
  else if(lineValue[1] < 1000)                 
  { 
    ledRed(1); 
    while(lineValue[0] > 1000 && lineValue[2] > 1000)
    {
      motors.setSpeeds(-maxSpeed/2, -maxSpeed/2);
    }
    delay(500); 
    dir = 'B';     
  }  

  //Normale Fahrt
  else                                                                
  {
    motors.setSpeeds(maxSpeed, maxSpeed);    
    ledGreen(1); 
    dir = 'F';     
  }
}

//Funktion Abbiegen
void Crossroad()
{
  ledYellow(1); 
  dir = 'A';

  //Kodierung analysieren
  if(lineValue[0] > 500 && lineValue[0] < 1500) leftCode = true;
  else leftCode = false;
  if(lineValue[2] > 500 && lineValue[2] < 1500) rightCode = true;
  else rightCode = false;

  //Zufallszahl erzeugen
  if(leftCode == true && rightCode == true) randy = random(1, 3);         //1, 2
  else if(leftCode == true && rightCode == false) randy = random(0, 2);   //0, 1
  else if(leftCode == false && rightCode == true)
  {
    randy = random(3);                                                    //0, (1), 2
    while(randy == 1) randy = random(3);                                  //!1 => 0, 2
  }

  //links Abbiegen
  if(randy == 1 && leftCode == true) 
  {
    //zur Kreuzungsmitte fahren
    driveRotation[0] = 0;
    driveRotation[1] = 0;
    while(driveRotation[0] != 1 && driveRotation[1] != 1)
    {
      EncoderUpdate();
      motors.setSpeeds(maxSpeed/2, maxSpeed/2);
    }

    //links drehen
    rotationAngle = 0;
    GyroUpdate();
    while(rotationAngle != 90)
    {
      GyroUpdate();
      motors.setSpeeds(0, maxSpeed/2);  
    }

    //geradeaus fahren
    motors.setSpeeds(maxSpeed/2, maxSpeed/2); 
  }

  //rechts Abbiegen
  else if(randy == 2 && rightCode == true) 
  {
    //rechts drehen
    rotationAngle = 0;
    GyroUpdate();
    while(rotationAngle != -90)
    {
      GyroUpdate();
      motors.setSpeeds(maxSpeed/2, 0);  
    }

    //geradeaus fahren
    motors.setSpeeds(maxSpeed/2, maxSpeed/2); 
  }  

  //nicht Abbiegen, geradeaus fahren
  else motors.setSpeeds(maxSpeed/2, maxSpeed/2); 
}

//Funktion Serielle Ausgabe
void SerialOutput()                                                     
{
  snprintf_P(report, sizeof(report),
    PSTR("Abstand: %3d %3d %3d %3d   Linie: %6d %6d %6d   Richtung: %3c   Weg: %6d %6d   Winkel: %6d"),
    proxValue[0], proxValue[1], proxValue[2], proxValue[3], 
    lineValue[0], lineValue[1], lineValue[2],
    dir, driveRotation[0], driveRotation[1], rotationAngle);
  Serial1.println(report);
}

/*-----------------------------------------------------------------*/

void loop() 
{
  ledGreen(0);
  ledYellow(0);
  ledRed(0);

  //Abfragen der Sensordaten
  LineUpdate();                                         
  ProxUpdate();
  GyroUpdate();
  CompassUpdate();
  EncoderUpdate();

  //Funktionsauswahl
  if(moveRate > 1000) CollisionDetection();         
  else if(proxValue[0] == 6 || proxValue[1] == 6) motors.setSpeeds(0, 0);  
  else if(proxValue[0] > 4 || proxValue[1] > 4) ObstacleAvoidance();
  else if((lineValue[0] > 500 && lineValue[0] < 1500) || (lineValue[2] > 500 && lineValue[2] < 1500)) Crossroad();
  else TrackKeeper();

  //Ausgabe über Bluetoothverbindung
  SerialOutput();                                                         
}