Code-source-robot-transporteur-integrations
De Wiki de bureau d'études PeiP
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 :
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)&¬(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);
}
