-         

Seite 1 von 2 12 LetzteLetzte
Ergebnis 1 bis 10 von 18

Thema: lienefolger: Xbus benutzen

  1. #1
    Neuer Benutzer Öfters hier
    Registriert seit
    30.11.2008
    Beiträge
    13

    lienefolger: Xbus benutzen

    Anzeige

    Hallo,

    Ich habe eine lienefolger gebaut für meine RP6 robot. Ich habe dabei unterstehend schema benutzt von lynxx motion:

    http://www.active-robots.com/product...ion/tra-v5.pdf

    Die lienefolger habe ich auf dem Experimentierplatine gebaut (nicht auf dem RP6 CONTROL M32). Es gibt drei sensoren, jeden sensor hat seine eigenen ausgang. Ich dachte das es möglich ware jeden sensor eine eigenen anschluss zu geben, der INT1, INT2 und INT3.

    Aber jetzt weiss ich es nicht, wie sol ich meine sensor communicieren lassen mit dem microcontroler? Und wie sol ich die sensorausgang auslesen?

    Hoffentlich kunnen sie mir helfen.

    Freundliche gruss,
    Fieke

    Eintschuldigung für meine schlechtes Deutsch, ich bin eine Hollander.

  2. #2
    Erfahrener Benutzer Robotik Einstein Avatar von Dirk
    Registriert seit
    30.04.2004
    Ort
    NRW
    Beiträge
    3.791
    @Fieke:
    Wenn du den Sensor genau so nachgebaut hast, braucht der 3 Digitalports. Die sind an der RP6Base aber nicht INT1, 2, 3, sondern du hast am XBUS die 3 Ports INT1, SCL und SDA frei, wenn du den I2C-Bus nicht brauchst.
    Wie du die 3 Ports ansprechen kannst, zeigt dieser Post:
    http://www.roboternetz.de/phpBB2/viewtopic.php?t=38575
    Da werden 3 LEDs angesteuert.
    Du must nur das Programm so ändern, dass die 3 Ports als Eingang geschaltet werden und im Programm abfragen.

    Gruß Dirk

  3. #3
    Neuer Benutzer Öfters hier
    Registriert seit
    30.11.2008
    Beiträge
    13
    Danke für ihren Hilfe!

  4. #4
    Neuer Benutzer Öfters hier
    Registriert seit
    30.11.2008
    Beiträge
    13
    Jetzt will ich die Ausgänge auslesen. Kann ich nach understehend vorbild einfach mit WriteInteger(SCL, DEC); die Werten von SCL, SDA und die INT1 auslesen?

    DDRA |= (E_INT1); // PA4 (IT1) als Ausgang definieren
    DDRC |= (SCL | SDA); // PC0, PC1 als Ausgänge definieren

    Gruss Fieke

  5. #5
    Erfahrener Benutzer Robotik Einstein Avatar von Dirk
    Registriert seit
    30.04.2004
    Ort
    NRW
    Beiträge
    3.791
    @Fieke:
    Ausgänge kann man nicht lesen, sondern nur Eingänge.

    Mit ...
    Code:
    // Eingänge definieren:
    	DDRA &= ~E_INT1;				// PA4 (IT1) als Eingang definieren
    //	PORTA |= E_INT1;				// evtl. Pullup für PA4
    	DDRC &= ~(SCL | SDA);			// PC0, PC1 als Eingänge definieren
    //	PORTC |= (SCL | SDA);			// evtl. Pullups für PC0, PC1
    ... definiert man SCL, SDA und INT1 als Eingänge.

    Abfragen kann man sie im Programm mit ...
    Code:
    // Abfrage der Pins:
    	temp1 = PINA & E_INT1;
    	temp2 = PINC & SCL;
    	temp3 = PINC & SDA;
    Den Wert von tempX kann man nicht mit WriteInteger o.ä. ausgeben, weil das nur ein Ja/Nein-Wert ist. tempX ist also entweder 0 oder <> 0. Damit erkennt der Roboter dann die Linie, der er folgen soll.
    Die Abfrage im Programm erfolgt dann mit ...

    if (tempX) {tuwas};
    oder ...
    if (!tempX) {tuwasanderes};

    Gruß Dirk

  6. #6
    Neuer Benutzer Öfters hier
    Registriert seit
    30.11.2008
    Beiträge
    13
    Vielen dank!!!

  7. #7
    Neuer Benutzer Öfters hier
    Registriert seit
    30.11.2008
    Beiträge
    13
    Hallo,

    Mit understehend Programma probier ich die Werten von die SCL, SDA und INT1 aus zu lesen.

    Code:
    #include "RP6RobotBaseLib.h" 	
    	
    int main(void)
    {
    	initRobotBase(); 				
    	powerON();
    
    	DDRA &= ~E_INT1;	
    	DDRC &= ~(SCL | SDA);
    	
    	uint8_t temp1;
    	uint8_t temp2;
    	uint8_t temp3;
    	
    	temp1 = PINA & E_INT1;
    	temp2 = PINC & SCL;
    	temp3 = PINC & SDA;
    
    	PORTC &= ~SCL;
    	PORTC &= ~SDA;
    				
    	while(true)
    	{
    		if(temp1)
    		{
    			writeString_P("\nINT1 is hoog\n");
    		}
    		
    		if(!temp1)
    		{
    			writeString_P("\nINT1 is laag\n");
    		}
    		
    		if(temp2)
    		{
    			writeString_P("\nSCL is hoog\n");
    		}
    		
    		if(!temp2)
    		{
    			writeString_P("\nSCL is laag\n");
    		}
    		
    		if(temp3)
    		{
    			writeString_P("\nSDA is hoog\n");
    		}
    		
    		if(!temp3)
    		{
    			writeString_P("\nSDA is laag\n");
    		}
    	}
    	
    	return 0;
    }
    Aber die Werte wird nur einmal ausgelesen. Die volgendes Werten die in dem Terminal erscheint, ist immer dasselbte. Bei ein neues Program start, wird wieder die Werten nur einmal ausgelezen.

    Was mach ich falsch? Wie kan ich die Werten kontinuierlich auslesen?

    Gruss Fieke

  8. #8
    Moderator Robotik Visionär Avatar von radbruch
    Registriert seit
    27.12.2006
    Ort
    Stuttgart
    Alter
    54
    Beiträge
    5.781
    Blog-Einträge
    8
    Hallo Fieke

    Das ist ein interessantes Projekt. Allerdings ist Linienfolgen nicht wirklich die Spezialdisziplin des RP6. Schnelle Motoränderungen belasten das Getriebe sehr.

    Könnte das die Lösung für dein Problem sein?
    Code:
    #include "RP6RobotBaseLib.h"
    
    int main(void)
    {
       initRobotBase();
       powerON();
    
       uint8_t temp_E_INT1;
       uint8_t temp_SCL;
       uint8_t temp_SDA;
    
       DDRA &= ~E_INT1; // Datenrichtung Eingang
       DDRC &= ~(SCL | SDA);
    
       PORTA &= ~E_INT1; // PullUps deaktivieren
       PORTC &= ~(SCL | SDA);
    
       while(true)
       {
       	temp_E_INT1 = PINA & E_INT1; // Pins einlesen
    		temp_SCL = PINC & SCL;
       	temp_SDA = PINC & SDA;
    
          if(temp_E_INT1)
          {
             writeString_P("\nINT1 is hoog\n");
          }
    
          if(!temp_E_INT1)
          {
             writeString_P("\nINT1 is laag\n");
          }
    
          if(temp_SCL)
          {
             writeString_P("\nSCL is hoog\n");
          }
    
          if(!temp_SCL)
          {
             writeString_P("\nSCL is laag\n");
          }
    
          if(temp_SDA)
          {
             writeString_P("\nSDA is hoog\n");
          }
    
          if(!temp_SDA)
          {
             writeString_P("\nSDA is laag\n");
          }
       mSleep(300);
       }
    
       return 0;
    }
    Du darst die Pinabfrage selbstverständlich auch direkt als Bedingung verwenden:
    Code:
          if(PINA & E_INT1)
          {
             writeString_P("\nINT1 is hoog\n");
          }
          else
          {
             writeString_P("\nINT1 is laag\n");
          }
    Gruß

    mic

    Atmel’s products are not intended, authorized, or warranted for use
    as components in applications intended to support or sustain life!

  9. #9
    Neuer Benutzer Öfters hier
    Registriert seit
    30.11.2008
    Beiträge
    13
    Hallo Mic,

    Vielen dank für Ihre Hilfe. Ich hat die Werten nur einmal ausgelesen, das auslesen hatte ich nicht in die While loop geprogrammiert.
    Jetzt kan ich die Werten gut auslesen. Auf dieser Moment programmier ich die Robot dass ihr die Liene kan folgen.
    Es geht noch nicht gut, aber es gibt ein Begin. Ich denke dass ich es weiter kann machen.

    Gruss Fieke

  10. #10
    Neuer Benutzer Öfters hier
    Registriert seit
    30.11.2008
    Beiträge
    13
    Hallo,

    Jetzt kann die Robot die Liene folgen, mit unterstehendes Program:

    Code:
    #include "RP6RobotBaseLib.h" 	// moet er altijd bij
    
    uint8_t licht(void)
    {
    	uint8_t vlinks;
    	uint8_t vrechts;
    	uint8_t lichtlinks;
    	uint8_t lichtrechts;
    	
    	task_ADC;
    	
    	lichtlinks = adcLSL;
    	lichtrechts = adcLSR;
    	
    	writeString_P("\nlichtlinks =	");
    	writeInteger(lichtlinks, DEC);
    	
    	writeString_P("\nlichtrechts =	");
    	writeInteger(lichtrechts, DEC);
    		
    	if(lichtlinks > 230 || lichtrechts > 230)
    	{
    		vlinks=0;
    		vrechts=0;
    		writeString_P("\nVeel licht\n");
    	}
    	
    	else 
    	{
    		vlinks=10;
    	}
    	
    	return vlinks;
    }
    
    
    uint8_t acsStateChanged(void)
    {
    	uint8_t vlinks;
    	uint8_t vrechts;
    	writeString_P("\nObstakel veranderd");
    	
    	if(obstacle_left || obstacle_right)
    	{
    		vlinks=0;
    		vrechts=0;
    		writeString_P("\n Obstakel HELP!!!");
    	}
    	
    	return vlinks;
    
    }
    
    
    
    uint8_t lijnvolgenlinks(uint8_t snelheidlinks, uint8_t snelheidrechts)
    {
    	
    	DDRA &= ~E_INT1;
    	
    	DDRC &= ~(SCL | SDA);
    	
    	uint8_t templinks;
    	uint8_t temprechts;
    	uint8_t tempmidden;
    	
    	PORTC &= ~SCL;
    	PORTC &= ~SDA;
    				
    	uint8_t m101;
    	uint8_t l100;
    	uint8_t ll110;
    	uint8_t r001;
    	uint8_t rr011;
    	uint8_t niks000;
    	uint8_t alles111;
    	
    	uint8_t delervooruit = 2;
    	uint8_t delerneteraf = 3;
    	uint8_t delerverafoptel = 2;
    	uint8_t delerverafaftrek = 2;
    	
    		
    		//writeString_P("\nlijnvolgenlinks\n");
    		
    		if (snelheidlinks > 20 || snelheidrechts > 20)
    		{
    			delervooruit = 5;
    		}
    		
    		tempmidden = PINA & E_INT1;
    		templinks = PINC & SCL;
    		temprechts = PINC & SDA;
    		uint8_t casus;
    		
    		if(templinks && !tempmidden && temprechts)
    		{
    			casus=101;
    		}
    		
    		if(templinks && !tempmidden && !temprechts)
    		{
    			casus=100;
    		}
    		
    		if(templinks && tempmidden && !temprechts)
    		{
    			casus=110;
    		}
    		
    		if(!templinks && !tempmidden && temprechts)
    		{
    			casus=001;
    		}
    		
    		if(!templinks && tempmidden && temprechts)
    		{
    			casus=011;
    		}
    		
    		if(!templinks && !tempmidden && !temprechts)
    		{
    			casus=000;
    		}
    		
    		if(templinks && tempmidden && temprechts)
    		{
    			casus=111;
    		}
    		
    		
    		switch(casus)
    		{
    			case 101: 	if(l100 > 0)
    						{
    							snelheidlinks = snelheidrechts - (snelheidrechts/delerneteraf);
    						}
    						
    						if(ll110 > 0)
    						{
    							snelheidlinks = snelheidrechts - (snelheidrechts/delerverafaftrek);
    						}
    						
    						if(r001 > 0)
    						{
    							snelheidrechts = snelheidlinks - (snelheidlinks/delerneteraf);
    						}
    						
    						if(rr011 > 0)
    						{
    							snelheidrechts = snelheidlinks - (snelheidlinks/delerverafaftrek);
    						}
    						
    						else
    						{
    			
    							snelheidlinks = snelheidlinks +(snelheidlinks/delervooruit);		
    							//writeString_P("\n 101 snelheidlinks");
    							//writeInteger(snelheidlinks,DEC);
    						
    							snelheidrechts = snelheidlinks;
    							//writeString_P("\n 101 snelheidrechts");
    							//writeInteger(snelheidrechts,DEC);
    						}
    						
    						
    						m101++; l100=0; ll110=0; r001=0; rr011=0; niks000=0; alles111=0; break;
    			
    			case 100: 	//writeString_P("\n 100 slinks");
    						//writeInteger(snelheidlinks,DEC);
    			
    						snelheidrechts=snelheidrechts-(snelheidrechts/delerneteraf);
    						//writeString_P("\n 100 srechts");
    						//writeInteger(snelheidrechts,DEC);
    												
    						m101=0; l100++; ll110=0; r001=0; rr011=0; niks000=0; alles111=0; break;
    			
    			case 110: 	snelheidlinks=snelheidlinks+(snelheidlinks/delerverafoptel);
    						//writeString_P("\n 110 slinks");
    						//writeInteger(snelheidlinks,DEC);
    			
    						snelheidrechts=snelheidrechts-(snelheidrechts/delerverafaftrek);
    						//writeString_P("\n 110 srechts");
    						//writeInteger(snelheidrechts,DEC);
    						
    										
    						m101=0; l100=0; ll110++; r001=0; rr011=0; niks000=0; alles111=0; break;
    			
    			case 001: 	snelheidlinks=snelheidlinks-(snelheidlinks/delerneteraf);
    						//writeString_P("\n 001 slinks");
    						//writeInteger(snelheidlinks,DEC);
    						
    						//writeString_P("\n 001 srechts");
    						//writeInteger(snelheidrechts,DEC);
    			
    						m101=0; l100=0; ll110=0; r001++; rr011=0; niks000=0; alles111=0; break;
    			
    			case 011: 	snelheidlinks=snelheidlinks-(snelheidlinks/delerverafaftrek);
    						//writeString_P("\n 011 slinks");
    						//writeInteger(snelheidlinks,DEC);
    			
    						snelheidrechts=snelheidrechts+(snelheidrechts/delerverafoptel);
    						//writeString_P("\n 011 srechts");
    						//writeInteger(snelheidrechts,DEC);
    											
    						m101=0; l100=0; ll110=0; r001=0; rr011++; niks000=0; alles111=0; break;
    			
    			case 000: 								
    
    						m101=0; l100=0; ll110=0; r001=0; rr011=0; niks000++; alles111=0; break;
    		
    			case 111: 				
    						m101=0; l100=0; ll110=0; r001=0; rr011=0; niks000=0; alles111++; break;
    			
    			default: writeString("\n Iets anders\n"); break;
    		}
    		
    		if(snelheidlinks > 50)
    		{
    			snelheidlinks = 50;
    		}
    		
    		if(snelheidrechts > 50)
    		{
    			snelheidrechts = 50;
    		}
    		
    		if(snelheidlinks < 10)
    		{
    			snelheidlinks=10;
    		
    		}
    		
    		if(snelheidrechts <10)
    		{
    			snelheidrechts=10;
    		}
    					
    		//writeString("\n main snelheidlinks = ");
    		//writeInteger(snelheidlinks,DEC);
    		
    		//writeString("\n main snelheidrechts = ");
    		//writeInteger(snelheidrechts,DEC);
    		
    		//writeString("\n m101 = ");
    		//writeInteger(m101,DEC);
    		
    		//writeString("\n l100 = ");
    		//writeInteger(l100,DEC);
    		
    		//writeString("\n ll110 = ");
    		//writeInteger(ll110,DEC);
    		
    		//writeString("\n r001 = ");
    		//writeInteger(r001,DEC);
    		
    		//writeString("\n rr011 = ");
    		//writeInteger(rr011,DEC);
    		
    		//writeString("\n niks000 = ");
    		//writeInteger(niks000,DEC);
    		
    		//writeString("\n alles111 = ");
    		//writeInteger(alles111,DEC);
    		
    			
    		return snelheidlinks;
    		
    	
    }
    
    uint8_t lijnvolgenrechts(uint8_t snelheidlinks, uint8_t snelheidrechts)
    {
    		
    	uint8_t templinks;
    	uint8_t temprechts;
    	uint8_t tempmidden;
    					
    	uint8_t m101;
    	uint8_t l100;
    	uint8_t ll110;
    	uint8_t r001;
    	uint8_t rr011;
    	uint8_t niks000;
    	uint8_t alles111;
    	
    	uint8_t delervooruit = 2;
    	uint8_t delerneteraf = 3;
    	uint8_t delerverafoptel = 2;
    	uint8_t delerverafaftrek = 2;
    		
    		
    		//writeString_P("\nlijnvolgenrechts\n");
    		
    		if (snelheidlinks > 20 || snelheidrechts > 20)
    		{
    			delervooruit = 5;
    		}
    		
    		tempmidden = PINA & E_INT1;
    		templinks = PINC & SCL;
    		temprechts = PINC & SDA;
    		uint8_t casus;
    		
    		if(templinks && !tempmidden && temprechts)
    		{
    			casus=101;
    		}
    		
    		if(templinks && !tempmidden && !temprechts)
    		{
    			casus=100;
    		}
    		
    		if(templinks && tempmidden && !temprechts)
    		{
    			casus=110;
    		}
    		
    		if(!templinks && !tempmidden && temprechts)
    		{
    			casus=001;
    		}
    		
    		if(!templinks && tempmidden && temprechts)
    		{
    			casus=011;
    		}
    		
    		if(!templinks && !tempmidden && !temprechts)
    		{
    			casus=000;
    		}
    		
    		if(templinks && tempmidden && temprechts)
    		{
    			casus=111;
    		}
    		
    		
    		switch(casus)
    		{
    			case 101: 	if(l100 > 0)
    						{
    							snelheidlinks = snelheidrechts - (snelheidrechts/delerneteraf);
    						}
    						
    						if(ll110 > 0)
    						{
    							snelheidlinks = snelheidrechts - (snelheidrechts/delerverafaftrek);
    						}
    						
    						if(r001 > 0)
    						{
    							snelheidrechts = snelheidlinks - (snelheidlinks/delerneteraf);
    						}
    						
    						if(rr011 > 0)
    						{
    							snelheidrechts = snelheidlinks - (snelheidlinks/delerverafaftrek);
    						}
    						
    						else
    						{
    			
    							snelheidlinks = snelheidlinks +(snelheidlinks/delervooruit);		
    							//writeString_P("\n 101 snelheidlinks");
    							//writeInteger(snelheidlinks,DEC);
    						
    							snelheidrechts = snelheidlinks;
    							//writeString_P("\n 101 snelheidrechts");
    							//writeInteger(snelheidrechts,DEC);
    						}
    						
    						
    						m101++; l100=0; ll110=0; r001=0; rr011=0; niks000=0; alles111=0; break;
    			
    			case 100: 	//writeString_P("\n 100 slinks");
    						//writeInteger(snelheidlinks,DEC);
    			
    						snelheidrechts=snelheidrechts-(snelheidrechts/delerneteraf);
    						//writeString_P("\n 100 srechts");
    						//writeInteger(snelheidrechts,DEC);
    												
    						m101=0; l100++; ll110=0; r001=0; rr011=0; niks000=0; alles111=0; break;
    			
    			case 110: 	snelheidlinks=snelheidlinks+(snelheidlinks/delerverafoptel);
    						//writeString_P("\n 110 slinks");
    						//writeInteger(snelheidlinks,DEC);
    			
    						snelheidrechts=snelheidrechts-(snelheidrechts/delerverafaftrek);
    						//writeString_P("\n 110 srechts");
    						//writeInteger(snelheidrechts,DEC);
    						
    										
    						m101=0; l100=0; ll110++; r001=0; rr011=0; niks000=0; alles111=0; break;
    			
    			case 001: 	snelheidlinks=snelheidlinks-(snelheidlinks/delerneteraf);
    						//writeString_P("\n 001 slinks");
    						//writeInteger(snelheidlinks,DEC);
    						
    						//writeString_P("\n 001 srechts");
    						//writeInteger(snelheidrechts,DEC);
    			
    						m101=0; l100=0; ll110=0; r001++; rr011=0; niks000=0; alles111=0; break;
    			
    			case 011: 	snelheidlinks=snelheidlinks-(snelheidlinks/delerverafaftrek);
    						//writeString_P("\n 011 slinks");
    						//writeInteger(snelheidlinks,DEC);
    			
    						snelheidrechts=snelheidrechts+(snelheidrechts/delerverafoptel);
    						//writeString_P("\n 011 srechts");
    						//writeInteger(snelheidrechts,DEC);
    											
    						m101=0; l100=0; ll110=0; r001=0; rr011++; niks000=0; alles111=0; break;
    			
    			case 000: 								
    
    						m101=0; l100=0; ll110=0; r001=0; rr011=0; niks000++; alles111=0; break;
    		
    			case 111: 				
    						m101=0; l100=0; ll110=0; r001=0; rr011=0; niks000=0; alles111++; break;
    			
    			default: writeString("\n Iets anders\n"); break;
    		}
    		
    		if(snelheidlinks > 50)
    		{
    			snelheidlinks = 50;
    		}
    		
    		if(snelheidrechts > 50)
    		{
    			snelheidrechts = 50;
    		}
    		
    		if(snelheidlinks < 10)
    		{
    			snelheidlinks=10;
    		
    		}
    		
    		if(snelheidrechts <10)
    		{
    			snelheidrechts=10;
    		}
    					
    		//writeString("\n main snelheidlinks = ");
    		//writeInteger(snelheidlinks,DEC);
    		
    		//writeString("\n main snelheidrechts = ");
    		//writeInteger(snelheidrechts,DEC);
    		
    		//writeString("\n m101 = ");
    		//writeInteger(m101,DEC);
    		
    		//writeString("\n l100 = ");
    		//writeInteger(l100,DEC);
    		
    		//writeString("\n ll110 = ");
    		//writeInteger(ll110,DEC);
    		
    		//writeString("\n r001 = ");
    		//writeInteger(r001,DEC);
    		
    		//writeString("\n rr011 = ");
    		//writeInteger(rr011,DEC);
    		
    		//writeString("\n niks000 = ");
    		//writeInteger(niks000,DEC);
    		
    		//writeString("\n alles111 = ");
    		//writeInteger(alles111,DEC);
    		
    		
    		
    		return snelheidrechts;
    		
    	
    }
    	
    int main(void)
    {
    	initRobotBase(); 			// altijd eerst dit voor rest programma
    	//writeString_P("Hoi, hier is robby de robot! Gezellig!\n Alles goed?\n");
    	
    	//ACS_setStateChangedHandler(acsStateChanged);
    	
    	DDRA &= ~E_INT1;
    	
    	DDRC &= ~(SCL | SDA);
    	
    	PORTC &= ~SCL;
    	PORTC &= ~SDA;
    	uint8_t vstart = 10;			
    	
    	powerON();
    	
    	//setACSPwrMed();
    	
    	uint8_t slinks = vstart;
    	uint8_t srechts = vstart;
    	uint8_t snelheidlinks;
    	uint16_t xlinks;
    	
    	
    	
    	while(true)
    	{	
    		uint16_t tijdelijk;
    		slinks = lijnvolgenlinks(slinks,srechts);
    		srechts = lijnvolgenrechts(slinks,srechts);
    		
    		task_motionControl();
    		task_ADC();
    		xlinks = licht();
    		
    		writeString_P("\nxlinks = ");
    		writeInteger(xlinks, DEC);
    		
    		//xlinks == tijdelijk;
    		
    		//writeString_P("\ntijdelijk = ");
    		//writeInteger(tijdelijk, DEC);
    		
    		if(xlinks == 0)
    		{
    			slinks = 0;
    			srechts = 0;
    			writeString_P("\nin de if als er veel licht is");
    		}
    		
    		//task_ACS();
    		//snelheidlinks = acsStateChanged();
    		//if (snelheidlinks = 0)
    		//{
    		//	slinks = 0;
    		//	srechts = 0;
    		//}
    		writeString_P("\nslinks =");
    		writeInteger(slinks,DEC);
    		
    		writeString_P("\nsrechts =");
    		writeInteger(srechts,DEC);
    		
    		moveAtSpeed(slinks,srechts);
    		
    		//mSleep(1000);
    		writeString_P("\n\n");
    		
    	}
    	
    	return 0;
    }
    Aber ich will auch dass die Robot stopt wenn der eine Object vor die Robot ist. Ich habe Program womit die Robot Abstand halt.

    Code:
    #include "RP6RobotBaseLib.h" 	// moet er altijd bij
    
    void acsStateChanged(void)
    {
    	//writeString_P("ACS status is veranderd\n");
    	
    	if(obstacle_left && obstacle_right)
    	{
    		//writeString_P("Iets beide\n");
    		task_ADC();
    		task_motionControl();
    		setMotorDir(FWD, FWD);
    		moveAtSpeed(0,0);
    	}
    	
    	if(obstacle_left && !obstacle_right)
    	{
    		//writeString_P("Iets links\n");
    		task_ADC();
    		task_motionControl();
    		setMotorDir(BWD, FWD);
    		moveAtSpeed(40,40);
    	}
    	
    	if(!obstacle_left && obstacle_right)
    	{
    		//writeString_P("Iets rechts\n");
    		task_ADC();
    		task_motionControl();
    		setMotorDir(FWD, BWD);
    		moveAtSpeed(40,40);
    	}
    	
    	if(!obstacle_left && !obstacle_right)
    	{
    		//writeString_P("Niks\n");
    		task_ADC();
    		task_motionControl();
    		setMotorDir(FWD, FWD);
    		moveAtSpeed(40,40);
    	}
    }
    	
    int main(void)
    {
    	initRobotBase(); 			// altijd eerst dit voor rest programma
    	//writeString("Hoi, hier is robby de robot! Gezellig!\n Alles goed?\n");
    	
    	ACS_setStateChangedHandler(acsStateChanged);
    	powerON();
    	setACSPwrMed();
    	
    			
    	while(true)
    	{
    		task_ACS();
    		task_motionControl();
    		
    	}
    	
    	return 0;
    }
    Aber wie kann ich beide Program kombinieren? Ich habe es geprobiert mit unterstehend Program, aber denn kann ihr die Liene nicht mir volgen. Denn wurde dass Lienefolg Program nur einmal gelesen, also kann die Robot die Liene nicht mehr folgen.

    Code:
    void bumper(void)
    {
    	uint8_t vlinks;
    	uint8_t vrechts;
    	
    	writeString_P("\n bumper checken!");
    	
    		
    	uint8_t bumperl = getBumperLeft();
    	uint8_t bumperr = getBumperRight();
    
    	if (bumperl || bumperr)
    	{
    		moveAtSpeed(0,0);
    					
    		task_motionControl();
    		task_ADC();
    		
    		move(40, BWD, DIST_MM(100), true);
    		
    		rotate(40, RIGHT, 180, true);
    				
    		writeString_P("\n OEPS, bumper geraakt");
    	}
    
    }
    
    uint8_t licht(void)
    {
    	uint8_t vlinks;
    	uint8_t vrechts;
    	uint8_t lichtlinks;
    	uint8_t lichtrechts;
    	
    	
    	task_ADC;
    	
    	lichtlinks = adcLSL;
    	lichtrechts = adcLSR;
    	
    	writeString_P("\nlichtlinks =	");
    	writeInteger(lichtlinks, DEC);
    	
    	writeString_P("\nlichtrechts =	");
    	writeInteger(lichtrechts, DEC);
    		
    	if(lichtlinks > 300 || lichtrechts > 300)
    	{
    		vlinks=0;
    		vrechts=0;
    		writeString_P("\nVeel licht\n");
    	}
    	
    	else 
    	{
    		vlinks=10;
    	}
    	
    	return vlinks;
    }
    
    
    uint8_t lijnvolgenlinks(uint8_t snelheidlinks, uint8_t snelheidrechts)
    {
    	
    	DDRA &= ~E_INT1;
    	
    	DDRC &= ~(SCL | SDA);
    	
    	uint8_t templinks;
    	uint8_t temprechts;
    	uint8_t tempmidden;
    	
    	PORTC &= ~SCL;
    	PORTC &= ~SDA;
    				
    	uint8_t m101;
    	uint8_t l100;
    	uint8_t ll110;
    	uint8_t r001;
    	uint8_t rr011;
    	uint8_t niks000;
    	uint8_t alles111;
    	
    	uint8_t delervooruit = 2;
    	uint8_t delerneteraf = 3;
    	uint8_t delerverafoptel = 2;
    	uint8_t delerverafaftrek = 2;
    	
    		
    		//writeString_P("\nlijnvolgenlinks\n");
    		
    		if (snelheidlinks > 20 || snelheidrechts > 20)
    		{
    			delervooruit = 5;
    		}
    		
    		tempmidden = PINA & E_INT1;
    		templinks = PINC & SCL;
    		temprechts = PINC & SDA;
    		uint8_t casus;
    		
    		if(templinks && !tempmidden && temprechts)
    		{
    			casus=101;
    		}
    		
    		if(templinks && !tempmidden && !temprechts)
    		{
    			casus=100;
    		}
    		
    		if(templinks && tempmidden && !temprechts)
    		{
    			casus=110;
    		}
    		
    		if(!templinks && !tempmidden && temprechts)
    		{
    			casus=001;
    		}
    		
    		if(!templinks && tempmidden && temprechts)
    		{
    			casus=011;
    		}
    		
    		if(!templinks && !tempmidden && !temprechts)
    		{
    			casus=000;
    		}
    		
    		if(templinks && tempmidden && temprechts)
    		{
    			casus=111;
    		}
    		
    		
    		switch(casus)
    		{
    			case 101: 	if(l100 > 0)
    						{
    							snelheidlinks = snelheidrechts - (snelheidrechts/delerneteraf);
    						}
    						
    						if(ll110 > 0)
    						{
    							snelheidlinks = snelheidrechts - (snelheidrechts/delerverafaftrek);
    						}
    						
    						if(r001 > 0)
    						{
    							snelheidrechts = snelheidlinks - (snelheidlinks/delerneteraf);
    						}
    						
    						if(rr011 > 0)
    						{
    							snelheidrechts = snelheidlinks - (snelheidlinks/delerverafaftrek);
    						}
    						
    						else
    						{
    			
    							snelheidlinks = snelheidlinks +(snelheidlinks/delervooruit);		
    							//writeString_P("\n 101 snelheidlinks");
    							//writeInteger(snelheidlinks,DEC);
    						
    							snelheidrechts = snelheidlinks;
    							//writeString_P("\n 101 snelheidrechts");
    							//writeInteger(snelheidrechts,DEC);
    						}
    						
    						
    						m101++; l100=0; ll110=0; r001=0; rr011=0; niks000=0; alles111=0; break;
    			
    			case 100: 	//writeString_P("\n 100 slinks");
    						//writeInteger(snelheidlinks,DEC);
    			
    						snelheidrechts=snelheidrechts-(snelheidrechts/delerneteraf);
    						//writeString_P("\n 100 srechts");
    						//writeInteger(snelheidrechts,DEC);
    												
    						m101=0; l100++; ll110=0; r001=0; rr011=0; niks000=0; alles111=0; break;
    			
    			case 110: 	snelheidlinks=snelheidlinks+(snelheidlinks/delerverafoptel);
    						//writeString_P("\n 110 slinks");
    						//writeInteger(snelheidlinks,DEC);
    			
    						snelheidrechts=snelheidrechts-(snelheidrechts/delerverafaftrek);
    						//writeString_P("\n 110 srechts");
    						//writeInteger(snelheidrechts,DEC);
    						
    										
    						m101=0; l100=0; ll110++; r001=0; rr011=0; niks000=0; alles111=0; break;
    			
    			case 001: 	snelheidlinks=snelheidlinks-(snelheidlinks/delerneteraf);
    						//writeString_P("\n 001 slinks");
    						//writeInteger(snelheidlinks,DEC);
    						
    						//writeString_P("\n 001 srechts");
    						//writeInteger(snelheidrechts,DEC);
    			
    						m101=0; l100=0; ll110=0; r001++; rr011=0; niks000=0; alles111=0; break;
    			
    			case 011: 	snelheidlinks=snelheidlinks-(snelheidlinks/delerverafaftrek);
    						//writeString_P("\n 011 slinks");
    						//writeInteger(snelheidlinks,DEC);
    			
    						snelheidrechts=snelheidrechts+(snelheidrechts/delerverafoptel);
    						//writeString_P("\n 011 srechts");
    						//writeInteger(snelheidrechts,DEC);
    											
    						m101=0; l100=0; ll110=0; r001=0; rr011++; niks000=0; alles111=0; break;
    			
    			case 000: 								
    
    						m101=0; l100=0; ll110=0; r001=0; rr011=0; niks000++; alles111=0; break;
    		
    			case 111: 				
    						m101=0; l100=0; ll110=0; r001=0; rr011=0; niks000=0; alles111++; break;
    			
    			default: writeString("\n Iets anders\n"); break;
    		}
    		
    		if(snelheidlinks > 50)
    		{
    			snelheidlinks = 50;
    		}
    		
    		if(snelheidrechts > 50)
    		{
    			snelheidrechts = 50;
    		}
    		
    		if(snelheidlinks < 10)
    		{
    			snelheidlinks=10;
    		
    		}
    		
    		if(snelheidrechts <10)
    		{
    			snelheidrechts=10;
    		}
    					
    		//writeString("\n main snelheidlinks = ");
    		//writeInteger(snelheidlinks,DEC);
    		
    		//writeString("\n main snelheidrechts = ");
    		//writeInteger(snelheidrechts,DEC);
    		
    		//writeString("\n m101 = ");
    		//writeInteger(m101,DEC);
    		
    		//writeString("\n l100 = ");
    		//writeInteger(l100,DEC);
    		
    		//writeString("\n ll110 = ");
    		//writeInteger(ll110,DEC);
    		
    		//writeString("\n r001 = ");
    		//writeInteger(r001,DEC);
    		
    		//writeString("\n rr011 = ");
    		//writeInteger(rr011,DEC);
    		
    		//writeString("\n niks000 = ");
    		//writeInteger(niks000,DEC);
    		
    		//writeString("\n alles111 = ");
    		//writeInteger(alles111,DEC);
    		
    			
    		return snelheidlinks;
    		
    	
    }
    
    uint8_t lijnvolgenrechts(uint8_t snelheidlinks, uint8_t snelheidrechts)
    {
    		
    	uint8_t templinks;
    	uint8_t temprechts;
    	uint8_t tempmidden;
    					
    	uint8_t m101;
    	uint8_t l100;
    	uint8_t ll110;
    	uint8_t r001;
    	uint8_t rr011;
    	uint8_t niks000;
    	uint8_t alles111;
    	
    	uint8_t delervooruit = 2;
    	uint8_t delerneteraf = 3;
    	uint8_t delerverafoptel = 2;
    	uint8_t delerverafaftrek = 2;
    		
    		
    		//writeString_P("\nlijnvolgenrechts\n");
    		
    		if (snelheidlinks > 20 || snelheidrechts > 20)
    		{
    			delervooruit = 5;
    		}
    		
    		tempmidden = PINA & E_INT1;
    		templinks = PINC & SCL;
    		temprechts = PINC & SDA;
    		uint8_t casus;
    		
    		if(templinks && !tempmidden && temprechts)
    		{
    			casus=101;
    		}
    		
    		if(templinks && !tempmidden && !temprechts)
    		{
    			casus=100;
    		}
    		
    		if(templinks && tempmidden && !temprechts)
    		{
    			casus=110;
    		}
    		
    		if(!templinks && !tempmidden && temprechts)
    		{
    			casus=001;
    		}
    		
    		if(!templinks && tempmidden && temprechts)
    		{
    			casus=011;
    		}
    		
    		if(!templinks && !tempmidden && !temprechts)
    		{
    			casus=000;
    		}
    		
    		if(templinks && tempmidden && temprechts)
    		{
    			casus=111;
    		}
    		
    		
    		switch(casus)
    		{
    			case 101: 	if(l100 > 0)
    						{
    							snelheidlinks = snelheidrechts - (snelheidrechts/delerneteraf);
    						}
    						
    						if(ll110 > 0)
    						{
    							snelheidlinks = snelheidrechts - (snelheidrechts/delerverafaftrek);
    						}
    						
    						if(r001 > 0)
    						{
    							snelheidrechts = snelheidlinks - (snelheidlinks/delerneteraf);
    						}
    						
    						if(rr011 > 0)
    						{
    							snelheidrechts = snelheidlinks - (snelheidlinks/delerverafaftrek);
    						}
    						
    						else
    						{
    			
    							snelheidlinks = snelheidlinks +(snelheidlinks/delervooruit);		
    							//writeString_P("\n 101 snelheidlinks");
    							//writeInteger(snelheidlinks,DEC);
    						
    							snelheidrechts = snelheidlinks;
    							//writeString_P("\n 101 snelheidrechts");
    							//writeInteger(snelheidrechts,DEC);
    						}
    						
    						
    						m101++; l100=0; ll110=0; r001=0; rr011=0; niks000=0; alles111=0; break;
    			
    			case 100: 	//writeString_P("\n 100 slinks");
    						//writeInteger(snelheidlinks,DEC);
    			
    						snelheidrechts=snelheidrechts-(snelheidrechts/delerneteraf);
    						//writeString_P("\n 100 srechts");
    						//writeInteger(snelheidrechts,DEC);
    												
    						m101=0; l100++; ll110=0; r001=0; rr011=0; niks000=0; alles111=0; break;
    			
    			case 110: 	snelheidlinks=snelheidlinks+(snelheidlinks/delerverafoptel);
    						//writeString_P("\n 110 slinks");
    						//writeInteger(snelheidlinks,DEC);
    			
    						snelheidrechts=snelheidrechts-(snelheidrechts/delerverafaftrek);
    						//writeString_P("\n 110 srechts");
    						//writeInteger(snelheidrechts,DEC);
    						
    										
    						m101=0; l100=0; ll110++; r001=0; rr011=0; niks000=0; alles111=0; break;
    			
    			case 001: 	snelheidlinks=snelheidlinks-(snelheidlinks/delerneteraf);
    						//writeString_P("\n 001 slinks");
    						//writeInteger(snelheidlinks,DEC);
    						
    						//writeString_P("\n 001 srechts");
    						//writeInteger(snelheidrechts,DEC);
    			
    						m101=0; l100=0; ll110=0; r001++; rr011=0; niks000=0; alles111=0; break;
    			
    			case 011: 	snelheidlinks=snelheidlinks-(snelheidlinks/delerverafaftrek);
    						//writeString_P("\n 011 slinks");
    						//writeInteger(snelheidlinks,DEC);
    			
    						snelheidrechts=snelheidrechts+(snelheidrechts/delerverafoptel);
    						//writeString_P("\n 011 srechts");
    						//writeInteger(snelheidrechts,DEC);
    											
    						m101=0; l100=0; ll110=0; r001=0; rr011++; niks000=0; alles111=0; break;
    			
    			case 000: 								
    
    						m101=0; l100=0; ll110=0; r001=0; rr011=0; niks000++; alles111=0; break;
    		
    			case 111: 				
    						m101=0; l100=0; ll110=0; r001=0; rr011=0; niks000=0; alles111++; break;
    			
    			default: writeString("\n Iets anders\n"); break;
    		}
    		
    		if(snelheidlinks > 50)
    		{
    			snelheidlinks = 50;
    		}
    		
    		if(snelheidrechts > 50)
    		{
    			snelheidrechts = 50;
    		}
    		
    		if(snelheidlinks < 10)
    		{
    			snelheidlinks=10;
    		
    		}
    		
    		if(snelheidrechts <10)
    		{
    			snelheidrechts=10;
    		}
    					
    		//writeString("\n main snelheidlinks = ");
    		//writeInteger(snelheidlinks,DEC);
    		
    		//writeString("\n main snelheidrechts = ");
    		//writeInteger(snelheidrechts,DEC);
    		
    		//writeString("\n m101 = ");
    		//writeInteger(m101,DEC);
    		
    		//writeString("\n l100 = ");
    		//writeInteger(l100,DEC);
    		
    		//writeString("\n ll110 = ");
    		//writeInteger(ll110,DEC);
    		
    		//writeString("\n r001 = ");
    		//writeInteger(r001,DEC);
    		
    		//writeString("\n rr011 = ");
    		//writeInteger(rr011,DEC);
    		
    		//writeString("\n niks000 = ");
    		//writeInteger(niks000,DEC);
    		
    		//writeString("\n alles111 = ");
    		//writeInteger(alles111,DEC);
    		
    		
    		
    		return snelheidrechts;
    		
    	
    }
    
    
    
    
    void acsStateChanged(void)
    {
    	//writeString_P("ACS status is veranderd\n");
    	uint8_t x;
    	uint8_t y;
    	
    	if(obstacle_left || obstacle_right)
    	{
    		
    		task_ADC();
    		task_motionControl();
    		setMotorDir(FWD, FWD);
    		moveAtSpeed(0,0);
    		x++;
    		y=0;
    		
    	}
    	
    	else
    	{
    		
    	DDRA &= ~E_INT1;
    	
    	DDRC &= ~(SCL | SDA);
    	
    	PORTC &= ~SCL;
    	PORTC &= ~SDA;
    	uint8_t vstart = 10;			
    	uint8_t slinks = vstart;
    	uint8_t srechts = vstart;
    	uint8_t snelheidlinks;
    	uint16_t xlinks;
    	uint8_t zlinks;
    	
    	slinks = lijnvolgenlinks(slinks,srechts);
    	srechts = lijnvolgenrechts(slinks,srechts);
    		
    	task_motionControl();
    	task_ADC();
    	xlinks = licht();
    		
    	//writeString_P("\nxlinks = ");
    	//writeInteger(xlinks, DEC);
    		
    			
    	if(xlinks == 0)
    	{
    		slinks = 0;
    		srechts = 0;
    		//writeString_P("\nin de if als er veel licht is");
    	}
    		
    		
    	bumper();
    			
    	//writeString_P("\nslinks =");
    	//writeInteger(slinks,DEC);
    		
    	//writeString_P("\nsrechts =");
    	//writeInteger(srechts,DEC);
    	setMotorDir(FWD, FWD);
    	moveAtSpeed(slinks,srechts);
    	
    		
    	//writeString_P("\n\n");
    	}
    	
    	y++;
    	x=0;
    }
    	
    int main(void)
    {
    	initRobotBase(); 			// altijd eerst dit voor rest programma
    	//writeString("Hoi, hier is robby de robot! Gezellig!\n Alles goed?\n");
    	
    	ACS_setStateChangedHandler(acsStateChanged);
    	powerON();
    	setACSPwrMed();
    	
    			
    	while(true)
    	{
    		task_ACS();
    		task_motionControl();
    		acsStateChanged();	
    	}
    	
    	return 0;
    }
    Wie kan ich die Robot die Liene folgen und abstand halten lassen?

    Gruss Fieke

Seite 1 von 2 12 LetzteLetzte

Berechtigungen

  • Neue Themen erstellen: Nein
  • Themen beantworten: Nein
  • Anhänge hochladen: Nein
  • Beiträge bearbeiten: Nein
  •