Zumo32U4/Zumo32U4/Hauptprojekt/Hauptprojekt.ino

//Verfassser: Felix Stange MET2016
//Datum: 09.04.2017
//Projekt: Der Zumo fährt automatisch. Dabei vermeidet er Kollisionen mit der Umgebung durch Nutzung der Abstandssensoren 
//(links, vorne, rechts). Kommt es dennoch zu einer Kollision, wird diese durch die Beschleunigungssensoren erkannt. 

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

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

uint8_t proxLeft;
uint8_t proxFrontLeft;
uint8_t proxFrontRight;
uint8_t proxFront;
uint8_t proxRight;
uint8_t maxspeed = 200;
char dir;

int8_t acc;

char report[120];

void setup()
{
	lcd.clear();
	lcd.print("Press A");
	lcd.gotoXY(0, 1);
	lcd.print("to start");
	buttonA.waitForButton();
	delay(500);
	proxSensors.initThreeSensors();

	Wire.begin();
	compass.init();
	compass.enableDefault();
}

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

	proxSensors.read();
	proxLeft = proxSensors.countsLeftWithLeftLeds();
	proxFrontLeft = proxSensors.countsFrontWithLeftLeds();
	proxFrontRight = proxSensors.countsFrontWithRightLeds();
	proxRight = proxSensors.countsRightWithRightLeds();

	compass.read();
	acc = compass.a.x;

	if ((proxFrontLeft > 4) && (proxFrontLeft > proxFrontRight))
	{
		motors.setLeftSpeed(maxspeed);
		motors.setRightSpeed(0);
		ledRed(1);
		dir = 'R';
		delay(100);
	}
	else if ((proxFrontRight > 4) && (proxFrontLeft < proxFrontRight))
	{
		motors.setLeftSpeed(0);
		motors.setRightSpeed(maxspeed);
		ledRed(1);
		dir = 'L';
		delay(100);
	}
	else if (((proxFrontRight > 5) && (proxFrontLeft > 5)) || (abs(acc) > 1000))
	{
		motors.setLeftSpeed(-maxspeed / 2);
		motors.setRightSpeed(-maxspeed / 2);
		ledRed(1);
		dir = 'B';
		delay(200);
		motors.setLeftSpeed(0);
		motors.setRightSpeed(-maxspeed / 2);
		delay(200);
	}
	else if ((proxLeft > 4) && (proxLeft > proxRight))
	{
		motors.setLeftSpeed(maxspeed);
		motors.setRightSpeed(maxspeed / 2);
		ledYellow(1);
		dir = 'r';
		delay(100);
	}
	else if (proxRight > 4)
	{
		motors.setLeftSpeed(maxspeed / 2);
		motors.setRightSpeed(maxspeed);
		ledYellow(1);
		dir = 'l';
		delay(100);
	}
	else
	{
		motors.setLeftSpeed(maxspeed);
		motors.setRightSpeed(maxspeed);
		ledGreen(1);
		dir = 'F';
		delay(100);
	}

	proxFront = (proxFrontLeft + proxFrontRight) / 2;

	lcd.clear();
	lcd.print("L");
	lcd.print(proxLeft);
	lcd.print(" F");
	lcd.print(proxFront);
	lcd.print(" R");
	lcd.print(proxRight);
	lcd.gotoXY(0, 1);
	lcd.print("Dir ");
	lcd.print(dir);

	snprintf_P(report, sizeof(report),
		PSTR("Abstand: %6d %6d %6d %6d   Richtung: %6c   Beschleunigung: %6d"),
		proxLeft, proxFrontLeft, proxFrontRight, proxRight,
		dir, acc);
	Serial.println(report);
}