Jwasilew : /* Code Source du maitre */

2013-05-12T20:34:55Z

‎Code Source du maitre

Jwasilew : /* Avant propos */

2013-05-12T19:59:15Z

‎Avant propos

Jwasilew : /* Code Source du maitre */

2013-05-12T19:58:44Z

‎Code Source du maitre

Jwasilew : /* Code Source de l'esclave */

2013-05-12T19:58:06Z

‎Code Source de l'esclave

Jwasilew : 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... »

2013-05-12T19:56:15Z

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... »

Nouvelle page

== 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>

← Version précédente		Version du 12 mai 2013 à 20:34
Ligne 195 :		Ligne 195 :

	// Color Definitions		// 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 BLACK INPUT_BLACKCOLOR

← Version précédente		Version du 12 mai 2013 à 19:59
Ligne 11 :		Ligne 11 :


−	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	+	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 ==		== Code Source de l'esclave ==

← Version précédente		Version du 12 mai 2013 à 19:58
Ligne 160 :		Ligne 160 :

	<pre>		<pre>
		+	/*
		+	Author : Jean Wasilewski & Deborah Saunders
		+	Creation : 02/02/2013
		+	File : motorsv3.nxc
		+	*/
		+
	// Includes		// Includes

← Version précédente		Version du 12 mai 2013 à 19:58
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>

Code-source-robot-transporteur - Historique des versions

Jwasilew : /* Code Source du maitre */

Jwasilew : /* Avant propos */

Jwasilew : /* Code Source du maitre */

Jwasilew : /* Code Source de l'esclave */

Jwasilew : 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... »