Code-source-robot-transporteur
De Wiki de bureau d'études PeiP
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 :
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 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
/*
Author : Jean Wasilewski & Deborah Saunders
Creation : 02/02/2013
File : motorsv3.nxc
*/
// 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 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
}
}
