Öffentliche Projekte

#include <niboburger/base.h>
#include <niboburger/iodefs.h>
#include <niboburger/usart.h>
#include <niboburger/motpwm.h>
#include <niboburger/analog.h>
#include <niboburger/led.h>
#include <niboburger/key.h>
#include <niboburger/delay.h>
#include <niboburger/utils.h>
#include <niboburger/robomain.h>
//#include <setjmp.h>
#include "maroon.h"
//#include <stdint.h>
#include <avr/interrupt.h>

enum {
        EVENT_NONE              =  0,
        EVENT_KEY1              =  1,
        EVENT_KEY2              =  2,
        EVENT_KEY3              =  3,
        EVENT_TIMEOUT           =  4,
    };
	
  //Variablen für den allgemeinen Ablauf
    bool bInit=false;	
    bool bFirstTime=false;
	bool bFirstTimeGeradeAus=false;
  	bool bRun=0;
	bool bMoveDone=0;
	char nCnt1=0;													//reserviert für check_for_update()
	char nCnt2=0;													//reserviert für check_for_update_color()
	char nCnt3=0;
	char nGetriebeuebersetzung=1;									// -1 - grosses Getriebe 125:1 -- 1 - kleines Getriebe 25:1  --> beeinflußt Drehrichtung
  	uint8_t nAktiverSchwellwertwert=0;								//Schwellwert erstmal auf 0
	uint8_t nZustandIrSensoren=0;									//aktuelle IR-Sensor Zustand
	uint8_t nAlterZustandIrSensoren=0;									//Vergleichsvariable um ein festfahren festzustellen
	
  //Info
  //Schaltpunkt ist Oberflächen und Umgebungslicht abhängig
	char nIrSchwellwertFern=15;										//Schwellwert IR Sensor ca. 40 mm
	char nIrSchwellwertNah=35;										//Schwellwert IR Sensor ca. 25 mm
	char nIrSchwellwertDicht=45;									//Schwellwert IR Sensor ca. 15 mm

//Geschwindigkeiten festlegen
  	uint16_t nEingestellteGeschwindigkeit=0;						//Geschwindigkeit erstmal auf 0
	uint16_t nGeschwindigkeitVorwaertsLinks=0;						//Geschwindigkeit erstmal auf 0
  	uint16_t nGeschwindigkeitVorwaertsRechts=0;						//Geschwindigkeit erstmal auf 0
	uint16_t nLowSpeed=126;											//langsame Geschwindigkeit
	uint16_t nMidSpeed=512;											//normale Geschwindigkeit
	uint16_t nMaxSpeed=1024;										//schnelle Geschwindigkeit

//Variablen für Analogsensoren
  //Info
  //ab hier Variablen für Farbwertauswertung
	uint16_t nBL_VorFahrt=0;										//herausgerechnetes Umgebungslicht
  	uint16_t nBC_VorFahrt=0;										//herausgerechnetes Umgebungslicht
  	uint16_t nBR_VorFahrt=0;										//herausgerechnetes Umgebungslicht
	uint16_t nBL_NachFahrt=0;										//herausgerechnetes Umgebungslicht
  	uint16_t nBC_NachFahrt=0;										//herausgerechnetes Umgebungslicht
  	uint16_t nBR_NachFahrt=0;										//herausgerechnetes Umgebungslicht
	int nGrenzwert_Boden=0;
	
	int nFLL;														//herausgerechnetes Umgebungslicht
    int nFL;														//herausgerechnetes Umgebungslicht
    int nFR;														//herausgerechnetes Umgebungslicht
    int nFRR;														//herausgerechnetes Umgebungslicht
	uint16_t nSollPos=0;	
	uint16_t nSollPosLinks=0;
  	uint16_t nSollPosRechts=0;
	uint8_t mTemp0;
	uint8_t mTemp1;
	uint8_t mTemp2=0;

//Funktionen deklarieren
    int gerade_aus_fahren(int nSollPos, int nEingestellteGeschwindigkeit);
    void links_langsamer(int nSollPos);
    void rechts_langsamer(int nSollPos);
    void nach_links_drehen(int nSollPosLinks, int nSollPosRechts);
    void nach_rechts_drehen(int nSollPosLinks, int nSollPosRechts);
    void zurueck(int nSollPos);
    void drehen(int nSollPosLinks, int nSollPosRechts);
    void handle_Event();
    void check_for_update();
    void check_for_update_color();

//ab hier Funktionen definieren
void setup()	{
  	activate_output_group(IO_LEDS);  			// LED bits als Output
  	nibo_checkMonitorVoltage();					//Batterien prüfen, SOS blinken, wenn Spannung zu niedrig
    maroon_setup();
  	maroon_welcome();
  	surface_init();			//ruft bei Bedarf analog_init() und line_readPersistent() auf
   	motpwm_init();
    motpid_init();
    odometry_init();
    odometry_reset();
	
  	//Drehrichtung festlegen je nach Getriebeübersetzung
    nLowSpeed=nLowSpeed*nGetriebeuebersetzung;
    nMidSpeed=nMidSpeed*nGetriebeuebersetzung;
    nMaxSpeed=nMaxSpeed*nGetriebeuebersetzung;
  
	//Zuweisung der Grundgeschindigkeit
    nEingestellteGeschwindigkeit=nMaxSpeed;
  	nGeschwindigkeitVorwaertsLinks=nEingestellteGeschwindigkeit;
  	nGeschwindigkeitVorwaertsRechts=nEingestellteGeschwindigkeit;
  
 	//Festlegung, welcher Schwellwert aktiv ist
	nAktiverSchwellwertwert=nIrSchwellwertNah;
  	nGrenzwert_Boden=3;
  
  	delay(1500);
  
}

/* KEY */

uint8_t key_getEvent() {
  uint8_t c = key_get_char();
  if (c=='a') return EVENT_KEY1;
  if (c=='b') return EVENT_KEY2;
  if (c=='c') return EVENT_KEY3;
  return EVENT_NONE;
}

int get_Event (int nAktiverSchwellwertwert)	{
  
    nFLL = analog_getValueExt(ANALOG_FLL, 2);		//herausgerechnetes Umgebungslicht
    nFL = analog_getValueExt(ANALOG_FL, 2);			//herausgerechnetes Umgebungslicht
    nFR = analog_getValueExt(ANALOG_FR, 2);			//herausgerechnetes Umgebungslicht
    nFRR = analog_getValueExt(ANALOG_FRR, 2);		//herausgerechnetes Umgebungslicht
    nZustandIrSensoren=0;							//aktuelle IR-Sensor Zustand

//If Anweisungen zum feststellen der Zustande von den IR Sensoren -- 16 mögliche Zustände

//			  nFLL	  nFL	  nFR	  nFRR		Aktion
    // -- Z1		0		0		0		0		keine - aktuelle Geschwindigkeit beibehalten und gerade aus fahren
    // -- Z2		0		0		0		1		links Drehzahl anpassen
    // -- Z3		0		0		1		0		stop - nach links drehen 90°
    // -- Z4		0		0		1		1		stop - nach links drehen 90°
    // -- Z5		0		1		0		0		stop - nach rechts drehen 90°
    // -- Z6		0		1		0		1		zurück - nach links 90° drehen
    // -- Z7		0		1		1		0		zurück - Random 90° drehen
    // -- Z8		0		1		1		1		zurück - nach links 90° drehen
    // -- Z9		1		0		0		0		rechts Drehzahl anpassen
  	// -- Z10       1       0       0       1		keine - aktuelle Geschwindigkeit beibehalten und gerade aus fahren
    // -- Z11		1		0		1		0		zurück - Random 90° drehen
    // -- Z12		1		0		1		1		zurück - Random 90° drehen
    // -- Z13		1		1		0		0		stop - nach rechts drehen 90°
    // -- Z14		1		1		0		1		zurück - Random 90° drehen
    // -- Z15		1		1		1		0		stop - nach rechts drehen 90°
    // -- Z16		1		1		1		1		zurück - Random 90° drehen
  
  if ((nAktiverSchwellwertwert>nFLL) && (nAktiverSchwellwertwert>nFL) && (nAktiverSchwellwertwert>nFR) && (nAktiverSchwellwertwert>nFRR)) {				//Zustand 1
    nZustandIrSensoren=1;																																
  }
  else if ((nAktiverSchwellwertwert>nFLL) && (nAktiverSchwellwertwert>nFL) && (nAktiverSchwellwertwert>nFR) && (nAktiverSchwellwertwert<nFRR)) {		//Zustand 2
    nZustandIrSensoren=2;
  }
  else if ((nAktiverSchwellwertwert>nFLL) && (nAktiverSchwellwertwert>nFL) && (nAktiverSchwellwertwert<nFR) && (nAktiverSchwellwertwert>nFRR)) {		//Zustand 3
    nZustandIrSensoren=3;
  }
  else if ((nAktiverSchwellwertwert>nFLL) && (nAktiverSchwellwertwert>nFL) && (nAktiverSchwellwertwert<nFR) && (nAktiverSchwellwertwert<nFRR)) {		//Zustand 4
    nZustandIrSensoren=4;
  }
  else if ((nAktiverSchwellwertwert>nFLL) && (nAktiverSchwellwertwert<nFL) && (nAktiverSchwellwertwert>nFR) && (nAktiverSchwellwertwert>nFRR)) {		//Zustand 5
    nZustandIrSensoren=5;
  }
  else if ((nAktiverSchwellwertwert>nFLL) && (nAktiverSchwellwertwert<nFL) && (nAktiverSchwellwertwert>nFR) && (nAktiverSchwellwertwert<nFRR)) {		//Zustand 6
    nZustandIrSensoren=6;
  }
  else if ((nAktiverSchwellwertwert>nFLL) && (nAktiverSchwellwertwert<nFL) && (nAktiverSchwellwertwert<nFR) && (nAktiverSchwellwertwert>nFRR)) {		//Zustand 7
    nZustandIrSensoren=7;
  }
  else if ((nAktiverSchwellwertwert>nFLL) && (nAktiverSchwellwertwert<nFL) && (nAktiverSchwellwertwert<nFR) && (nAktiverSchwellwertwert<nFRR)) {		//Zustand 8
    nZustandIrSensoren=8;
  }
  else if ((nAktiverSchwellwertwert<nFLL) && (nAktiverSchwellwertwert>nFL) && (nAktiverSchwellwertwert>nFR) && (nAktiverSchwellwertwert>nFRR)) {		//Zustand 9
    nZustandIrSensoren=9;
  }
  else if ((nAktiverSchwellwertwert<nFLL) && (nAktiverSchwellwertwert>nFL) && (nAktiverSchwellwertwert>nFR) && (nAktiverSchwellwertwert<nFRR)) {		//Zustand 10
    nZustandIrSensoren=10;
  }
  else if ((nAktiverSchwellwertwert<nFLL) && (nAktiverSchwellwertwert>nFL) && (nAktiverSchwellwertwert<nFR) && (nAktiverSchwellwertwert>nFRR)) {		//Zustand 11
    nZustandIrSensoren=11;
  }
  else if ((nAktiverSchwellwertwert<nFLL) && (nAktiverSchwellwertwert>nFL) && (nAktiverSchwellwertwert<nFR) && (nAktiverSchwellwertwert<nFRR)) {		//Zustand 12
    nZustandIrSensoren=12;
  }
  else if ((nAktiverSchwellwertwert<nFLL) && (nAktiverSchwellwertwert<nFL) && (nAktiverSchwellwertwert>nFR) && (nAktiverSchwellwertwert>nFRR)) {		//Zustand 13
    nZustandIrSensoren=13;
  }
  else if ((nAktiverSchwellwertwert<nFLL) && (nAktiverSchwellwertwert<nFL) && (nAktiverSchwellwertwert>nFR) && (nAktiverSchwellwertwert<nFRR)) {		//Zustand 14
    nZustandIrSensoren=14;
  }
  else if ((nAktiverSchwellwertwert<nFLL) && (nAktiverSchwellwertwert<nFL) && (nAktiverSchwellwertwert<nFR) && (nAktiverSchwellwertwert>nFRR)) {		//Zustand 15
    nZustandIrSensoren=15;
  }
  else if ((nAktiverSchwellwertwert<nFLL) && (nAktiverSchwellwertwert<nFL) && (nAktiverSchwellwertwert<nFR) && (nAktiverSchwellwertwert<nFRR)) {		//Zustand 16
    nZustandIrSensoren=16;
  }
  
  /*ab hier Sonderzustäne*/
  
  if (nZustandIrSensoren>1)	{
    if (nAlterZustandIrSensoren==nZustandIrSensoren)	{
      nCnt3=nCnt3+1;
      if (nCnt3=5)	{
        nZustandIrSensoren=50;
        nCnt3=0;
    	}      
  	}
    else {nCnt3=0;}
    nAlterZustandIrSensoren=nZustandIrSensoren;
  }
  
  /* nach jedem Fahrbefehl auf Farbänderungen prüfen, um festzustellen, ob sich Roboter sich festgefahren hat. Zustand muss 5 mal hintereinander festgestellt werden */
  
  if(bMoveDone==1){
    if((((nBL_VorFahrt-nBL_NachFahrt)<nGrenzwert_Boden) || ((nBL_NachFahrt-nBL_VorFahrt)<nGrenzwert_Boden))
      &&
      (((nBC_VorFahrt-nBC_NachFahrt)<nGrenzwert_Boden) || ((nBC_NachFahrt-nBC_VorFahrt)<nGrenzwert_Boden))
      &&
      (((nBR_VorFahrt-nBR_NachFahrt)<nGrenzwert_Boden) || ((nBR_NachFahrt-nBR_VorFahrt)<nGrenzwert_Boden))
      &&
      nCnt2<5)	{
          nCnt2=nCnt2+1; 
          if (nCnt2>4)	{
            nZustandIrSensoren=100;
            led_setall(1,1,1,1);
            nCnt2=0;
          }
    }
  }
  return nZustandIrSensoren;
}

int handle_Event (int nZustandIrSensoren)	{  
  
  	//je nach erkanntem Zustand Aktion durchführen
    switch (nZustandIrSensoren) {
      
            case 1:
      			//gerade aus fahren
      			bMoveDone=0;
      			check_for_update_color();
      			nSollPos=50;
      			gerade_aus_fahren(nSollPos, nEingestellteGeschwindigkeit);
      			check_for_update();
      			bMoveDone=1;
      			check_for_update_color();
      			break;
           
            case 2:
           		//links langsamer als rechts um Hindernis auszuweichen -- mit aktueller Geschwindigkeit
      			bMoveDone=0;
      			check_for_update_color();
      			nSollPos=10;
      			links_langsamer(nSollPos);
      			check_for_update();
      			bMoveDone=1;
      			check_for_update_color();
      			break;
           
            case 3:
           		//stop -- nach links drehen
      			bMoveDone=0;
      			check_for_update_color();
      			motpid_stop(0);
           		nSollPosLinks=-90;
      			nSollPosRechts=0;
           		nach_links_drehen(nSollPosLinks, nSollPosRechts);
      			check_for_update();
      			bMoveDone=1;
      			check_for_update_color();
      			break;
           
            case 4:
           		//stop -- nach links drehen
      			bMoveDone=0;
      			check_for_update_color();
      			motpid_stop(0);
           		nSollPosLinks=-90;
      			nSollPosRechts=0;
           		nach_links_drehen(nSollPosLinks, nSollPosRechts);
      			check_for_update();
      			bMoveDone=1;
      			check_for_update_color();
      			break;
           
            case 5:
           		//stop -- nach rechts drehen
      			bMoveDone=0;
      			check_for_update_color();
      			motpid_stop(0);
           		nSollPosLinks=0;
      			nSollPosRechts=-90;
           		nach_rechts_drehen(nSollPosLinks, nSollPosRechts);
      			check_for_update();
      			bMoveDone=1;
      			check_for_update_color();
      			break;
           
           case 6:
           		//stop -- zurück -- nach links drehen
      			bMoveDone=0;
      			check_for_update_color();
      			motpid_stop(0);
      			nSollPos=-90;
      			zurueck(nSollPos);
      			nSollPosLinks=-90;
      			nSollPosRechts=0;
           		nach_links_drehen(nSollPosLinks, nSollPosRechts);
      			check_for_update();
      			bMoveDone=1;
      			check_for_update_color();
      			break;
           
           case 7:
           		//stop -- zurück -- Random drehen -->Todo
      			bMoveDone=0;
      			check_for_update_color();
      			motpid_stop(0);
      			nSollPos=-90;
      			zurueck(nSollPos);
      			nSollPosLinks=0;
      			nSollPosRechts=-90;
           		nach_rechts_drehen(nSollPosLinks, nSollPosRechts);
      			check_for_update();
      			bMoveDone=1;
      			check_for_update_color();
      			break;
           
           case 8:
      			//stop -- nach links drehen
      			bMoveDone=0;
      			check_for_update_color();
      			motpid_stop(0);
           		nSollPosLinks=-90;
      			nSollPosRechts=0;
           		nach_links_drehen(nSollPosLinks, nSollPosRechts);
      			check_for_update();
      			bMoveDone=1;
      			check_for_update_color();
      			break;
           
           case 9:															
                //rechts Zielposition kleiner als links um Hindernis auszuweichen -- mit aktueller Geschwindigkeit
      			bMoveDone=0;
      			check_for_update_color();
      			nSollPos=10;
      			rechts_langsamer(nSollPos);
      			check_for_update();
      			bMoveDone=1;
      			check_for_update_color();
      			break;
           
           case 10:
           		//gerade aus fahren bis ein weiterer Sensor anspricht
      			bMoveDone=0;
      			check_for_update_color();
      			nSollPos=10;
      			gerade_aus_fahren(nSollPos, nEingestellteGeschwindigkeit);
      			check_for_update();
      			bMoveDone=1;
      			check_for_update_color();
      			break;
           
           case 11:
           		//stop -- zurück -- drehen
      			bMoveDone=0;
      			check_for_update_color();
      			motpid_stop(0);
      			nSollPos=-90;
      			zurueck(nSollPos);
      			nSollPosLinks=0;
      			nSollPosRechts=-90;
           		nach_rechts_drehen(nSollPosLinks, nSollPosRechts);
      			check_for_update();
      			bMoveDone=1;
      			check_for_update_color();
      			break;
           
           case 12:
           		//stop -- zurück -- drehen
      			bMoveDone=0;
      			check_for_update_color();
      			motpid_stop(0);
      			nSollPos=-90;
      			zurueck(nSollPos);
      			nSollPosLinks=0;
      			nSollPosRechts=-90;
           		nach_rechts_drehen(nSollPosLinks, nSollPosRechts);
      			check_for_update();
      			bMoveDone=1;
      			check_for_update_color();
      			break;
           
           case 13:
           		//stop -- nach rechts drehen
      			bMoveDone=0;
      			check_for_update_color();
      			motpid_stop(0);
           		nSollPosLinks=0;
      			nSollPosRechts=-90;
           		nach_rechts_drehen(nSollPosLinks, nSollPosRechts);
      			check_for_update();
      			bMoveDone=1;
      			check_for_update_color();
      			break;
           
           case 14:
           		//stop -- zurück -- drehen
      			bMoveDone=0;
      			check_for_update_color();
      			motpid_stop(0);
      			nSollPos=-90;
      			zurueck(nSollPos);
      			nSollPosLinks=0;
      			nSollPosRechts=-90;
           		nach_rechts_drehen(nSollPosLinks, nSollPosRechts);
      			check_for_update();
      			bMoveDone=1;
      			check_for_update_color();
      			break;
           
           case 15:
           		//stop -- nach rechts drehen
      			bMoveDone=0;
      			check_for_update_color();
      			motpid_stop(0);
      			nSollPosLinks=0;
      			nSollPosRechts=-90;
           		nach_rechts_drehen(nSollPosLinks, nSollPosRechts);
      			check_for_update();
      			bMoveDone=1;
      			check_for_update_color();
      			break;
           
           case 16:
           		//stop -- zurück -- drehen
      			bMoveDone=0;
      			check_for_update_color();
      			motpid_stop(0);
      			nSollPos=-90;
      			zurueck(nSollPos);
      			nSollPosLinks=0;
      			nSollPosRechts=-90;
           		nach_rechts_drehen(nSollPosLinks, nSollPosRechts);
      			check_for_update();
      			bMoveDone=1;
      			check_for_update_color();
      			break;
      
      		case 100:
      			//Farbsensoren erkennen keinen Wechsel nach Fahrbefehl, zurück und neu orientieren
      			
      			bMoveDone=0;
      			check_for_update_color();
      			motpid_stop(0);
      			nSollPos=-90;
      			zurueck(nSollPos);
      			led_setall(0,0,0,0);
      			nFLL = analog_getValueExt(ANALOG_FLL, 2);		//herausgerechnetes Umgebungslicht
      			nFRR = analog_getValueExt(ANALOG_FRR, 2);		//herausgerechnetes Umgebungslicht
      			if (nFLL<nFRR)	{								//links ist mehr Luft als rechts, bedeutet nach links 180° drehen
                  nSollPosLinks=135;
                  nSollPosRechts=-135;
                  drehen(nSollPosLinks, nSollPosRechts);
                  check_for_update();
                }
        		else	{
                  nSollPosLinks=-135;
                  nSollPosRechts=135;
                  drehen(nSollPosLinks, nSollPosRechts);
                  check_for_update();
                }
      			bMoveDone=1;
      			check_for_update_color();
      			break;
          }
}

void check_for_update()	{																																//auf Änderung der Analogwerte prüfen
  mTemp1=analog_getValueExt(ANALOG_FLL, 2)+analog_getValueExt(ANALOG_FL, 2)+analog_getValueExt(ANALOG_FR, 2)+analog_getValueExt(ANALOG_FRR, 2);			//alle Analogwerte addieren
  if (mTemp0!=mTemp1)	{																																//wenn alt zu neu nicht übereinstimmt
    if (nCnt1) {																																		//Zähler dekrementiren und Funktion verlassen
      nCnt1--;
      return;
    }
    nCnt1=10;																																			//Zähler auf 20 (somit max. alle 20 Zyklen, sonst flackert die Anzeige)
    maroon_loop();																																		//Anzeige beschreiben
    mTemp0=analog_getValueExt(ANALOG_FLL, 2)+analog_getValueExt(ANALOG_FL, 2)+analog_getValueExt(ANALOG_FR, 2)+analog_getValueExt(ANALOG_FRR, 2);		//neue Analogwertsumme bilden
  }
}

void check_for_update_color()	{
  
  if(bMoveDone==0)	{
  	nBL_VorFahrt=surface_getAbs(SURFACE_L);
	nBC_VorFahrt=surface_getAbs(SURFACE_C);
	nBR_VorFahrt=surface_getAbs(SURFACE_R);
    return;
  }
  if(bMoveDone==1)	{
	nBL_NachFahrt=surface_getAbs(SURFACE_L);
	nBC_NachFahrt=surface_getAbs(SURFACE_C);
	nBR_NachFahrt=surface_getAbs(SURFACE_R);
    return;
  }  
}
  
int gerade_aus_fahren(int nSollPos, int nEingestellteGeschwindigkeit)	{																//Aufruf der Funktion und Übergabe der Variablen
  if (bFirstTimeGeradeAus==false)	{  																											//erster GeradeAusLauf mit vorgegebenen Werten
    odometry_reset();
    while ((odometry_getLeft(0)<nSollPos) || (odometry_getRight(0)<nSollPos)) {
      motpwm_setLeft(nEingestellteGeschwindigkeit);	
      motpwm_setRight(nEingestellteGeschwindigkeit);
      
    }
   bFirstTimeGeradeAus=true;
   } 
  else {																										//alle weiteren GeradeAusLauf mit berechneten Werten
    odometry_reset();
    while ((odometry_getLeft(0)<nSollPos) || (odometry_getRight(0)<nSollPos)) {
      motpwm_setLeft(nGeschwindigkeitVorwaertsLinks);	
      motpwm_setRight(nGeschwindigkeitVorwaertsRechts);
      
    }
}
  
//Auswertung der Odometrie im Vergleich zum Fahrauftrag -- beim gerade aus fahren
  
  if (odometry_getLeft(0)>odometry_getRight(0))	{ 													//links zu schnell
    if (nEingestellteGeschwindigkeit>=(nGeschwindigkeitVorwaertsRechts+(odometry_getLeft(0)-odometry_getRight(0))))	{
  		nGeschwindigkeitVorwaertsRechts=(nGeschwindigkeitVorwaertsRechts+(odometry_getLeft(0)-odometry_getRight(0)));
    }
    else {nGeschwindigkeitVorwaertsLinks=(nGeschwindigkeitVorwaertsLinks-(odometry_getLeft(0)-odometry_getRight(0)));}
  } 
  else if (odometry_getRight(0)>odometry_getLeft(0))	{ 													//links zu schnell
  	if (nEingestellteGeschwindigkeit>=(nGeschwindigkeitVorwaertsLinks+(odometry_getRight(0)-odometry_getLeft(0))))	{
  		nGeschwindigkeitVorwaertsLinks=(nGeschwindigkeitVorwaertsLinks+(odometry_getRight(0)-odometry_getLeft(0)));
    }
    else {nGeschwindigkeitVorwaertsRechts=(nGeschwindigkeitVorwaertsRechts-(odometry_getRight(0)-odometry_getLeft(0)));}
  }
  return nGeschwindigkeitVorwaertsLinks, nGeschwindigkeitVorwaertsRechts;
}

void links_langsamer(int nSollPos)	{
  odometry_reset();
  while (odometry_getRight(0)<nSollPos)	{
    motpwm_setLeft(nGeschwindigkeitVorwaertsLinks/2);	
    motpwm_setRight(nGeschwindigkeitVorwaertsRechts);
    
  }
}

void rechts_langsamer(int nSollPos)	{
  odometry_reset();
  while (odometry_getLeft(0)<nSollPos)	{
    motpwm_setLeft(nGeschwindigkeitVorwaertsLinks);	
    motpwm_setRight(nGeschwindigkeitVorwaertsRechts/2);
    
  }
}

void nach_links_drehen(int nSollPosLinks, int nSollPosRechts)	{
  odometry_reset();
   while (odometry_getLeft(0)>nSollPosLinks)	{									//linkes Rad rückwärts drehen
     motpwm_setLeft(nGeschwindigkeitVorwaertsLinks*(-1));
     
   }  
}
void nach_rechts_drehen(int nSollPosLinks, int nSollPosRechts)	{
  odometry_reset();
  while (odometry_getRight(0)>nSollPosRechts)	{									//rechtes Rad rückwärts drehen
    motpwm_setRight(nGeschwindigkeitVorwaertsRechts*(-1));
    
  }
}

void zurueck(int nSollPos)	{
  odometry_reset();
    while ((odometry_getLeft(0)>nSollPos) || (odometry_getRight(0)>nSollPos))	{	//rückwärts fahren
      motpwm_setLeft(nGeschwindigkeitVorwaertsLinks*(-1));
      motpwm_setRight(nGeschwindigkeitVorwaertsRechts*(-1));
      
    }
}

void drehen(int nSollPosLinks, int nSollPosRechts)	{
  odometry_reset();
  if (nSollPosLinks<nSollPosRechts)	{
      while (odometry_getLeft(0)>nSollPosLinks)	{									//linkes Rad rückwärts drehen
          motpwm_setLeft(nGeschwindigkeitVorwaertsLinks*(-1));
          motpwm_setRight(nGeschwindigkeitVorwaertsRechts);
     }  
  }
  else {
    while (odometry_getRight(0)>nSollPosRechts)	{									//rechtes Rad rückwärts drehen
      	motpwm_setLeft(nGeschwindigkeitVorwaertsLinks);
    	motpwm_setRight(nGeschwindigkeitVorwaertsRechts*(-1));
  	}
  }
}

void loop() {
  if (bInit==false)	{
    activate_output_group(IO_LEDS);  			// LED bits als Output
  	//nibo_checkMonitorVoltage();				//Batterien prüfen, SOS blinken, wenn Spannung zu niedrig
    maroon_setup();
  	maroon_welcome();
    analog_init();
    motpwm_init();
    motpid_init();
    odometry_init();
    odometry_reset();
    delay(500);									//warten
    bInit=true;
  }
  //T-FlipFlop, um mit einer Taste Ein- und Auszuscshalten
  char key = key_get_char();
  switch (key) {
    case 'A':
      mTemp2=1;
      break;
    case 'a':
    if (mTemp2==1)	{
      if(bRun==0)	{bRun=1,mTemp2=0;}
      else {bRun=0,mTemp2=0;}
      break;
  	}
  }
  if (bRun==true) {
      //analog_wait_update();
      maroon_loop();
      get_Event(nAktiverSchwellwertwert);
      int nZustandIrSensoren=get_Event(nAktiverSchwellwertwert);
      handle_Event(nZustandIrSensoren);
  }
  if (bRun==false) {
    motpid_stop(0);
    analog_wait_update();
    maroon_loop();
    get_Event(nAktiverSchwellwertwert);
  }
}

Öffentliche Projekte

Welcome

Projektverwaltung

Nachrichten

Neuigkeiten

Site-Statistic

hinderniserkennung/main.cpp [read only]

Werbung

Online