Öffentliche Projekte

#include <nibobee/robomain.h>

int power_l;	//Leistung des linken Motoren
int power_r;	//Leistung des rechten Motor

int pwm;	//Differenz für Geschwindigkeits anpassung
int ticksl;	//Odo links
int ticksr;	//Odo rechts
int faktor;	
int basis;

void Motortest()
{	//LED Lichterkette****************************
  	for(int i=0; i<4; i++)
 	{	for (int ledNr=0; ledNr<4; ledNr++)
    	{	led_set(ledNr, 1);
      		delay(150);
     		led_set(ledNr, 0);
    	}
  	}
  //Motortest warnung*****************************
  for (int i=0; i<8; i++)
  {	led_set(2, 1);
    delay(80);
    led_set(2, 0);
    led_set(1, 1);
    delay(80);
    led_set(1, 0);
    }
 	delay(1500);
	odometry_reset ();
  	//Straight Test******************************
  	while((odometry_getLeft(0)< 40) && (odometry_getRight(0)< 40))
  	{
		power_l = 875;
   		power_r = 800;
   		motpwm_setLeft(power_l);
  		motpwm_setRight(power_r);
   		led_set(0, 1);	
   		led_set(3, 1);
  	}
 		led_set(0, 0);	 
   		led_set(3, 0);	
odometry_reset ();
  //Rückwärts************************************
  while((odometry_getLeft(0)> -40) && (odometry_getRight(0)> -40))
  {		power_l = -875;
   		power_r = -800;
   		motpwm_setLeft(power_l);
  		motpwm_setRight(power_r);
   		led_set(0, 1);
   		led_set(3, 1);
      	delay(100);
    	led_set(0, 0);
   		led_set(3, 0);
	}
	//Lefturn*************************************
  //(30 ticks nur auf einer laufenden Seite sind cirka 90 Grad !)
 	odometry_reset ();
  	while(odometry_getRight(0) < 30)
   {
    power_l = 0;
    power_r = 800;
    motpwm_setLeft(power_l);
  	motpwm_setRight(power_r);
    led_set(3, 1);
    
   }
  	delay(250);
	led_set(3, 0);
  //Righturn****************************************
  odometry_reset ();
while(odometry_getLeft(0) < 30)
  { power_r = 0;
    power_l = 875;
    motpwm_setLeft(power_l);
  	motpwm_setRight(power_r);
    led_set(0, 1);
  }
  delay(250);
  led_set(0, 0);

motpwm_stop ();
}
//Funktionen ***************************
void Righturn()
{	odometry_reset ();

while(odometry_getLeft(0) < 30)
  { power_r = 0;
    power_l = 875;
    motpwm_setLeft(power_l);
  	motpwm_setRight(power_r);
    led_set(0, 1);
  }
  delay(250);
  led_set(0, 0);

}

void Lefturn()
{
 	odometry_reset ();
  	while(odometry_getRight(0) < 30)
   {
    power_l = 0;
    power_r = 800;
    motpwm_setLeft(power_l);
  	motpwm_setRight(power_r);
    led_set(3, 1);

}
  delay(200);
  led_set(3, 0);
}
void Straight()
{	odometry_reset ();
  	odometry_init ();
  	motpwm_init ();

power_l = 875;
  	power_r = 800;
  	motpwm_setLeft(power_l);
  	motpwm_setRight(power_r);
  	
}

void Backward()
{	
	odometry_reset ();
  while((odometry_getLeft(0)> -40) && (odometry_getRight(0)> -40))
  {		power_l = -875;
   		power_r = -800;
   		motpwm_setLeft(power_l);
  		motpwm_setRight(power_r);
   		led_set(0, 1);
   		led_set(3, 1);
      	delay(100);
    	led_set(0, 0);
   		led_set(3, 0);
	}
}
void U_turn()
{	
  odometry_reset ();
  while((odometry_getLeft(0)< 30) && (odometry_getRight(0)> -30))
  {		power_l = 700;
   		power_r = -650;
   		motpwm_setLeft(power_l);
  		motpwm_setRight(power_r);
   		led_set(0, 1);
   		led_set(3, 1);
      	delay(100);
    	led_set(0, 0);
   		led_set(3, 0);
	}
}
//Funktionen zu ende************************************************************************

void setup() {
led_init() 		;
motpwm_init()	;
feeler_init()	;
odometry_init ();
Motortest();
}
void loop()
{
	while((feeler_getLeft () == 0 ) && (feeler_getRight () == 0))
	{	Straight();	led_set(1, 1);	led_set(2, 1);		delay(120); 	led_set(1, 0);		led_set(2, 0);	
	}

if ((feeler_getLeft	() == -1) && (feeler_getRight () == -1))
	{	led_set(1, 1);	
		led_set(2, 1);
		Backward();
		U_turn();
    	delay(50);
    	led_set(1, 0);	
		led_set(2, 0);
    }

if ((feeler_getLeft	() == 0) && (feeler_getRight () == -1))
	{	led_set(2, 1);
		led_set(1, 0);
		Backward();
		Lefturn();
    	delay(50);
     	led_set(2, 0);
    }

if ((feeler_getLeft	() == -1) && (feeler_getRight () == 0))
	{	led_set(2, 0);
		led_set(1, 1);
		Backward();
		Righturn();
    	delay(50);
     	led_set(1, 0);
    }

if ((feeler_getLeft	() == +1) && (feeler_getRight () == +1))
	{	led_set(2, 1);
		led_set(1, 1);
		Backward();
		U_turn();	
    	delay(50);
     	led_set(2, 0);
		led_set(1, 0);
    }

if ((feeler_getLeft () == +1) && (feeler_getRight () == 0))
	{	Righturn();		led_set(2, 0);
		led_set(1, 1);	delay(50);	led_set(1, 0); 	}

if ((feeler_getLeft == 0) && (feeler_getRight () == +1))
	{	Lefturn();		led_set(2, 1);
		led_set(1, 0);	delay(50);	led_set(2, 0);		}

}

Öffentliche Projekte

Welcome

Projektverwaltung

Nachrichten

Neuigkeiten

Site-Statistic

hinderniss_fuehler2_1/main.cpp [read only]

Werbung

Online