Code-source-robot-transporteur-integrations : 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, avec toutes les intégrations, programmé par Déborah Saunders et Jean Wasilew... »)
 
(→‎Code Source du maitre)
 
Ligne 160 : Ligne 160 :
  
 
<pre>
 
<pre>
 +
// Includes
 +
 +
#include "NXCDefs.h"
 +
#include "BTlib.nxc"
 +
#include "RFIDlib.nxc"
 +
#include "NBCCommon.h"
 +
 +
// Input Definitions
 +
 +
#define SENS_LIGHT  IN_1
 +
#define SENS_RFID  IN_2
 +
#define SENS_COMPA IN_3
 +
#define SENS_ULTRA IN_4
 +
 +
// Output Definitions
 +
 +
#define MOTOR_LEFT  OUT_A
 +
#define MOTOR_RIGHT  OUT_B
 +
#define MOTORS      OUT_AB
 +
 +
// BlueTooth Definitions
 +
 +
#define BT_CONN 1
 +
#define BT_SERV 0
 +
#define MAILBOX 0
 +
#define MAILPOS 9
 +
 +
// Speed Definitions
 +
 +
//#define SPEED_SLOW -50
 +
//#define SPEED_FAST -80
 +
#define SPEED_SLOW -20
 +
#define SPEED_FAST -45
 +
 +
 +
// Card Definitions
 +
 +
#define CARTE1 4
 +
#define CARTE2 253
 +
#define CARTE3 221
 +
#define CARTE4 128
 +
#define CARTE5 47
 +
#define CARTE6 187
 +
#define CARTE7 66
 +
#define CARTE8 68
 +
 +
/*
 +
#define CARTE1 78
 +
#define CARTE2 82
 +
#define CARTE3 111
 +
#define CARTE4 145
 +
#define CARTE5 31
 +
#define CARTE6 157
 +
#define CARTE7 5
 +
#define CARTE8 105
 +
*/
 +
 +
// Compass Definitions
 +
 +
#define COMPASS1 152
 +
#define COMPASS3 121
 +
#define COMPASS4 (360 - COMPASS3)
 +
#define COMPASS8 (360 - COMPASS8)
 +
 +
// Color Definitions
 +
 +
#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  NOCOLOR   -1
 +
 +
// Headers
 +
 +
task get_object();
 +
task let_object();
 +
task follow_line();
 +
task t_rfid();
 +
task compass_run();
 +
 +
// Globals
 +
 +
int color_main;
 +
int color_other;
 +
bool got_object;
 +
int read;
 +
int color_read;
 +
int last_rfid;
 +
bool blind_run;
 +
int cap;
 +
int e;
 +
 +
string s_rfid;
 +
string s_line;
 +
string s_comp1;
 +
string s_comp2;
 +
 +
 +
int color_line = GREEN;
 +
int color_base = YELLOW;
 +
int color_depo = BLUE;
 +
 +
// Not Function
 +
 +
bool not(bool in)
 +
{
 +
    if(in)
 +
    {
 +
        return false;
 +
    }
 +
    else
 +
    {
 +
        return true;
 +
    }
 +
}
 +
 +
// 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);
 +
    RFIDDataToString(a,data);
 +
return a[3];
 +
}
 +
 +
// BT Initialization Procedure
 +
 +
sub BTCheck(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);
 +
}
 +
 +
// Find Back Line global
 +
 +
int rl;
 +
 +
 +
// Find Back Line Function
 +
 +
void find_back_line()
 +
{
 +
    bool line_found = false;
 +
    int initClock = 0;
 +
    int clock = 0;
 +
    int multi = 100;
 +
    int cpt = 0;
 +
   
 +
          while ((!line_found)&&(cpt<=5))
 +
    {
 +
   
 +
   
 +
          OnFwd(MOTORS, rl * SPEED_FAST);
 +
          SysColorSensorRead(csr);
 +
          clock = clock + 1;
 +
          if(csr.ColorValue == GREEN)
 +
          {
 +
                  line_found = true;
 +
          }
 +
          if(clock<multi)
 +
          {
 +
                  rl = -1;
 +
          }
 +
          else if(clock<=(multi*2))
 +
          {
 +
                  //rl = rl * -1;
 +
                  rl = 1;
 +
          }
 +
          else
 +
          {
 +
              multi=multi*4;
 +
              cpt = cpt +1;
 +
          }
 +
    }
 +
   
 +
    //Off(MOTORS);
 +
}
 +
 +
// Initialization Function
 +
 +
int init()
 +
{
 +
BTCheck(BT_CONN);
 +
BTCheck(BT_SERV);
 +
init_colorreader();
 +
init_rfid();
 +
read = -1;
 +
color_read = -1;
 +
rl = -1;
 +
color_main = GREEN;
 +
color_other = NOCOLOR;
 +
got_object = false;
 +
last_rfid = -1;
 +
SetSensorLowspeed(SENS_COMPA);
 +
blind_run = false;
 +
cap = -1;
 +
e = 8;
 +
SetSensorLowspeed(SENS_ULTRA);
 +
}
 +
 +
// SafeQuit_FollowLine Function
 +
 +
int SafeQuit_FollowLine()
 +
{
 +
s_line = "                                      ";
 +
TextOut(5,LCD_LINE2,s_line);
 +
Off(MOTORS);
 +
}
 +
 +
// SafeQuit_GetObject Function
 +
 +
int SafeQuit_GetObject()
 +
{
 +
s_line = "                                      ";
 +
TextOut(5,LCD_LINE3,s_line);
 +
Off(MOTORS);
 +
StopTask(get_object);
 +
}
 +
 +
// SafeQuit_LetObject Function
 +
 +
int SafeQuit_LetObject()
 +
{
 +
s_line = "                                      ";
 +
TextOut(5,LCD_LINE3,s_line);
 +
Off(MOTORS);
 +
StopTask(let_object);
 +
}
 +
 +
// SafeQuit_CompassRun Function
 +
 +
int SafeQuit_CompassRun()
 +
{
 +
s_line = "                                      ";
 +
TextOut(5,LCD_LINE3,s_line);
 +
TextOut(5,LCD_LINE4,s_line);
 +
Off(MOTORS);
 +
StopTask(compass_run);
 +
}
 +
 +
 +
// Compass Run Task
 +
// Output : 3 & 4
 +
 +
task compass_run()
 +
{
 +
bool line = false;
 +
int comp = -1;
 +
TextOut(5,LCD_LINE3,"co running                      ");
 +
rouler();
 +
Wait(MS_800);
 +
TextOut(5,LCD_LINE3,"co follow compass              ");
 +
while(not(line))
 +
{
 +
SysColorSensorRead(csr);
 +
if(csr.ColorValue == color_line)
 +
{
 +
Off(MOTORS);
 +
line = true;
 +
StartTask(follow_line);
 +
SafeQuit_CompassRun();
 +
}
 +
else
 +
{
 +
s_comp1 = NumToStr(color_read);
 +
s_comp1 = StrCat("c c:",s_comp1);
 +
s_comp1 = StrCat(s_comp1," l:");
 +
s_comp1 = StrCat(s_comp1,NumToStr(color_main));
 +
s_comp1 = StrCat(s_comp1, "                                      ");
 +
TextOut(5,LCD_LINE3,s_comp1);
 +
 +
comp = SensorHTCompass(IN_3);
 +
 +
s_comp2 = NumToStr(comp);
 +
s_comp2 = StrCat("co c:",s_comp2);
 +
s_comp2 = StrCat(s_comp2," w:");
 +
s_comp2 = StrCat(s_comp2,NumToStr(cap));
 +
s_comp2 = StrCat(s_comp2, "                                      ");
 +
TextOut(5,LCD_LINE4,s_comp2);
 +
 +
if((comp - e) > cap)
 +
{
 +
OnFwd(OUT_A, 35);
 +
OnFwd(OUT_B, -35);
 +
}
 +
else if((comp + e) < cap)
 +
{
 +
OnFwd(OUT_A, -35);
 +
OnFwd(OUT_B, 35);
 +
}
 +
else
 +
{
 +
OnFwd(OUT_A, 35);
 +
OnFwd(OUT_B, 35);
 +
}
 +
}
 +
}
 +
}
 +
 +
// Get Object Task
 +
// Ouput : 3
 +
 +
task get_object()
 +
{
 +
bool etape1 = false;
 +
bool etape2 = false;
 +
 +
Off(MOTORS);
 +
 +
while(!etape1)
 +
{
 +
TextOut(5,LCD_LINE3,"go e1                      ");
 +
color_line = YELLOW;
 +
color_base = GREEN;
 +
color_depo = BLUE;
 +
SysColorSensorRead(csr);
 +
if(csr.ColorValue == color_depo)
 +
{
 +
// Action colis
 +
Off(MOTORS);
 +
TextOut(5,LCD_LINE3,"go e1 BT ON          ");
 +
 +
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_LINE3,"go e1 U Turn            ");
 +
OnFwdSync(OUT_AB,70,60);
 +
Wait(2500);
 +
Off(MOTORS);
 +
got_object = true;
 +
etape1 = true;
 +
TextOut(5,LCD_LINE3,"go e1 done            ");
 +
}
 +
else if(csr.ColorValue == color_line)
 +
{
 +
TextOut(5,LCD_LINE3,"go e1 line ok              ");
 +
rouler();
 +
}
 +
else
 +
{
 +
TextOut(5,LCD_LINE3,"go e1 line search              ");
 +
bool line_found = false;
 +
int initClock = 0;
 +
int clock = 0;
 +
int multi = 100;
 +
int cpt = 0;
 +
            while ((!line_found)&&(cpt<=5))
 +
{
 +
OnFwd(MOTORS, rl * SPEED_FAST);
 +
SysColorSensorRead(csr);
 +
clock = clock + 1;
 +
if((csr.ColorValue == color_line)||(csr.ColorValue == color_depo))
 +
{
 +
line_found = true;
 +
}
 +
if(clock<multi)
 +
{
 +
rl = -1;
 +
}
 +
else if(clock<=(multi*2))
 +
{
 +
//rl = rl * -1;
 +
rl = 1;
 +
}
 +
else
 +
{
 +
multi=multi*4;
 +
cpt = cpt +1;
 +
}
 +
}
 +
Off(MOTORS);
 +
}
 +
    }
 +
 +
    while(!etape2)
 +
    {
 +
        TextOut(5,LCD_LINE3,"go e2                            ");
 +
        color_line = YELLOW;
 +
        color_base = BLUE;
 +
        color_depo = GREEN;
 +
        SysColorSensorRead(csr);
 +
    if(csr.ColorValue == color_depo)
 +
        {
 +
            //Wait(SEC_2);
 +
Off(MOTORS);
 +
            etape2 = true;
 +
TextOut(5,LCD_LINE3,"go e2 done                      ");
 +
StartTask(follow_line);
 +
SafeQuit_GetObject();
 +
        }
 +
else if(csr.ColorValue == color_line)
 +
        {
 +
    TextOut(5,LCD_LINE3,"go e2 line ok              ");
 +
    rouler();
 +
}
 +
        else
 +
        {
 +
TextOut(5,LCD_LINE3,"go e2 line search              ");
 +
bool line_found = false;
 +
int initClock = 0;
 +
int clock = 0;
 +
int multi = 100;
 +
int cpt = 0;
 +
while ((!line_found)&&(cpt<=5))
 +
{
 +
OnFwd(MOTORS, rl * SPEED_FAST);
 +
SysColorSensorRead(csr);
 +
clock = clock + 1;
 +
if((csr.ColorValue == color_line)||(csr.ColorValue == color_depo))
 +
{
 +
line_found = true;
 +
}
 +
if(clock<multi)
 +
{
 +
rl = -1;
 +
}
 +
else if(clock<=(multi*2))
 +
{
 +
//rl = rl * -1;
 +
rl = 1;
 +
}
 +
else
 +
{
 +
multi=multi*4;
 +
cpt = cpt +1;
 +
}
 +
}
 +
Off(MOTORS);
 +
}
 +
    }
 +
}
 +
 +
// Let Object Task
 +
// Output : 3
 +
 +
task let_object()
 +
{
 +
bool etape1 = false;
 +
bool etape2 = false;
 +
 +
while(!etape1)
 +
{
 +
TextOut(5,LCD_LINE3,"go e1                      ");
 +
        color_line = YELLOW;
 +
        color_base = GREEN;
 +
        color_depo = BLUE;
 +
        SysColorSensorRead(csr);
 +
    if(csr.ColorValue == color_depo)
 +
        {
 +
    // Action colis
 +
    Off(MOTORS);
 +
    TextOut(5,LCD_LINE3,"go e1 BT ON          ");
 +
 +
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_LINE3,"go e1 U Turn            ");
 +
            OnFwd(MOTORS,50);
 +
            Wait(SEC_1);
 +
            Off(MOTORS);
 +
            OnFwdSync(OUT_AB,70,60);
 +
            Wait(2500);
 +
            Off(MOTORS);
 +
got_object = false;
 +
            etape1 = true;
 +
TextOut(5,LCD_LINE3,"go e1 done            ");
 +
}
 +
else if(csr.ColorValue == color_line)
 +
        {
 +
TextOut(5,LCD_LINE3,"go e1 line ok              ");
 +
rouler();
 +
}
 +
        else
 +
        {
 +
    // Retrouver ligne
 +
    TextOut(5,LCD_LINE3,"go e1 line search              ");
 +
bool line_found = false;
 +
int initClock = 0;
 +
int clock = 0;
 +
int multi = 100;
 +
int cpt = 0;
 +
            while ((!line_found)&&(cpt<=5))
 +
{
 +
OnFwd(MOTORS, rl * SPEED_FAST);
 +
SysColorSensorRead(csr);
 +
clock = clock + 1;
 +
if((csr.ColorValue == color_line)||(csr.ColorValue == color_depo))
 +
{
 +
line_found = true;
 +
}
 +
if(clock<multi)
 +
{
 +
rl = -1;
 +
}
 +
else if(clock<=(multi*2))
 +
{
 +
//rl = rl * -1;
 +
rl = 1;
 +
}
 +
else
 +
{
 +
multi=multi*4;
 +
cpt = cpt +1;
 +
}
 +
}
 +
TextOut(5,LCD_LINE1,"Looking for line ...");
 +
}
 +
}
 +
 +
while(!etape2)
 +
{
 +
        TextOut(5,LCD_LINE3,"go e2                            ");
 +
        color_line = YELLOW;
 +
        color_base = BLUE;
 +
        color_depo = GREEN;
 +
        SysColorSensorRead(csr);
 +
    if(csr.ColorValue == color_depo)
 +
        {
 +
            //Wait(SEC_2);
 +
Off(MOTORS);
 +
            etape2 = true;
 +
TextOut(5,LCD_LINE3,"go e2 done                      ");
 +
StartTask(follow_line);
 +
SafeQuit_LetObject();
 +
        }
 +
else if(csr.ColorValue == color_line)
 +
        {
 +
    TextOut(5,LCD_LINE3,"go e2 line ok              ");
 +
    rouler();
 +
}
 +
        else
 +
        {
 +
TextOut(5,LCD_LINE3,"go e2 line search              ");
 +
bool line_found = false;
 +
int initClock = 0;
 +
int clock = 0;
 +
int multi = 100;
 +
int cpt = 0;
 +
while ((!line_found)&&(cpt<=5))
 +
{
 +
OnFwd(MOTORS, rl * SPEED_FAST);
 +
SysColorSensorRead(csr);
 +
clock = clock + 1;
 +
if((csr.ColorValue == color_line)||(csr.ColorValue == color_depo))
 +
{
 +
line_found = true;
 +
}
 +
if(clock<multi)
 +
{
 +
rl = -1;
 +
}
 +
else if(clock<=(multi*2))
 +
{
 +
//rl = rl * -1;
 +
rl = 1;
 +
}
 +
else
 +
{
 +
multi=multi*4;
 +
cpt = cpt +1;
 +
}
 +
}
 +
Off(MOTORS);
 +
}
 +
}
 +
}
 +
 +
// Follow Line Task
 +
// Ouput : 2
 +
 +
task follow_line()
 +
{
 +
while(true)
 +
{
 +
SysColorSensorRead(csr);
 +
color_read = csr.ColorValue;
 +
if(color_read == color_main)
 +
{
 +
s_line = NumToStr(color_read);
 +
s_line = StrCat("c c:",s_line);
 +
s_line = StrCat(s_line," l:");
 +
s_line = StrCat(s_line,NumToStr(color_main));
 +
s_line = StrCat(s_line, "                                      ");
 +
TextOut(5,LCD_LINE2,s_line);
 +
if(SensorUS(SENS_ULTRA) > 20)
 +
{
 +
rouler();
 +
}
 +
else
 +
{
 +
Off(MOTORS);
 +
}
 +
}
 +
else
 +
{
 +
s_line = NumToStr(color_read);
 +
s_line = StrCat("c c:",s_line);
 +
s_line = StrCat(s_line," l:");
 +
s_line = StrCat(s_line,NumToStr(color_main));
 +
s_line = StrCat(s_line, "                                      ");
 +
TextOut(5,LCD_LINE2,s_line);
 +
find_back_line();
 +
}
 +
}
 +
}
 +
 +
// RFID Task
 +
// Output : 1
 +
 +
task t_rfid()
 +
{
 +
while(true)
 +
{
 +
read = check_rfid();
 +
if(not(read == 0)&&not(read == last_rfid))
 +
{
 +
last_rfid = read;
 +
}
 +
 +
// Then  check cards
 +
 +
if(read == CARTE1)
 +
{
 +
PlayTone(440, 500);
 +
if(not(blind_run))
 +
{
 +
StopTask(follow_line);
 +
SafeQuit_FollowLine();
 +
cap = COMPASS1;
 +
StartTask(compass_run);
 +
}
 +
}
 +
else if(read == CARTE2)
 +
{
 +
PlayTone(440, 500);
 +
 +
if(not(got_object))
 +
{
 +
StopTask(follow_line);
 +
SafeQuit_FollowLine();
 +
StartTask(get_object);
 +
}
 +
}
 +
else if(read == CARTE3)
 +
{
 +
// Discontinue
 +
}
 +
else if(read == CARTE4)
 +
{
 +
// Discontinue
 +
}
 +
else if(read == CARTE5)
 +
{
 +
// Checkpoint
 +
}
 +
else if(read == CARTE6)
 +
{
 +
PlayTone(440, 500);
 +
if(got_object)
 +
{
 +
StopTask(follow_line);
 +
SafeQuit_FollowLine();
 +
StartTask(let_object);
 +
}
 +
}
 +
else if(read == CARTE7)
 +
{
 +
//PlayTone(440, 500);
 +
// Checkpoint
 +
}
 +
else if(read == CARTE8)
 +
{
 +
// Discontinue
 +
}
 +
 +
s_rfid = NumToStr(read);
 +
s_rfid = StrCat("r c:",s_rfid);
 +
s_rfid = StrCat(s_rfid, " l:");
 +
s_rfid = StrCat(s_rfid, NumToStr(last_rfid));
 +
s_rfid = StrCat(s_rfid, "                                      ");
 +
TextOut(5,LCD_LINE1,s_rfid);
 +
 +
BTSendMessage(BT_SERV, MAILPOS, NumToStr(last_rfid));
 +
}
 +
}
 +
 +
// Main Task
 +
 +
task main()
 +
{
 +
init();
 +
Precedes(follow_line, t_rfid);
 +
}
 
</pre>
 
</pre>

Version actuelle datée du 12 mai 2013 à 20:36

Avant propos

Vous êtes sur la page contenant le code source du robot transporteur NXT, avec toutes les intégrations, 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 :


Communication2.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. La boussole se branche sur l'entré 3, et le capteur ultrasons sur la 4. Le moteur de levage se branche sur l'entrée B, et la pince quant à elle se branche sur le connecteur A

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_LIGHT  IN_1
#define SENS_RFID   IN_2
#define SENS_COMPA	IN_3
#define SENS_ULTRA	IN_4

// Output Definitions

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

// BlueTooth Definitions

#define BT_CONN 1
#define BT_SERV 0
#define MAILBOX 0
#define MAILPOS 9

// Speed Definitions

//#define SPEED_SLOW -50
//#define SPEED_FAST -80
#define SPEED_SLOW -20
#define SPEED_FAST -45


// Card Definitions

#define	CARTE1	4
#define	CARTE2	253
#define	CARTE3	221
#define	CARTE4	128
#define	CARTE5	47
#define	CARTE6	187
#define	CARTE7	66
#define	CARTE8	68

/*
#define	CARTE1	78
#define	CARTE2	82
#define	CARTE3	111
#define	CARTE4	145
#define	CARTE5	31
#define	CARTE6	157
#define	CARTE7	5
#define	CARTE8	105
*/

// Compass Definitions

#define COMPASS1 152
#define COMPASS3 121
#define COMPASS4 (360 - COMPASS3)
#define COMPASS8 (360 - COMPASS8)

// Color Definitions

#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  NOCOLOR		  -1

// Headers

task get_object();
task let_object();
task follow_line();
task t_rfid();
task compass_run();

// Globals

int color_main;
int color_other;
bool got_object;
int read;
int color_read;
int last_rfid;
bool blind_run;
int cap;
int e;

string s_rfid;
string s_line;
string s_comp1;
string s_comp2;


int color_line = GREEN;
int color_base = YELLOW;
int color_depo = BLUE;

// Not Function

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

// 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);
    RFIDDataToString(a,data);
	return a[3];
}

// BT Initialization Procedure

sub BTCheck(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);
}

// Find Back Line global

int rl;


// Find Back Line Function

void find_back_line()
{
     bool line_found = false;
     int initClock = 0;
     int clock = 0;
     int multi = 100;
     int cpt = 0;
     
           while ((!line_found)&&(cpt<=5))
     {
     
     
           OnFwd(MOTORS, rl * SPEED_FAST);
           SysColorSensorRead(csr);
           clock = clock + 1;
           if(csr.ColorValue == GREEN)
           {
                  line_found = true;
           }
           if(clock<multi)
           {
                  rl = -1;
           }
           else if(clock<=(multi*2))
           {
                  //rl = rl * -1;
                  rl = 1;
           }
           else
           {
               multi=multi*4;
               cpt = cpt +1;
           }
     }
     
     //Off(MOTORS);
}

// Initialization Function

int init()
{
	BTCheck(BT_CONN);
	BTCheck(BT_SERV);
	init_colorreader();
	init_rfid();
	read = -1;
	color_read = -1;
	rl = -1;
	color_main = GREEN;
	color_other = NOCOLOR;
	got_object = false;
	last_rfid = -1;
	SetSensorLowspeed(SENS_COMPA);
	blind_run = false;
	cap = -1;
	e = 8;
	SetSensorLowspeed(SENS_ULTRA);
}

// SafeQuit_FollowLine Function

int SafeQuit_FollowLine()
{
	s_line = "                                      ";
	TextOut(5,LCD_LINE2,s_line);
	Off(MOTORS);
}

// SafeQuit_GetObject Function

int SafeQuit_GetObject()
{
	s_line = "                                      ";
	TextOut(5,LCD_LINE3,s_line);
	Off(MOTORS);
	StopTask(get_object);
}

// SafeQuit_LetObject Function

int SafeQuit_LetObject()
{
	s_line = "                                      ";
	TextOut(5,LCD_LINE3,s_line);
	Off(MOTORS);
	StopTask(let_object);
}

// SafeQuit_CompassRun Function

int SafeQuit_CompassRun()
{
	s_line = "                                      ";
	TextOut(5,LCD_LINE3,s_line);
	TextOut(5,LCD_LINE4,s_line);
	Off(MOTORS);
	StopTask(compass_run);
}


// Compass Run Task
// Output : 3 & 4

task compass_run()
{
	bool line = false;
	int comp = -1;
	TextOut(5,LCD_LINE3,"co running                      ");
	rouler();
	Wait(MS_800);
	TextOut(5,LCD_LINE3,"co follow compass               ");
	while(not(line))
	{
		SysColorSensorRead(csr);
		if(csr.ColorValue == color_line)
		{
			Off(MOTORS);
			line = true;
			StartTask(follow_line);
			SafeQuit_CompassRun();			
		}
		else
		{
			s_comp1 = NumToStr(color_read);
			s_comp1 = StrCat("c c:",s_comp1);
			s_comp1 = StrCat(s_comp1," l:");
			s_comp1 = StrCat(s_comp1,NumToStr(color_main));
			s_comp1 = StrCat(s_comp1, "                                      ");
			TextOut(5,LCD_LINE3,s_comp1);
			
			comp = SensorHTCompass(IN_3);
			
			s_comp2 = NumToStr(comp);
			s_comp2 = StrCat("co c:",s_comp2);
			s_comp2 = StrCat(s_comp2," w:");
			s_comp2 = StrCat(s_comp2,NumToStr(cap));
			s_comp2 = StrCat(s_comp2, "                                      ");
			TextOut(5,LCD_LINE4,s_comp2);
			
			if((comp - e) > cap)
			{
				OnFwd(OUT_A, 35);
				OnFwd(OUT_B, -35);
			}
			else if((comp + e) < cap)
			{
				OnFwd(OUT_A, -35);
				OnFwd(OUT_B, 35);
			}
			else
			{
				OnFwd(OUT_A, 35);
				OnFwd(OUT_B, 35);
			}
		}
	}
}

// Get Object Task
// Ouput : 3

task get_object()
{
	bool etape1 = false;
	bool etape2 = false;
	
	Off(MOTORS);
	
	while(!etape1)
	{
		TextOut(5,LCD_LINE3,"go e1                      ");
		color_line = YELLOW;
		color_base = GREEN;
		color_depo = BLUE;
		SysColorSensorRead(csr);
		if(csr.ColorValue == color_depo)
		{
			// Action colis
			Off(MOTORS);
			TextOut(5,LCD_LINE3,"go e1 BT ON           ");
	
			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_LINE3,"go e1 U Turn            ");
			OnFwdSync(OUT_AB,70,60);
			Wait(2500);
			Off(MOTORS);
			got_object = true;
			etape1 = true;
			TextOut(5,LCD_LINE3,"go e1 done             ");
		}
		else if(csr.ColorValue == color_line)
		{
			TextOut(5,LCD_LINE3,"go e1 line ok              ");
			rouler();
		}
		else
		{
			TextOut(5,LCD_LINE3,"go e1 line search              ");
			bool line_found = false;
			int initClock = 0;
			int clock = 0;
			int multi = 100;
			int cpt = 0;
            while ((!line_found)&&(cpt<=5))
			{
				OnFwd(MOTORS, rl * SPEED_FAST);
				SysColorSensorRead(csr);
				clock = clock + 1;
				if((csr.ColorValue == color_line)||(csr.ColorValue == color_depo))
				{
					line_found = true;
				}
				if(clock<multi)
				{
					rl = -1;
				}
				else if(clock<=(multi*2))
				{
					//rl = rl * -1;
					rl = 1;
				}
				else
				{
					multi=multi*4;
					cpt = cpt +1;
				}
			}
			Off(MOTORS);
		}
    }
	
    while(!etape2)
    {
        TextOut(5,LCD_LINE3,"go e2                            ");
        color_line = YELLOW;
        color_base = BLUE;
        color_depo = GREEN;
        SysColorSensorRead(csr);
    	if(csr.ColorValue == color_depo)
        {
            //Wait(SEC_2);
			Off(MOTORS);
            etape2 = true;
			TextOut(5,LCD_LINE3,"go e2 done                       ");
			StartTask(follow_line);
			SafeQuit_GetObject();
        }
		else if(csr.ColorValue == color_line)
        {
		    TextOut(5,LCD_LINE3,"go e2 line ok              ");
		    rouler();
		}
        else
        {
			TextOut(5,LCD_LINE3,"go e2 line search              ");
			bool line_found = false;
			int initClock = 0;
			int clock = 0;
			int multi = 100;
			int cpt = 0;
			while ((!line_found)&&(cpt<=5))
			{
				OnFwd(MOTORS, rl * SPEED_FAST);
				SysColorSensorRead(csr);
				clock = clock + 1;
				if((csr.ColorValue == color_line)||(csr.ColorValue == color_depo))
				{
					line_found = true;
				}
				if(clock<multi)
				{
					rl = -1;
				}
				else if(clock<=(multi*2))
				{
					//rl = rl * -1;
					rl = 1;
				}
				else
				{
					multi=multi*4;
					cpt = cpt +1;
				}
			}
			Off(MOTORS);
		}
    }
}

// Let Object Task
// Output : 3

task let_object()
{
	bool etape1 = false;
	bool etape2 = false;
	
	while(!etape1)
	{
		TextOut(5,LCD_LINE3,"go e1                      ");
        color_line = YELLOW;
        color_base = GREEN;
        color_depo = BLUE;
        SysColorSensorRead(csr);
    	if(csr.ColorValue == color_depo)
        {
		    // Action colis
		    Off(MOTORS);
		    TextOut(5,LCD_LINE3,"go e1 BT ON           ");

			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_LINE3,"go e1 U Turn            ");
            OnFwd(MOTORS,50);
            Wait(SEC_1);
            Off(MOTORS);
            OnFwdSync(OUT_AB,70,60);
            Wait(2500);
            Off(MOTORS);
			got_object = false;
            etape1 = true;
			TextOut(5,LCD_LINE3,"go e1 done             ");
		}
		else if(csr.ColorValue == color_line)
        {
			TextOut(5,LCD_LINE3,"go e1 line ok              ");
			rouler();
		}
        else
        {
		    // Retrouver ligne
		    TextOut(5,LCD_LINE3,"go e1 line search              ");
			bool line_found = false;
			int initClock = 0;
			int clock = 0;
			int multi = 100;
			int cpt = 0;
            while ((!line_found)&&(cpt<=5))
			{
				OnFwd(MOTORS, rl * SPEED_FAST);
				SysColorSensorRead(csr);
				clock = clock + 1;
				if((csr.ColorValue == color_line)||(csr.ColorValue == color_depo))
				{
					line_found = true;
				}
				if(clock<multi)
				{
					rl = -1;
				}
				else if(clock<=(multi*2))
				{
					//rl = rl * -1;
					rl = 1;
				}
				else
				{
					multi=multi*4;
					cpt = cpt +1;
				}
			}
			TextOut(5,LCD_LINE1,"Looking for line ...");
		}
	}
	
	while(!etape2)
	{
        TextOut(5,LCD_LINE3,"go e2                            ");
        color_line = YELLOW;
        color_base = BLUE;
        color_depo = GREEN;
        SysColorSensorRead(csr);
    	if(csr.ColorValue == color_depo)
        {
            //Wait(SEC_2);
			Off(MOTORS);
            etape2 = true;
			TextOut(5,LCD_LINE3,"go e2 done                       ");
			StartTask(follow_line);
			SafeQuit_LetObject();
        }
		else if(csr.ColorValue == color_line)
        {
		    TextOut(5,LCD_LINE3,"go e2 line ok              ");
		    rouler();
		}
        else
        {
			TextOut(5,LCD_LINE3,"go e2 line search              ");
			bool line_found = false;
			int initClock = 0;
			int clock = 0;
			int multi = 100;
			int cpt = 0;
			while ((!line_found)&&(cpt<=5))
			{
				OnFwd(MOTORS, rl * SPEED_FAST);
				SysColorSensorRead(csr);
				clock = clock + 1;
				if((csr.ColorValue == color_line)||(csr.ColorValue == color_depo))
				{
					line_found = true;
				}
				if(clock<multi)
				{
					rl = -1;
				}
				else if(clock<=(multi*2))
				{
					//rl = rl * -1;
					rl = 1;
				}
				else
				{
					multi=multi*4;
					cpt = cpt +1;
				}
			}
			Off(MOTORS);
		}
	}
}

// Follow Line Task
// Ouput : 2

task follow_line()
{
	while(true)
	{
		SysColorSensorRead(csr);
		color_read = csr.ColorValue;
		if(color_read == color_main)
		{
			s_line = NumToStr(color_read);
			s_line = StrCat("c c:",s_line);
			s_line = StrCat(s_line," l:");
			s_line = StrCat(s_line,NumToStr(color_main));
			s_line = StrCat(s_line, "                                      ");
			TextOut(5,LCD_LINE2,s_line);
			if(SensorUS(SENS_ULTRA) > 20)
			{
				rouler();
			}
			else
			{
				Off(MOTORS);
			}
		}
		else
		{
			s_line = NumToStr(color_read);
			s_line = StrCat("c c:",s_line);
			s_line = StrCat(s_line," l:");
			s_line = StrCat(s_line,NumToStr(color_main));
			s_line = StrCat(s_line, "                                      ");
			TextOut(5,LCD_LINE2,s_line);
			find_back_line();
		}
	}
}

// RFID Task
// Output : 1

task t_rfid()
{
	while(true)
	{
		read = check_rfid();
		if(not(read == 0)&&not(read == last_rfid))
		{
			last_rfid = read;
		}
		
		// Then  check cards
		
		if(read == CARTE1)
		{
			PlayTone(440, 500);	
			if(not(blind_run))
			{
				StopTask(follow_line);
				SafeQuit_FollowLine();
				cap = COMPASS1;
				StartTask(compass_run);
			}
		}
		else if(read == CARTE2)
		{
			PlayTone(440, 500);
			
			if(not(got_object))
			{
				StopTask(follow_line);
				SafeQuit_FollowLine();
				StartTask(get_object);
			}
		}
		else if(read == CARTE3)
		{
			// Discontinue
		}
		else if(read == CARTE4)
		{
			// Discontinue
		}
		else if(read == CARTE5)
		{
			// Checkpoint
		}
		else if(read == CARTE6)
		{
			PlayTone(440, 500);			
			if(got_object)
			{
				StopTask(follow_line);
				SafeQuit_FollowLine();
				StartTask(let_object);
			}
		}
		else if(read == CARTE7)
		{
			//PlayTone(440, 500);
			// Checkpoint
		}
		else if(read == CARTE8)
		{
			// Discontinue
		}
		
		s_rfid = NumToStr(read);
		s_rfid = StrCat("r c:",s_rfid);
		s_rfid = StrCat(s_rfid, " l:");
		s_rfid = StrCat(s_rfid, NumToStr(last_rfid));
		s_rfid = StrCat(s_rfid, "                                      ");
		TextOut(5,LCD_LINE1,s_rfid);
		
		BTSendMessage(BT_SERV, MAILPOS, NumToStr(last_rfid));
	}
}

// Main Task

task main()
{
	init();
	Precedes(follow_line, t_rfid);
}
Récupérée de « https://peip-ima.plil.fr/mediawiki/index.php?title=Code-source-robot-transporteur-integrations&oldid=1903 »