Code-source-robot-transporteur-integrations

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