Zumo32U4/Zumo32U4/Hauptprojekt/Hauptprojekt.ino

//Verfassser: Felix Stange MET2016
//Datum: 20.07.2017
//Projekt: Der Zumo fährt automatisch zwischen 2 Linien ohne diese zu überfahren mithilfe der 
//Liniensensoren (5), ähnlich einer Spurhalteautomatik (dunkler Belag und hellen Streifen). 
//Außerdem werden Kollisionen verhindert durch Nutzung der vorderen Abstandssensoren. Kommt es 
//dennoch zu einer Kollision, wird diese durch die Beschleunigungssensoren erkannt. 
//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). 

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

Zumo32U4ProximitySensors proxSensors;
Zumo32U4LineSensors lineSensors;
Zumo32U4Motors motors;
Zumo32U4LCD lcd;
Zumo32U4ButtonA buttonA;
LSM303 compass;

uint16_t lineValues[5];               //von links (0) nach rechts (5)
uint16_t lineReferences[5]; 

uint8_t proxValues[2];                //Frontempfänger mit Werten von linker und rechter LED
int16_t comValue;                     //Beschleunigungswerte auf x-Achse (längs des Zumo)

int maxSpeed = 200;                   //Maximale Geschwindigkeit (möglich 400)
char dir;                             //Fahrtrichtung
char report[120];                     //Ausgabe über Serial

void LineSensorCalibration()
{
  lcd.clear();
  lcd.print("Press A");
  lcd.gotoXY(0, 1);
  lcd.print("to calib");
  buttonA.waitForButton();  
  lcd.clear();
  lcd.print("Calibrate");
  delay(500);
  
  for(uint16_t i = 0; i < 120; i++)
  {
    if (i > 30 && i <= 90) motors.setSpeeds(200, -200);
    else motors.setSpeeds(-200, 200);
    lineSensors.calibrate();
  }
  motors.setSpeeds(0, 0);
}

void setup() 
{
  lineSensors.initFiveSensors();
  lineSensors.read(lineReferences);  
  proxSensors.initFrontSensor();
  Wire.begin();
  compass.init();
  compass.enableDefault();  

  LineSensorCalibration();
  
  lcd.clear();
  lcd.print("Press A");
  lcd.gotoXY(0, 1);
  lcd.print("to start");
  buttonA.waitForButton();

//  Serial1.begin(9600);
//  if(Serial1.available()) Serial1.println("bluetooth available");  
  Serial.begin(9600);
  delay(500);
}

void SerialOutput()
{
  snprintf_P(report, sizeof(report),
    PSTR("Abstand: %3d %3d   Linien: %6d %6d %6d %6d %6d   Richtung: %3c   Beschleunigung: %6d"),
    proxValues[0], proxValues[1], 
    lineValues[0], lineValues[1], lineValues[2], lineValues[3], lineValues[4], 
    dir, comValue);
  Serial.println(report);
//  if(Serial1.available()) Serial1.println(report);
}

void loop() 
{
  ledGreen(0);
  ledYellow(0);
  ledRed(0);
  lcd.clear();

  lineSensors.read(lineValues);
  proxSensors.read();
  proxValues[0] = proxSensors.countsFrontWithLeftLeds();
  proxValues[1] = proxSensors.countsFrontWithRightLeds();  
  compass.read();
  comValue = compass.a.x;
  dir = 'x';

  SerialOutput();
}