Gruppe 3
hallo
Diff: Roboter.cpp
- Revision:
- 20:a90e5b54bf7b
- Parent:
- 19:adae367247d4
- Child:
- 21:69ee872b8ee9
--- a/Roboter.cpp Fri May 12 12:25:34 2017 +0000
+++ b/Roboter.cpp Sat May 13 16:50:57 2017 +0000
@@ -27,16 +27,14 @@
int Roboter::readSensors() // Sensoren auslesen
{
- int l = 0;
- float x = 0.12; // Distanz ab welcher sensoren reagieren sollen
+ int lr = 1;
+ float x = 0.13; // Distanz ab welcher sensoren reagieren sollen
if(sensors[1] < x || sensors[5] < x) { // Sensor rechts und links
- l = 6;
- } else {
- l = 1;
+ lr = 6;
}
-
- return l;
+ //printf("%d",lr);
+ return lr;
}
@@ -46,26 +44,26 @@
float x = 0.15; // Distanz ab welcher sensoren reagieren sollen
while(sensors[0] < (x-0.02f) && sensors[1] < x && sensors[5] < x) { // alle sensoren aktiv, roboter fährt nach hinten
- desiredSpeedLeft = -v;
- desiredSpeedRight = v;
+ desiredSpeedLeft = -v*2;
+ desiredSpeedRight = v*2;
}
while(sensors[0] < (x-0.02f) && sensors[5] > x) { // sensor vorne, roboter dreht nach links
- desiredSpeedLeft = -v;
- desiredSpeedRight = -v;
+ desiredSpeedLeft = -v*2;
+ desiredSpeedRight = -v*2;
}
while(sensors[1] < x) { // sensor rechts, roboter dreht nach links
- desiredSpeedLeft = -v;
- desiredSpeedRight = -v;
+ desiredSpeedLeft = -v*2;
+ desiredSpeedRight = -v*2;
}
while(sensors[5] < x && sensors[1] > x) { // sensor links, roboter dreht nach rechts
- desiredSpeedLeft = v;
- desiredSpeedRight = v;
+ desiredSpeedLeft = v*2;
+ desiredSpeedRight = v*2;
}
@@ -73,7 +71,7 @@
int Roboter::ausweichen2() // Hindernissen ausweichen beim Anfahren des Klotzes
{
- float x = 0.12; // Distanz ab welcher sensoren reagieren sollen
+ float x = 0.13; // Distanz ab welcher sensoren reagieren sollen
static int l = 0;
static int r = 0;
static int ii = 0;
@@ -88,20 +86,24 @@
}
if(r == 1) {
- if(ii < 9000) {
+ if(ii < 500) { // linksdrehend rückwärts fahren
desiredSpeedLeft = -(v+30);
- desiredSpeedRight = v-10;
+ desiredSpeedRight = v+30;
}
- if(ii > 8999 && ii < 24000) {
+ if(ii > 499 && ii < 1100) { // links drehen
+ desiredSpeedLeft = -v*2;
+ desiredSpeedRight = -v*2;
+ }
+ if(ii > 1099 && ii < 2900) { // gerade fahren
desiredSpeedLeft = v*2;
desiredSpeedRight = -v*2;
}
- if(ii > 23999 && ii < 33000) {
- desiredSpeedLeft = v*1.8f;
- desiredSpeedRight = v*1.8f;
+ if(ii > 2899 && ii < 3900) { // rechts drehen
+ desiredSpeedLeft = v*2;
+ desiredSpeedRight = v*2;
}
ii++;
- if(ii > 32999) {
+ if(ii > 3899) {
desiredSpeedLeft = 0;
desiredSpeedRight = 0;
r = 0;
@@ -111,20 +113,24 @@
}
if(l == 1) {
- if(ii < 9000) {
- desiredSpeedLeft = -(v-10);
+ if(ii < 500) { // rückwärts fahren
+ desiredSpeedLeft = -(v+30);
desiredSpeedRight = v+30;
}
- if(ii > 8999 && ii < 24000) {
+ if(ii > 499 && ii < 1100) { // rechts drehen
+ desiredSpeedLeft = v*2;
+ desiredSpeedRight = v*2;
+ }
+ if(ii > 1099 && ii < 2900) { // gerade fahren
desiredSpeedLeft = v*2;
desiredSpeedRight = -v*2;
}
- if(ii > 23999 && ii < 33000) {
- desiredSpeedLeft = -v*1.8f;
- desiredSpeedRight = -v*1.8f;
+ if(ii > 2899 && ii < 3900) { // links drehen
+ desiredSpeedLeft = -v*2;
+ desiredSpeedRight = -v*2;
}
ii++;
- if(ii > 32999) {
+ if(ii > 3899) {
desiredSpeedLeft = 0;
desiredSpeedRight = 0;
l = 0;
@@ -138,48 +144,46 @@
int Roboter::pickup() // Klotz aufnehmen
{
- //static int anz = 0;
static int ii = 0;
int r; // Rückegabewert
desiredSpeedLeft = 0;
desiredSpeedRight = 0;
- if(ii < 7000) { // Arm nach unten fahren
+ if(ii < 650) { // Arm nach unten fahren
servo2->SetPosition(2250);
}
- if(ii > 6999 && ii < 12000) { // gerade fahren
+ if(ii > 649 && ii < 1100) { // gerade fahren
desiredSpeedLeft = v;
desiredSpeedRight = -v;
}
- if(ii > 11999 && ii < 17000) { // gerade fahren + Greifer schliessen
- desiredSpeedLeft = v;
- desiredSpeedRight = -v;
+ if(ii > 1099 && ii < 1500) { // gerade fahren + Greifer schliessen
+ if(ii < 1400) {
+ desiredSpeedLeft = v;
+ desiredSpeedRight = -v;
+ }
servo1->SetPosition(500);
}
- if(ii > 16999 && ii < 19000) {
- desiredSpeedLeft = v;
- desiredSpeedRight = -v;
+ if(ii > 1499 && ii < 1900) { // rückwärts fahren
+ desiredSpeedLeft = -v;
+ desiredSpeedRight = v;
}
- if(ii > 18999 && ii < 26000) { // Arm nach oben fahren
+ if(ii > 1899 && ii < 2550) { // Arm nach oben fahren
desiredSpeedLeft = 0;
desiredSpeedRight = 0;
servo2->SetPosition(700);
}
- if(ii > 25999 && ii < 31000) { // Greifer öffnen
+ if(ii > 2549 && ii < 3000) { // Greifer öffnen
servo1->SetPosition(1500);
}
+
ii++;
- //anz++;
r = 5; // Rückegabewert
- if( ii > 30999 ) {
+ if( ii > 2999 ) {
r = 1;
ii = 0;
}
- /*if(anz == 31001) {
- r = 7;
- anz = 0;
- }*/
+
return r;
}
@@ -197,19 +201,19 @@
}
if(cam > 100 && cam <200 ) { // links fahren, wenn wir Werte von 100 bis 199 haben
cam = cam -99.0f; // cam nimmt die Werte von 1 bis 100 an
- speedValue = (v/2) + (cam * (maxWert /100.0f)); // Berechnung des speedValue's
+ speedValue = (v) + (cam * (maxWert /100.0f)); // Berechnung des speedValue's
desiredSpeedLeft = -speedValue;
desiredSpeedRight = -speedValue;
}
if(cam > 200 && cam < 300) { // rechts fahren, wenn wir Werte von 200 bis 299 haben
cam = cam -199.0f; // cam nimmt die Werte von 1 bis 100 an
- speedValue = (v/2) + (cam * (maxWert /100.0f)); // Berechnung des speedValue's
+ speedValue = (v) + (cam * (maxWert /100.0f)); // Berechnung des speedValue's
desiredSpeedLeft = speedValue;
desiredSpeedRight = speedValue;
}
if(cam >= 300 && cam <400) { // gerade fahren, wenn wir Werte von 300 bis 399 haben
cam = cam-299.0f; // cam nimmt die Werte von 1 bis 100 an
- speedValue = (v*2) + (cam * (maxWert /100.0f)); // Berechnung des speedValue's
+ speedValue = (v*3) + (cam * (maxWert /100.0f)); // Berechnung des speedValue's
desiredSpeedLeft = speedValue;
desiredSpeedRight = -speedValue;
}
@@ -219,9 +223,9 @@
{
int r;
static int time2 = 0;
- if(time2 < 100) { // 1s gerade aus fahren
- desiredSpeedLeft = v*5;
- desiredSpeedRight = -v*5;
+ if(time2 < 10) { // 1s gerade aus fahren
+ desiredSpeedLeft = v*4;
+ desiredSpeedRight = -v*4;
time2 ++;
r = 1; // state
} else {
@@ -236,21 +240,27 @@
int Roboter::back()
{
static int ii = 0;
- int s;
+ int s = 7;
- if(ii < 3000) {
+ if(ii < 1000) {
desiredSpeedLeft = -v;
desiredSpeedRight = v;
- s = 7;
}
+
+ if(ii > 999 && ii < 2000) {
+ desiredSpeedLeft = v*2;
+ desiredSpeedRight = v*2;
+ }
+
ii++;
- if(ii > 2999) {
+ if(ii > 1999) {
desiredSpeedLeft = 0;
desiredSpeedRight = 0;
ii = 0;
s = 1;
}
+
return s;
}
@@ -258,7 +268,7 @@
{
int r;
static int time1 = 0;
- if(time1 < 40) { // 1s im Kreis drehen
+ if(time1 < 10) { // 1s im Kreis drehen
desiredSpeedLeft = v*1.7f;
desiredSpeedRight = v*1.7f;
time1 ++;
@@ -272,17 +282,20 @@
return r;
}
-int Roboter::stateAuswahl(float cam, int temp)
+int Roboter::stateAuswahl(float cam, int tempState, int temp)
{
- int s;
+ int s = 1;
if(cam >= 100.0f && cam < 400.0f ) { // Die Kamera erkennt ein grünen Klotz , Werte von 100 und 399
s = 4;
}
- if(cam == 400.0f) { // Roboter in Position
+ if(cam == 400.0f && temp != 5) { // Roboter in Position
s = 5;
}
- if(cam == 0.0f) {
- s = temp;
+ if(cam == 400.0f && temp == 5) {
+ s = 7;
+ }
+ if(cam < 1) {
+ s = tempState;
}
if(cam == 1) {
s = 1;