Öffentliche Projekte

#include <nibobee/robomain.h>

// in diesem Bereich können globale Variablen definiert werden
// ...

int wait10Seconds(); //Deklarieren der Funktion
int slowEngineStartAllWheels(int forward); //Fahrt starten Vorwärts oder Rückwärts

/*
* Rückwärts fahren 45 Grad lenken
* @left: nach links lenken = 1, nach rechts lenken = 0
*/
void moveBack45Degree(int left);
/*
* zurückfahren
*/
void moveBack();
/*
* stoppen
*/
void stopp();

int getDelayFromSpeed(int speed);

int maxSpeed = 1024; //maximale Geschwindigkeit

void setup() {
  //Initiation
  led_init();
  motpwm_init();
  feeler_init();
  
  //10 Sekunden warten bis es losgeht. So haben wir Zeit den Roboter abzusetzen.
  wait10Seconds();
  //Geradeaus fahren
  slowEngineStartAllWheels(1);
}

void loop() {
  //Wenn es hier losgeht, dann fährt das Fahrzeug schon vorwärts.
  //Wenn also das Fahrzeug bereits vor einem Hinderniss steht, dann kann es kurz rucken.
  
  //Status der Fühler abfragen
  int status_l, status_r;
  status_l = feeler_getLeft();
  status_r = feeler_getRight();

if (status_l == -1 && status_r == -1){
    //Wir sind geradeaus in was reingefahren
    stopp();
    led_set(1, 1);
    led_set(2, 1);
    moveBack45Degree(1); //nach links ausweichen
    led_set(1, 0);
    led_set(2, 0);
  }
  else if(status_l == 0 && status_r == -1){
    //Wir sind rechts geradeaus in etwas hinein gefahren
    stopp();
    led_set(2, 1);
    moveBack45Degree(1); //nach links ausweichen
    led_set(2, 0);
  }
  else if(status_l == -1 && status_r == 0){
    //Wir sind links geradeaus in etwas hinein gefahren
    stopp();
    led_set(1, 1);
    moveBack45Degree(0); //nach rechts ausweichen
    led_set(1, 0);
  }
    //Das hier stört mich, weil wir machmal auch nur beim drehen hängen
  else if(status_l == 1 && status_r == 1){
    //Wir sind beim rückwärts fahren mit beiden Fühlern hängen geblieben
    stopp();
    led_set(0, 1);
    led_set(3, 1);
    moveBack(); //nach hinten fahren
    moveBack45Degree(0); //nach rechts ausweichen
    led_set(0, 0);
    led_set(3, 0);
  }
  else if(status_l == 0 && status_r == 1){
    //Wir sind beim rückwärts fahren mit dem rechten Fühlern hängen geblieben
    stopp();
    led_set(3, 1);
    moveBack45Degree(1); //nach links ausweichen
    led_set(3, 0);
  }
  else if(status_l == 1 && status_r == 0){
    //Wir sind beim rückwärts fahren mit dem linken Fühlern hängen geblieben
    stopp();
    led_set(0, 1);
    moveBack45Degree(0); //nach rechts ausweichen
    led_set(0, 0);
  }
  
  
  //Motoren wieder mit der eingestellten Geschwindigkeit fahren lassen
  motpwm_setLeft(maxSpeed);
  motpwm_setRight(maxSpeed);  
}

/*
* Wartet 10 Sekunden. Die LED werden dabei als Anzeige verwendet.
*/
int wait10Seconds() {
  
  //Alle LED abschalten
  for(int i = 0; i < 4; i++) {
    led_set(i, 0);
  }
  
  //Wartezeit 2,5 Sekunden
  int delayTime = 2500;
  
  //For Schleife, den Wert der cLED (CounterLED) nutzen wir um die LED zu schalten
  for(int cLED = 0; cLED < 4; cLED++) {
    
    //LED schalten
    led_set(cLED, 1);
    //wir warten die Wartezeit
    delay(delayTime);
   
  } //end for
  
  //Alle LEDs abschalten dauert nochmal 1 Sek.
  for(int i = 3; i >= 0; i--) {
   	led_set(i, 0);
   	delay(250);
  }

return 0;
}

/*
* Wir wollen nur langsam losfahren
* Wenn forward = 1 ist wird Vorwärts gefahren, 0 = Rückwärts
*/
int slowEngineStartAllWheels(int forward){
  switch (forward){
    case 1:
		//langsam Vorwärts
    	for(int i = 1; i <= maxSpeed; i*=2){
        	motpwm_setLeft(i);
  			motpwm_setRight(i);
      		delay(50);
    	}
    	break;
    case 0:
    	//langsam Rückwärts
    	for(int i = 1; i <= maxSpeed; i*=2){
	        motpwm_setLeft(-i);
  			motpwm_setRight(-i);
      		delay(50);
    	}
    	break;
  }
  return 0;
}

/*
* Gibt die Delay passend zur Geschwindigkeit zurück.
* Wenn die Geschwindigkeit größer ist, dann dreht der Roboter z.B. schneller.
*/
int getDelayFromSpeed(int speed){
  //und welche Grundwerte haben wir hier?
  if(speed <= 256){
    return 500;
  }
  if(speed <= 512){
    return 375;
  }
  if(speed <= 768){
    return 250;
  }
  if(speed <= 1024){
    return 125;
  }
  else{
    return 0;
  }
}

/*
* Setzt den Roboter zurück und versetzt ihn um 45 Grad in die gewünschte Richtung
* @left: 1 = links, 0 = rechts
*/
void moveBack45Degree(int left){
  switch(left){
    case 1:
    	//erstmal ein klein wenig zurücksetzen
        motpwm_setLeft(-1024);
  		motpwm_setRight(-1024);
    	delay(getDelayFromSpeed(1024));
    	//nach links rückwärts
    	for(int i = 1; i <= maxSpeed; i*=2){
          //dreht auf der Stelle
          motpwm_setLeft(-i);
          motpwm_setRight(i);
          delay(25);
    	}
    	delay(getDelayFromSpeed(maxSpeed));
    	//anhalten
      	motpwm_setLeft(0);
		motpwm_setRight(0);
    	break;
    case 0:
    	//erstmal ein klein wenig zurücksetzen
        motpwm_setLeft(-1024);
  		motpwm_setRight(-1024);
    	delay(getDelayFromSpeed(1024));
    	//nach rechts rückwärts
    	for(int i = 1; i <= maxSpeed; i*=2){
          //dreht auf der Stelle
          motpwm_setLeft(i);
          motpwm_setRight(-i);
          delay(25);
    	}
    	delay(getDelayFromSpeed(maxSpeed));
      	//anhalten
      	motpwm_setLeft(0);
		motpwm_setRight(0);
    	break;
  }
}

void moveBack(){
  //erstmal ein klein wenig zurücksetzen
  motpwm_setLeft(-1024);
  motpwm_setRight(-1024);
  delay(getDelayFromSpeed(128));
}

void stopp(){
  //alle Motoren Stoppen
  motpwm_setLeft(0);
  motpwm_setRight(0);
}

Öffentliche Projekte

Welcome

Projektverwaltung

Nachrichten

Neuigkeiten

Site-Statistic

simplefeelerusing1/main.cpp [read only]

Werbung

Online