|
@@ -1,5 +1,5 @@
|
|
|
//Verfassser: Felix Stange 4056379 MET2016
|
|
|
-//Datum: 19.07.2017 erstellt, 11.10.2017 zuletzt geändert
|
|
|
+//Datum: 19.07.2017 erstellt, 18.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
|
|
@@ -21,15 +21,15 @@ Zumo32U4Encoders encoders;
|
|
|
LSM303 compass; //Beschleunigungssensor x-Achse
|
|
|
L3G gyro; //Drehbewegungssensor z-Achse
|
|
|
|
|
|
-uint16_t lineValue[3]; //von links (0) nach rechts (2)
|
|
|
+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)
|
|
|
+uint8_t proxValue[4]; //von links (0) nach rechts (3)
|
|
|
|
|
|
-int16_t countsLeft; //Encoder rechts
|
|
|
+int16_t countsLeft; //Encoder links
|
|
|
int16_t wayLeft;
|
|
|
-int16_t countsRight; //Encoder links
|
|
|
+int16_t countsRight; //Encoder rechts
|
|
|
int16_t wayRight;
|
|
|
|
|
|
int32_t rotationAngle = 0; //Drehwinkel
|
|
@@ -146,6 +146,7 @@ void setup()
|
|
|
|
|
|
//Zumo bereit zu starten
|
|
|
Serial1.println("Zumo ready to start, press A");
|
|
|
+ ledGreen(1);
|
|
|
buttonA.waitForButton();
|
|
|
delay(500);
|
|
|
}
|
|
@@ -198,11 +199,10 @@ void CompassUpdate()
|
|
|
}
|
|
|
|
|
|
//Update Encoder
|
|
|
-
|
|
|
void EncoderUpdate()
|
|
|
{
|
|
|
countsLeft = encoders.getCountsLeft();
|
|
|
- wayLeft = countsRight / 910; //12cpr (Motorwelle) * 75,81:1 (Getriebe) = 909,7cpr (Antriebszahnrad)
|
|
|
+ wayLeft = countsRight / 910; //12cpr (Motorwelle) * 75,81:1 (Getriebe) = 909,7cpr (Antriebszahnrad)
|
|
|
countsRight = encoders.getCountsRight();
|
|
|
wayRight = countsRight / 910;
|
|
|
}
|
|
@@ -229,27 +229,27 @@ void ObstacleAvoidance()
|
|
|
Serial1.println("obstacle avoidance");
|
|
|
ledYellow(1);
|
|
|
|
|
|
- //Schritt 1: links bis über Mittellinie fahren
|
|
|
- motors.setSpeeds(maxSpeed/2, maxSpeed);
|
|
|
+ //Schritt 1: links bis über Mittellinie fahren
|
|
|
rotationAngle = 0;
|
|
|
GyroUpdate();
|
|
|
while(lineValue[2] > 1000)
|
|
|
{
|
|
|
LineUpdate();
|
|
|
+ motors.setSpeeds(maxSpeed/2, maxSpeed);
|
|
|
}
|
|
|
|
|
|
- //Schritt 2: rechts fahren bis Fahrzeug gerade steht ohne dabei weitere Linien zu überfahren
|
|
|
- motors.setSpeeds(maxSpeed, maxSpeed/2);
|
|
|
+ //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] || proxValue[2]) < 4)
|
|
|
+ if(proxValue[1] < 4 || proxValue[2] < 4)
|
|
|
{
|
|
|
//Schritt 3: geradeaus fahren bis Hindernis passiert
|
|
|
maxSpeed = 400;
|
|
@@ -259,6 +259,7 @@ void ObstacleAvoidance()
|
|
|
proxValue[3] = proxSensors.countsRightWithRightLeds();
|
|
|
while(proxValue[3] > 4)
|
|
|
{
|
|
|
+ motors.setSpeeds(maxSpeed, maxSpeed);
|
|
|
proxSensors.read();
|
|
|
proxValue[3] = proxSensors.countsRightWithRightLeds();
|
|
|
TrackKeeper();
|
|
@@ -267,11 +268,11 @@ void ObstacleAvoidance()
|
|
|
}
|
|
|
|
|
|
//Schritt 4: rechts bis über Mittellinie fahren
|
|
|
- motors.setSpeeds(maxSpeed/2, maxSpeed);
|
|
|
+ motors.setSpeeds(maxSpeed, maxSpeed/2);
|
|
|
ledYellow(1);
|
|
|
rotationAngle = 0;
|
|
|
GyroUpdate();
|
|
|
- while(lineValue[0] - 200)
|
|
|
+ while(lineValue[0] < 1000)
|
|
|
{
|
|
|
LineUpdate();
|
|
|
}
|
|
@@ -281,6 +282,7 @@ void ObstacleAvoidance()
|
|
|
while(rotationAngle != 0)
|
|
|
{
|
|
|
GyroUpdate();
|
|
|
+ TrackKeeper();
|
|
|
}
|
|
|
}
|
|
|
|
|
@@ -291,7 +293,7 @@ void TrackKeeper()
|
|
|
//linke Linie erreicht, nach rechts fahren
|
|
|
if(lineValue[0] < 1000)
|
|
|
{
|
|
|
- motors.setSpeeds(maxSpeed, 0);
|
|
|
+ motors.setSpeeds(maxSpeed, maxSpeed/2);
|
|
|
ledYellow(1);
|
|
|
delay(200);
|
|
|
dir = 'R';
|
|
@@ -300,7 +302,7 @@ void TrackKeeper()
|
|
|
//rechte Linie erreicht, nach links fahren
|
|
|
else if(lineValue[2] < 1000)
|
|
|
{
|
|
|
- motors.setSpeeds(0, maxSpeed);
|
|
|
+ motors.setSpeeds(maxSpeed/2, maxSpeed);
|
|
|
ledYellow(1);
|
|
|
delay(200);
|
|
|
dir = 'L';
|
|
@@ -310,7 +312,9 @@ void TrackKeeper()
|
|
|
else if(lineValue[1] < 1000)
|
|
|
{
|
|
|
while(lineValue[0] > 1000)
|
|
|
- motors.setSpeeds(-maxSpeed/2, -maxSpeed/2);
|
|
|
+ {
|
|
|
+ motors.setSpeeds(-maxSpeed/2, -maxSpeed/2);
|
|
|
+ }
|
|
|
ledRed(1);
|
|
|
delay(1000);
|
|
|
dir = 'B';
|
|
@@ -342,7 +346,9 @@ void Crossroad()
|
|
|
if(randy == 1)
|
|
|
{
|
|
|
//zur Kreuzungsmitte fahren
|
|
|
- while((wayLeft != 1) && (wayRight != 1))
|
|
|
+ wayLeft = 0;
|
|
|
+ wayRight = 0;
|
|
|
+ while(wayLeft != 1 && wayRight != 1)
|
|
|
{
|
|
|
EncoderUpdate();
|
|
|
motors.setSpeeds(maxSpeed/2, maxSpeed/2);
|
|
@@ -353,8 +359,8 @@ void Crossroad()
|
|
|
GyroUpdate();
|
|
|
while(rotationAngle != 90)
|
|
|
{
|
|
|
- motors.setSpeeds(0, maxSpeed);
|
|
|
- GyroUpdate();
|
|
|
+ GyroUpdate();
|
|
|
+ motors.setSpeeds(0, maxSpeed);
|
|
|
}
|
|
|
|
|
|
//geradeaus fahren
|
|
@@ -369,13 +375,15 @@ void Crossroad()
|
|
|
GyroUpdate();
|
|
|
while(rotationAngle != 90)
|
|
|
{
|
|
|
- motors.setSpeeds(maxSpeed, 0);
|
|
|
- GyroUpdate();
|
|
|
+ GyroUpdate();
|
|
|
+ motors.setSpeeds(maxSpeed, 0);
|
|
|
}
|
|
|
|
|
|
//geradeaus fahren
|
|
|
motors.setSpeeds(maxSpeed/2, maxSpeed/2);
|
|
|
}
|
|
|
+
|
|
|
+ else motors.setSpeeds(maxSpeed, maxSpeed);
|
|
|
}
|
|
|
|
|
|
//Funktion Serielle Ausgabe
|
|
@@ -405,9 +413,9 @@ void loop()
|
|
|
EncoderUpdate();
|
|
|
|
|
|
//Funktionsauswahl
|
|
|
-/* if((compass.a.x - compassOffset) > 1000) CollisionDetection();
|
|
|
- else if((proxValue[0] || proxValue[1]) > 4) ObstacleAvoidance();
|
|
|
- else if((lineValue[0] < 1000)) && (lineValue[1] < 1000)) && (lineValue[2] < 1000))) Crossroad();
|
|
|
+/* if(moveRate > 1000) CollisionDetection();
|
|
|
+ else if(proxValue[0] > 4 || proxValue[1] > 4) ObstacleAvoidance();
|
|
|
+ else if(lineValue[0] < 1000 && lineValue[1] < 1000 && lineValue[2] < 1000) Crossroad();
|
|
|
else TrackKeeper(); */
|
|
|
|
|
|
//Ausgabe über Bluetoothverbindung
|