Code-source-robot-transporteur : Différence entre versions

De Wiki de bureau d'études PeiP
(Page créée avec « == 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 deu... »)
 
(→‎Code Source de l'esclave)
Ligne 16 : Ligne 16 :
  
 
<pre>
 
<pre>
 +
/*
 +
Author    : Jean Wasilewski & Deborah Saunders
 +
Creation  : 02/02/2013
 +
File : handv3.nxc
 +
*/
  
 +
// Includes
 +
 +
#include "NXCDefs.h"
 +
#include "BTlib.nxc"
 +
#include "RFIDlib.nxc"
 +
#include "NBCCommon.h"
 +
 +
// Output Definitions
 +
 +
#define MOTOR_PINCE OUT_A
 +
#define MOTOR_LEVAGE OUT_B
 +
 +
// BlueTooth Definitions
 +
 +
#define BT_CONN 0
 +
#define MAILBOX 0
 +
 +
// Speed Definitions
 +
 +
#define SPEED_SLOW 50
 +
#define SPEED_FAST 100
 +
 +
// Receive Definitions
 +
 +
#define DO_NOTHING 0
 +
#define DO_GET  1
 +
#define DO_LET  2
 +
 +
// Receive Definitions
 +
 +
#define SEND_NOTHING "0"
 +
#define SEND_RUN    "1"
 +
#define SEND_LEAVE  "2"
 +
 +
// Global vars
 +
 +
string state = SEND_NOTHING;
 +
bool stahps = false;
 +
bool dejafait = false;
 +
bool got_object = false;
 +
 +
bool not(bool in)
 +
{
 +
    if(in)
 +
    {
 +
        return false;
 +
    }
 +
    else
 +
    {
 +
        return true;
 +
    }
 +
}
 +
 +
// Master BlueTooth Check
 +
 +
sub masterBTCheck(int conn){
 +
  string cStr;
 +
  cStr = NumToStr(conn);
 +
  cStr = StrCat("on line",cStr);
 +
  if (!BTCommCheck(conn)){
 +
      TextOut(5,LCD_LINE2,"*Connect Slave");
 +
      TextOut(5,LCD_LINE3,cStr);
 +
      TextOut(60,LCD_LINE3,"from");
 +
      TextOut(5,LCD_LINE4,"this NXT menu");
 +
      Wait(7000);
 +
      Stop(true);
 +
  }
 +
}
 +
 +
void deposer_colis()
 +
{
 +
    if(dejafait)
 +
    {
 +
        RotateMotor(MOTOR_LEVAGE, 65, 540);
 +
        //Wait(SEC_1);
 +
        OnFwd(MOTOR_PINCE, 45);
 +
        Wait(SEC_1);
 +
        Off(MOTOR_PINCE);
 +
        dejafait = false;
 +
        state = SEND_LEAVE;
 +
    }
 +
 +
}
 +
 +
void recuperer_colis()
 +
{
 +
    //RotateMotor(MOTOR_PINCE, 85, -180);
 +
    OnFwd(MOTOR_PINCE, -45);
 +
    Wait(SEC_2);
 +
    if(!dejafait)
 +
    {
 +
        RotateMotor(MOTOR_LEVAGE, 65, -540);
 +
        dejafait = true;
 +
    }
 +
    Wait(SEC_1);
 +
    state = SEND_RUN;
 +
}
 +
 +
// Main Task
 +
 +
task main()
 +
{
 +
    int in;
 +
 +
    masterBTCheck(BT_CONN);
 +
 +
    while(true)
 +
    {
 +
                  ClearScreen();
 +
                  in = StrToNum(BTReceiveMessage(BT_CONN, MAILBOX, TRUE));
 +
                  if(in == DO_NOTHING)
 +
                  {
 +
                        TextOut(5,LCD_LINE1,"NOTHING COMMAND");
 +
                  }
 +
                  else if(in == DO_GET)
 +
                  {
 +
                      recuperer_colis();
 +
                      TextOut(5,LCD_LINE1,"GET COMMAND");
 +
                  }
 +
                  else if(in == DO_LET)
 +
                  {
 +
                      deposer_colis();
 +
                      TextOut(5,LCD_LINE1,"LET COMMAND");
 +
                  }
 +
                  else
 +
                  {
 +
                      TextOut(5,LCD_LINE1,"BT Command unrecognized");
 +
                  }
 +
    BTSendMessage(BT_CONN, MAILBOX, state);
 +
    TextOut(5, LCD_LINE7, NumToStr(in));
 +
    Wait(100); // Overload Protector
 +
    }
 +
}
 
</pre>
 
</pre>
  

Version du 12 mai 2013 à 19:58

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 :


Communication.jpg


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

/*
Author    : Jean Wasilewski & Deborah Saunders
Creation  : 02/02/2013
File : handv3.nxc
*/

// Includes

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

// Output Definitions

#define MOTOR_PINCE OUT_A
#define MOTOR_LEVAGE OUT_B

// BlueTooth Definitions

#define BT_CONN 0
#define MAILBOX 0

// Speed Definitions

#define SPEED_SLOW 50
#define SPEED_FAST 100

// Receive Definitions

#define DO_NOTHING 0
#define DO_GET   1
#define DO_LET   2

// Receive Definitions

#define SEND_NOTHING "0"
#define SEND_RUN     "1"
#define SEND_LEAVE   "2"

// Global vars

string state = SEND_NOTHING;
bool stahps = false;
bool dejafait = false;
bool got_object = false;

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

// Master BlueTooth Check

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

void deposer_colis()
{
     if(dejafait)
     {
         RotateMotor(MOTOR_LEVAGE, 65, 540);
         //Wait(SEC_1);
         OnFwd(MOTOR_PINCE, 45);
         Wait(SEC_1);
         Off(MOTOR_PINCE);
         dejafait = false;
         state = SEND_LEAVE;
     }

}

void recuperer_colis()
{
     //RotateMotor(MOTOR_PINCE, 85, -180);
     OnFwd(MOTOR_PINCE, -45);
     Wait(SEC_2);
     if(!dejafait)
     {
         RotateMotor(MOTOR_LEVAGE, 65, -540);
         dejafait = true;
     }
     Wait(SEC_1);
     state = SEND_RUN;
}

// Main Task

task main()
{
     int in;

     masterBTCheck(BT_CONN);

     while(true)
     {
                  ClearScreen();
                  in = StrToNum(BTReceiveMessage(BT_CONN, MAILBOX, TRUE));
                  if(in == DO_NOTHING)
                  {
                        TextOut(5,LCD_LINE1,"NOTHING COMMAND");
                  }
                  else if(in == DO_GET)
                  {
                       recuperer_colis();
                       TextOut(5,LCD_LINE1,"GET COMMAND");
                  }
                  else if(in == DO_LET)
                  {
                       deposer_colis();
                       TextOut(5,LCD_LINE1,"LET COMMAND");
                  }
                  else
                  {
                      TextOut(5,LCD_LINE1,"BT Command unrecognized");
                  }
     BTSendMessage(BT_CONN, MAILBOX, state);
     TextOut(5, LCD_LINE7, NumToStr(in));
     Wait(100); // Overload Protector
     }
}

Code Source du maitre

// 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
	}
}
Récupérée de « https://peip-ima.plil.fr/mediawiki/index.php?title=Code-source-robot-transporteur&oldid=1889 »