Zumo32U4/Zumo32U4/Hauptprojekt/Hauptprojekt.ino

//Verfassser: Felix Stange 4056379 MET2016
//Datum: 19.07.2017 erstellt, 11.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 (heller Belag und dunkle 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 Bluetoothkommunikation nicht genutzt werden. 

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

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

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

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

int16_t   countsLeft;                   //Encoder
int16_t   countsRight;

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

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

uint8_t   randy;                        //Zufallszahl für Richtung

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

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

//Setup Liniensensoren
void LineSensorSetup()                                            
{
  ledYellow(1);
  delay(500);
  
  //Kalibrierung
  uint32_t total[3] = {0, 0, 0};  
  for(uint8_t i = 0; i < 120; i++)                                
  {
    if (i > 30 && i <= 90) motors.setSpeeds(200, -200);
    else motors.setSpeeds(-200, 200);
    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 Drehbewegungssensor
void TurnSensorSetup()                                            
{                                                                 
  ledYellow(1);
  delay(500);
                                                                  //800Hz Ausgaberate
  gyro.writeReg(L3G::CTRL1, 0b11111111);                          //Tiefpassfilter bei 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;
  }
  ledYellow(0);
  gyroOffset = total / 1024;
}

//Setup Beschleunigungssensor
void MoveSensorSetup()                                            
{
  ledYellow(1);
  delay(500);

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

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

  //Initialisierung der Sensoren
  lineSensors.initThreeSensors();                                   
  proxSensors.initThreeSensors();
  Wire.begin();
  compass.init();
  compass.enableDefault();  
  gyro.init();

  //Kalibrierung der Sensoren
  Serial1.println("sensor calibration");                            
  buttonA.waitForButton();
  LineSensorSetup();                                                
  TurnSensorSetup();
  gyroLastUpdate = micros();
  MoveSensorSetup();
  compassLastUpdate = micros();  
  
  //Zumo bereit zu starten
  Serial1.println("Zumo ready to start");
  buttonA.waitForButton();

  delay(500);
}

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

//Update Drehbewegungssensor
void TurnSensorUpdate()                                                 
{
  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;
  turnAngle = (((int32_t)turnAngle >> 16) * 360) >> 16;
}

// Update Beschleunigungssensor
void MoveSensorUpdate()
{
  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 / 16384;
  moveDistance = (moveDistance * 1000) / 9,81;
}

//Update Encoder
//12cpr (Motorwelle) * 75,81:1 (Getriebe) = 909,7cpr (Antriebszahnrad)
void EncoderUpdate()
{
  static uint8_t lastDisplayTime;
  if ((uint8_t)(millis() - lastDisplayTime) >= 100)
  {
    lastDisplayTime = millis();
    countsLeft = encoders.getCountsLeft();
    countsRight = encoders.getCountsRight();
  }
}

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

 //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
  motors.setSpeeds(maxSpeed/2, maxSpeed);                             
  turnAngle = 0;                                                      
  TurnSensorUpdate();                                                 
  while(lineValues[2] < (lineOffset[2] -200))                
  {
    lineSensors.read(lineValues);
  }

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

  //Gegenverkehr beachten
  proxSensors.read();                                                 
  proxValues[1] = proxSensors.countsFrontWithLeftLeds();
  proxValues[2] = proxSensors.countsFrontWithRightLeds();  
  if((proxValues[1] || proxValues[2]) < 4)
  {
    //Schritt 3: geradeaus fahren bis Hindernis passiert
    maxSpeed = 400;
    motors.setSpeeds(maxSpeed, maxSpeed);                             
    delay(1000);
    proxSensors.read();
    proxValues[3] = proxSensors.countsRightWithRightLeds();  
    while(proxValues[3] > 4)
    {
      proxSensors.read();
      proxValues[3] = proxSensors.countsRightWithRightLeds();  
      TrackKeeper();
    }
    maxSpeed = 200;
  }
  
  //Schritt 4: rechts bis über Mittellinie fahren
  motors.setSpeeds(maxSpeed/2, maxSpeed);                             
  ledYellow(1);
  turnAngle = 0;                                                      
  TurnSensorUpdate();                                                 
  while(lineValues[0] < (lineOffset[0] -200))                
  {
    lineSensors.read(lineValues);
  }

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

//Funktion Spurhalten
void TrackKeeper()                                                   
{

  //linke Linie erreicht, nach rechts fahren
  if(lineValues[0] > (lineOffset[0] + 200))                          
  {
    motors.setSpeeds(maxSpeed, 0);
    ledYellow(1);
    delay(200);    
    dir = 'R';
  }

  //rechte Linie erreicht, nach links fahren
  else if(lineValues[2] > (lineOffset[2] + 200))                     
  {
    motors.setSpeeds(0, maxSpeed);  
    ledYellow(1); 
    delay(200); 
    dir = 'L';     
  }

  //Linie überfahren, zurücksetzen
  else if(lineValues[1] > (lineOffset[1] + 200))                     
  { 
    motors.setSpeeds(-maxSpeed/2, -maxSpeed/2);
    ledRed(1); 
    delay(1000); 
    dir = 'B';     
  }  

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

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

  //Zufallszahl erzeugen
  randy = random(3);

  //Kodierung analysieren


  //links Abbiegen
  if(randy == 1) 
  {
    //zur Kreuzungsmitte fahren
    while((countsLeft != 200) && (countsRight != 200))
    {
      EncoderUpdate();
      motors.setSpeeds(maxSpeed/2, maxSpeed/2);
    }

    //links drehen
    turnAngle = 0;
    TurnSensorUpdate();
    while(turnAngle != 90)
    {
    motors.setSpeeds(0, maxSpeed);  
    TurnSensorUpdate();
    }

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

  //rechts Abbiegen
  else if(randy == 2) 
  {
    //rechts drehen
    turnAngle = 0;
    TurnSensorUpdate();
    while(turnAngle != 90)
    {
    motors.setSpeeds(maxSpeed, 0);  
    TurnSensorUpdate();
    }

    //geradeaus fahren
    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   Beschleunigung: %6d   Winkel: %6d"),
    proxValues[0], proxValues[1], proxValues[2], proxValues[3], 
    lineValues[0], lineValues[1], lineValues[2],
    dir, moveDistance, turnAngle);
  Serial1.println(report);
}

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

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

  //Abfragen der Sensordaten
  lineSensors.read(lineValues);                                           
  proxSensors.read();
  proxValues[0] = proxSensors.countsLeftWithLeftLeds();
  proxValues[1] = proxSensors.countsFrontWithLeftLeds();
  proxValues[2] = proxSensors.countsFrontWithRightLeds();  
  proxValues[3] = proxSensors.countsRightWithRightLeds();  
  TurnSensorUpdate();
  MoveSensorUpdate();

  //Funktionsauswahl
/*   if((compass.a.x - compassOffset) > 4000) CollisionDetection();         
  else if((proxValues[0] || proxValues[1]) > 4) ObstacleAvoidance();
  else if((lineValues[0] > (lineOffset[0] + 200)) && (lineValues[1] > (lineOffset[1] + 200)) 
    && (lineValues[2] > (lineOffset[2] + 200))) Crossroad();
  else TrackKeeper(); */

  //Ausgabe über Bluetoothverbindung
  SerialOutput();                                                         
}