Voir la source de Code-source-robot-transporteur

== Avant propos ==

Vous êtes sur la page contenant le code source du robot transporteur NXT programmé par Déborah Saunders et Jean Wasilewski.

Le code se réparti en deux fichiers, le premier est lancé par le robot maitre, et le deuxième par le robot esclave.

Le schéma suivant explique comment raccorder les différents capteurs et servomoteurs :


[[Fichier:communication.jpg|center]]


Le servomoteurs gauche se branche sur l'entrée A, et le droit sur l'entrée B. Le capteur RFID se branche sur l'entrée 1, et celui de couleur sur le connecteur 2. Le moteur de levage se branche sur l'entrée 1, et la pince quant à elle se branche sur le connecteur B

== Code Source de l'esclave ==

<pre>

</pre>

== Code Source du maitre ==

<pre>
// Includes

#include "NXCDefs.h"
#include "BTlib.nxc"
#include "RFIDlib.nxc"
#include "NBCCommon.h"

// Input Definitions

#define SENS_RFID   IN_2
#define SENS_LIGHT  IN_1

// Output Definitions

#define MOTOR_LEFT   OUT_A
#define MOTOR_RIGHT  OUT_B
#define MOTORS       OUT_AB

// BlueTooth Definitions

#define BT_CONN 1
#define MAILBOX 0

// Speed Definitions

#define SPEED_SLOW -50
#define SPEED_FAST -80

// Color Definitions

/*
#define  INPUT_BLACKCOLOR  1
#define  INPUT_BLUECOLOR   2
#define  INPUT_GREENCOLOR  3
#define  INPUT_YELLOWCOLOR 4
#define  INPUT_REDCOLOR    5
#define  INPUT_WHITECOLOR  6
*/

#define  BLACK            INPUT_BLACKCOLOR
#define  BLUE             INPUT_BLUECOLOR
#define  GREEN            INPUT_GREENCOLOR
#define  YELLOW           INPUT_YELLOWCOLOR
#define  RED              INPUT_REDCOLOR
#define  WHITE            INPUT_WHITECOLOR

#define COLOR_LINE         BLACK
#define COLOR_BASE         RED
#define COLOR_DEPOSIT      BLUE

// State Definitions

#define GET_OBJECT      2
#define GOTOBASEGET     3
#define LET_OBJECT      1
#define GETOUTBASETOLET 4
#define GOTOBASELET     5
#define GETOUTBASETOGET 6

// Main Globals

int state = GET_OBJECT;
int color_line = BLACK;
int color_base = RED;
int color_depo = BLUE;
bool got_object = false;

bool not(bool in)
{
     if(in)
     {
         return false;
     }
     else
     {
         return true;
     }
}

// BT Initialization Procedure

sub slaveBTCheck(int conn)
{
   string cStr;
   cStr = NumToStr(conn);
   cStr = StrCat("on line",cStr);
   if (!BTCommCheck(conn))
   {
      TextOut(5,LCD_LINE2,"*Connect NXT");
      TextOut(5,LCD_LINE3,cStr);
      TextOut(60,LCD_LINE3,"from");
      TextOut(5,LCD_LINE4,"master NXT menu");
      Wait(3000);
      Stop(true);
   }
}

// Colorreader Globals

ColorSensorReadType csr;

// Colorreader Initialization Procedure

void init_colorreader()
{
    SetSensorColorFull(SENS_LIGHT);
    csr.Port = SENS_LIGHT;
    SysColorSensorRead(csr);
}

// Rouler function

void rouler()
{
	OnFwd(MOTOR_LEFT, SPEED_FAST);
	OnFwd(MOTOR_RIGHT, SPEED_FAST);
}

// RFID Globals

string data = "";
byte a[5];

// RFID Initiation

void init_rfid()
{
     string version, manufacturer, type;
     string serial = "";
     // set sensor port to read I2C data
     SetSensorLowspeed(SENS_RFID);
     // send dummy command to wake up sensor
     RFIDDummy(SENS_RFID);

     // get sensor version, manufacturer, type and serial number
     version = GetI2CString(SENS_RFID,RFID_ADDRESS,RFID_VERSION);
     manufacturer = GetI2CString(SENS_RFID,RFID_ADDRESS,RFID_MANUFACTURER);
     type = GetI2CString(SENS_RFID,RFID_ADDRESS,RFID_TYPE);
     GetRFIDSerial(SENS_RFID,serial);
}

// RFID Procedure

int check_rfid()
{
     GetRFIDArray(SENS_RFID,a,true);
     // convert data for visualization
     RFIDDataToString(a,data);
     if ((a[4]%3)==2)
     {
         TextOut(5, LCD_LINE3, "2");
         return 2;
     }
     else if ((a[4]%3)==1)
     {
         TextOut(5, LCD_LINE3, "1");
         return 1;
     }
     else
     {
         TextOut(5, LCD_LINE3, "0");
         return 0;
     }
}

// Find line global

int rl = -1;

// Find line functions

void find_back_line_1()
{
     bool line_found = false;
     int initClock = 0;
     int clock = 0;
     int multi = 100;
     int cpt = 0;
     
           while ((!line_found)&&(cpt<=5))
     {
     
     
           OnFwdSync(MOTORS, SPEED_FAST, rl * 80);
           SysColorSensorRead(csr);
           clock = clock + 1;
           if((csr.ColorValue == color_line)||(csr.ColorValue == color_depo))
           {
                  line_found = true;
           }
           if(clock<multi)
           {
                  rl = -1;
                  TextOut(5,LCD_LINE3,"CL TIME 1 ...");
           }
           else if(clock<=(multi*2))
           {
                  //rl = rl * -1;
                  rl = 1;
                  TextOut(5,LCD_LINE3,"CL TIME 2 ...");
           }
           else
           {
               multi=multi*4;
               cpt = cpt +1;
           }
     }
     
     //Off(MOTORS);
}


void find_back_line_2()
{
     bool line_found = false;
     int initClock = 0;
     int clock = 0;
     int multi = 100;
     int cpt = 0;

           while ((!line_found)&&(cpt<=5))
     {


           OnFwdSync(MOTORS, SPEED_FAST, rl * 80);
           SysColorSensorRead(csr);
           clock = clock + 1;
           if((csr.ColorValue == color_line)||(csr.ColorValue == color_base))
           {
                  line_found = true;
           }
           if(clock<multi)
           {
                  rl = -1;
                  TextOut(5,LCD_LINE3,"CL TIME 1 ...");
           }
           else if(clock<=(multi*2))
           {
                  //rl = rl * -1;
                  rl = 1;
                  TextOut(5,LCD_LINE3,"CL TIME 2 ...");
           }
           else
           {
               multi=multi*4;
               cpt = cpt +1;
           }
     }
     //if(csr.ColorValue == COLOR_LINE)
}

// Main task

task main()
{
  slaveBTCheck(BT_CONN);
	init_colorreader();
	init_rfid();
	while(true)
	{
		if(state == GET_OBJECT)
		{
      color_line = BLACK;
      color_base = RED;
      color_depo = BLUE;
      SysColorSensorRead(csr);
			if(csr.ColorValue == color_base)
			{
        TextOut(5,LCD_LINE5,"GET ITEM ...");
				TextOut(5,LCD_LINE1,"Base Detected ...");
				Off(MOTORS);
				//Wait(SEC_2);

				if(check_rfid() == LET_OBJECT)
				{
					TextOut(5,LCD_LINE2,"RFID Let Detected ...");
					Wait(SEC_3);
					state = GOTOBASEGET;
				}
				else
				{
					TextOut(5,LCD_LINE2,"RFID Unuseful Detected ...");
					Wait(SEC_3);
					rouler();
				}
      }
			else if(csr.ColorValue == color_line)
			{
				TextOut(5,LCD_LINE1,"Following Line ...");
				rouler();
			}
      else
      {
				// Retrouver ligne
				find_back_line_2();
				Off(MOTORS);
				TextOut(5,LCD_LINE1,"Looking for line ...");
			}
			
		}
		else if(state == GOTOBASEGET)
		{
         TextOut(5,LCD_LINE5,"GO TO BASE ...");
         color_line = RED;
         color_base = BLACK;
         color_depo = BLUE;
         SysColorSensorRead(csr);
    		 if(csr.ColorValue == color_depo)
         {
				     // Action colis
				     Off(MOTORS);
				     TextOut(5,LCD_LINE1,"Getting Object ...");

             while(not(BTReceiveMessage(BT_CONN, MAILBOX, TRUE) == "1"))
             {
                      BTSendMessage(BT_CONN, MAILBOX, "1");// Do_get
                      Wait(100); // Overload protector
             }
             Wait(SEC_2);
             TextOut(5,LCD_LINE5,"U Turn ...");
             OnFwdSync(OUT_AB,70,60);
             Wait(2500);
             Off(MOTORS);
             state = GETOUTBASETOLET;
		     }
		     else if(csr.ColorValue == color_line)
         {
				    TextOut(5,LCD_LINE1,"Following Line ...");
				    rouler();
			   }
         else
         {
				    // Retrouver ligne
				    find_back_line_1();
				    Off(MOTORS);
				    TextOut(5,LCD_LINE1,"Looking for line ...");
		     }
    }
    else if(state == GETOUTBASETOLET)
    {
         // To work
         TextOut(5,LCD_LINE5,"GET OUT BASE ...");
         color_line = RED;
         color_base = BLUE;
         color_depo = BLACK;
         SysColorSensorRead(csr);
    		 if(csr.ColorValue == color_depo)
         {
             //Wait(SEC_2);
             TextOut(5,LCD_LINE1,"Let object ...");
             state = LET_OBJECT;
         }
		     else if(csr.ColorValue == color_line)
         {
				    TextOut(5,LCD_LINE1,"Following Line ...");
				    rouler();
			   }
         else
         {
				    // Retrouver ligne
				    find_back_line_1();
				    Off(MOTORS);
				    TextOut(5,LCD_LINE1,"Looking for line ...");
		     }
    }
		else if(state == LET_OBJECT)
		{
         TextOut(5,LCD_LINE5,"LET OBJECT            ");
         color_line = BLACK;
         color_base = RED;
         color_depo = BLUE;
         SysColorSensorRead(csr);
			   if(csr.ColorValue == color_base)
			   {
              TextOut(5,LCD_LINE5,"LET ITEM ...");
				      TextOut(5,LCD_LINE1,"Base Detected ...");
				      Off(MOTORS);
				      //Wait(SEC_2);

				      if(check_rfid() == GET_OBJECT)
				      {
					            TextOut(5,LCD_LINE2,"RFID Let Detected ...");
                      Wait(SEC_3);
					            state = GOTOBASELET;
              }
				      else
				      {
					            TextOut(5,LCD_LINE2,"RFID Unuseful Detected ...");
					            Wait(SEC_3);
					            rouler();
				      }
          }
			    else if(csr.ColorValue == color_line)
			    {
				       TextOut(5,LCD_LINE1,"Following Line ...");
				       rouler();
          }
          else
          {
				      // Retrouver ligne
				      find_back_line_2();
				      Off(MOTORS);
				      TextOut(5,LCD_LINE1,"Looking for line ...");
          }
		}
		else if(state == GOTOBASELET)
		{
         TextOut(5,LCD_LINE5,"GO TO BASE ...");
         color_line = RED;
         color_base = BLACK;
         color_depo = BLUE;
         SysColorSensorRead(csr);
    		 if(csr.ColorValue == color_depo)
         {
				     // Action colis
				     Off(MOTORS);
				     TextOut(5,LCD_LINE1,"Letting Object ...");

             while(not(BTReceiveMessage(BT_CONN, MAILBOX, TRUE) == "2"))// Do_get
             {
                      BTSendMessage(BT_CONN, MAILBOX, "2");
                      Wait(100); // Overload protector
             }
             Wait(SEC_2);
             TextOut(5,LCD_LINE5,"U Turn ...");
             OnFwd(MOTORS,50);
             Wait(SEC_1);
             Off(MOTORS);
             OnFwdSync(OUT_AB,70,60);
             Wait(2500);
             Off(MOTORS);
             state = GETOUTBASETOGET;
		     }
		     else if(csr.ColorValue == color_line)
         {
				    TextOut(5,LCD_LINE1,"Following Line ...");
				    rouler();
			   }
         else
         {
				    // Retrouver ligne
				    find_back_line_1();
				    Off(MOTORS);
				    TextOut(5,LCD_LINE1,"Looking for line ...");
		     }
    }
    else if(state == GETOUTBASETOGET)
    {
         // To work
         TextOut(5,LCD_LINE5,"GET OUT BASE ...");
         color_line = RED;
         color_base = BLUE;
         color_depo = BLACK;
         SysColorSensorRead(csr);
    		 if(csr.ColorValue == color_depo)
         {
             //Wait(SEC_2);
             TextOut(5,LCD_LINE1,"Let object ...");
             state = GET_OBJECT;
         }
		     else if(csr.ColorValue == color_line)
         {
				    TextOut(5,LCD_LINE1,"Following Line ...");
				    rouler();
			   }
         else
         {
				    // Retrouver ligne
				    find_back_line_1();
				    Off(MOTORS);
				    TextOut(5,LCD_LINE1,"Looking for line ...");
		     }
    }
		else
		{
			TextOut(5,LCD_LINE1,"Error !");
			Wait(SEC_5);
		}
	Wait(100); // Overload Protector
	}
}
</pre>