레고 로봇으로 미로 탈출하기 (Escape the maze by LEGO NXT)

Notes

for Microsoft Visual Studio 2008 or 2010
Lego NXT software & fantom driver are REQUIRED.
DO NOT EXCUTE PROGRAM ON YOUR ROBOT. JUST USE IT FOR REFERENCE CODE.

Source Code

YSLegoExe.cpp (inclulde main function)

cpp

#include "LegoM.h"

using namespace std;


void Maze(LegoM* p);
void show() // show Test Menu 
{
   cout << "#############################" <<endl
   << "1.Maze " <<endl
   //<< "1.TouchSensor Test (Toggle+Response)" <<endl
   //<< "2.LightSensor Test (Response)" <<endl
   //<< "3.SoundSensor Test (Response)" <<endl
   << "4.UltrasonicSensor Test (Lowspeed)" <<endl
   << "5.Go Forward" <<endl
   << "6.Go Backward" <<endl
   << "7.Turn Left" <<endl
   << "8.Turn Right" <<endl
   << "9.Stop" <<endl
   << "0. Quit Program" <<endl
   << "#############################" <<endl;
   
}



int main(int argc, char* argv[])
{

 //Sleep(10000);
 LegoM* test = new LegoM(); 
 char menu;

 while(!test->Init()) // bluetooth connect
 {
  cout << "Initialize failed" << endl;
  cout << "Retry? (y/n) >>";
  cin >> menu;
  if(menu=='n') return 0;
  // Retry to connect
  //delete test;
  //test = new LegoM();
 }



 
 bool toggle = 0;
 int Response = 0;
 //show(); // show Test Menu
 do
 { 
  show();
  cout << "Command>>";   
  cin>>menu;
  if(menu<48 && menu>55) // IF input character is not number 
  {
   cout << "Wrong Input!" << endl;
   cout << "Command>>";
   cin>>menu; // Re-Input char
  }

  switch(menu)
  {
  case '1': // Maze
   Maze(test);
   test->Stop();
   //while(!kbhit()) { toggle = test->TouchToggle(toggle); }
   break;
   /*
  case '1': // Test Touch Sensor
   while(!kbhit()) { toggle = test->TouchToggle(toggle); }
   break;
  case '2':// Test Light Sensor
   while(!kbhit()) { Response = test->LightResponse(); }
   break;
  case '3':// Test Sound Sensor
   while(!kbhit()) { Response = test->SoundResponse(); }
   break;
   */
  case '4':// Test Ultrasonic Sensor
   while(!kbhit()) { 
    Response = test->UltrasonicResponse(); 
    Sleep(300);
   }
   break;
  case '5':// Test Go Foward
   //while(!kbhit()){ test->Move(20,0,20,0,MODE_MOTORON,REGULATION_MODE_MOTOR_SYNC,MOTOR_RUN_STATE_RUNNING,0);}
   while(!kbhit()) {
   test->GoForward();
   }
   test->Stop();
   // 00 04 02 20 01 02 00 20 00 00 00 00 00 00
   break;
  case '6':// Test Go Backward
   while(!kbhit()) {
   test->GoBackward();
   }
   test->Stop();
   break;
  case '7':// Test Turn Left
   //while(!kbhit()){ test->Move(55,80);}
   test->TurnLeft();
   break;
  case '8':// Test Turn Right
   //while(!kbhit()){ test->Move(80,55);}
   test->TurnRight();
   break;
  case '9':// Test Stop
   //while(!kbhit()){ test->Move(80,55);}
   test->Stop();
   break;
  default: break;
  }
  //show();
  //System("clr");
 } while(menu!='0');

 // bluetooth disconnect
 delete test;

 // program fin.
 return 0;
}

void Maze(LegoM* p)
{
 int r;bool t = false;
 while(!kbhit())
 {
  p->GoForward();
  
  while(!t) { 
   if(kbhit()) break;
   t = p->TouchToggle(t); 
  
  }
  if(kbhit()) break;

  p->Stop();
  p->Wait(50);
  p->GoBackward();
  p->Wait(300);
  p->Stop();
  p->Wait(200);
  
  r = p->UltrasonicResponse();
  if(r>=40) p->TurnRight();
  else p->TurnLeft();

  t=false;
 }

};

CommandConstant.h

cpp

#pragma once

#define SETINPUTMODE 1
#define GETINPUTVALUE 2
#define SETOUTPUTSTATE 3
#define LSWRITE 4
#define LSREAD 5

#define PORT_R_WHEEL 1
#define PORT_L_WHEEL 2

#define PORT_SOUND 1
#define PORT_LIGHT 2
#define PORT_TOUCH 0
#define PORT_ULTRASONIC 3

#define TYPE_NO_SENSOR 0x00
#define TYPE_SWITCH 0x01
#define TYPE_TEMPERATURE 0x02
#define TYPE_REFLECTION 0x03
#define TYPE_ANGLE 0x04
#define TYPE_LIGHT_ACTIVE 0x05
#define TYPE_LIGHT_INACTIVE 0x06
#define TYPE_SOUND_DB 0x07
#define TYPE_SOUND_DBA 0x08
#define TYPE_CUSTOM 0x09
#define TYPE_LOWSPEED 0x0A
#define TYPE_LOWSPEED_9V 0x0B
#define TYPE_NO_OF_SENSOR_TYPES 0x0C

#define MODE_RAWMODE 0x00
#define MODE_BOOLEANMODE 0x20
#define MODE_TRANSITIONCNTMODE 0x40
#define MODE_PERIODCOUNTERMODE 0x60
#define MODE_PCTFULLSCALEMODE 0x80
#define MODE_CELSIUSMODE 0xA0
#define MODE_FAHENHEITMODE 0xC0
#define MODE_ANGLESTEPSMODE 0xE0
#define MODE_SLOPEMASK 0x1F
#define MODE_MODEMASK 0xE0

#define MODE_MOTORON 0x01
#define MODE_BRAKE 0x02
#define MODE_REGULATED 0x04

#define REGULATION_MODE_IDLE 0x00
#define REGULATION_MODE_MOTOR_SPEED 0x01
#define REGULATION_MODE_MOTOR_SYNC 0x02

#define MOTOR_RUN_STATE_IDLE 0x00
#define MOTOR_RUN_STATE_RAMPUP 0x10
#define MOTOR_RUN_STATE_RUNNING 0x20
#define MOTOR_RUN_STATE_RAMPDOWN 0x40

LegoM.h

cpp

#pragma once
#include "YSLegoCommon.h"
class YSLego;
class TouchSensor;
class LightSensor;
class SoundSensor;
class UltrasonicSensor;
class LegoWheel;

class LegoM
{
 public:

  LegoM();
  ~LegoM();
 
  bool Init();
  
  bool TouchSetup();
  bool TouchUnset();
  bool TouchToggle(bool Key);

  bool LightSetup();
  bool LightUnset();
  int LightResponse();

  bool SoundSetup();
  bool SoundUnset();
  int SoundResponse();
 
  bool UltrasonicSetup();
  bool UltrasonicUnset();
  int UltrasonicResponse();
 
  void Move(int L_Speed,int R_Speed);
  void TurnLeft();
  void TurnRight();
  void Stop();
  void GoForward();
  void GoBackward();

  void Wait(int t);

  int response2int(int *resp);

  char* CreateCommand(const int CommandType
         ,const int CommandValue1
         ,const int CommandValue2);

  char* hexstr(const int CommandValue,char* temp); 

  int response[255];
  int response_length;

 private:

  YSLego* pLego;
  TouchSensor* pTouchSensor;
  LightSensor* pLightSensor;
  SoundSensor* pSoundSensor;
  UltrasonicSensor* pUltrasonicSensor;
  LegoWheel* pL_LegoWheel;
  LegoWheel* pR_LegoWheel;

  bool mForever;

  char c_Value[40];

};

LegoWheel.h

cpp

#pragma once
class YSLego;
class LegoM;

class LegoWheel
{
 public:
  LegoWheel(YSLego* aLego,int vPort);
  ~LegoWheel();

  //bool SetInputMode(LegoM* aLegoM);
  void MotorOn(LegoM* aLegoM);
  void Move(LegoM* aLegoM,int Speed);

 private: 
  int mPort;
  bool mForever;
  YSLego* pLego;
};

LightSensor.h

cpp

#pragma once
class YSLego;
class LegoM;

class LightSensor
{
 public:
  LightSensor(YSLego* aLego);
  ~LightSensor();

  bool SetInputMode(LegoM* aLegoM);
  bool UnsetInputMode(LegoM* aLegoM);
  int isResponse(LegoM* aLegoM);

 private: 
  int key;
  YSLego* pLego;
};

SoundSensor.h

cpp

#pragma once
class YSLego;
class LegoM;

class SoundSensor
{
 public:
  SoundSensor(YSLego* aLego);
  ~SoundSensor();

  bool SetInputMode(LegoM* aLegoM);
  bool UnsetInputMode(LegoM* aLegoM);
  int isResponse(LegoM* aLegoM);

 private: 
  int key;
  YSLego* pLego;
};

TouchSensor.h

cpp

#pragma once
class YSLego;
class LegoM;

class TouchSensor
{
 public:
  TouchSensor(YSLego* aLego);
  ~TouchSensor();

  bool SetInputMode(LegoM* aLegoM);
  bool UnsetInputMode(LegoM* aLegoM);
  bool isToggle(LegoM* aLegoM,bool vkey);

 private: 
  bool key;
  YSLego* pLego;
};

UltrasonicSensor.h

cpp

#pragma once
class YSLego;
class LegoM;

class UltrasonicSensor
{
 public:
  UltrasonicSensor(YSLego* aLego);
  ~UltrasonicSensor();

  bool SetInputMode(LegoM* aLegoM);
  bool UnsetInputMode(LegoM* aLegoM);
  int isResponse(LegoM* aLegoM);

  bool ResetInputMode(LegoM* aLegoM);

 private: 
  int key;
  YSLego* pLego;

  int mDistance;

};

YSLego.h

cpp

#pragma once

#include "YSLegoCommon.h"


#define NoConnect  0
#define USBConnect  1
#define BTConnect  2


#define MAX_CMD_LEN  64


////////////////////////////////////////////////
// BYTE 0: type of the message

#define TYPE_DIR_CMD_RESPONSE   0x00
#define TYPE_SYS_CMD_RESPONSE   0x01

#define TYPE_DIR_CMD_NO_RESPONSE  0x80
#define TYPE_SYS_CMD_NO_RESPONSE  0x81

#define TYPE_REPLY      0x02



////////////////////////////////////////////////
// START PROGRAM

// CMD

// byte 0: TYPE_DIR_CMD_RESPONSE or TYPE_DIR_CMD_NO_RESPONSE
// byte 1: 0x00 
// byte 2~21: filename. [15.3]+null terminator

// RETURN

// byte 0:
// byte 1:
// byte 2:


////////////////////////////////////////////////
// BYTE 0: type of the message



////////////////////////////////////////////////
// BYTE 0: type of the message



////////////////////////////////////////////////
// BYTE 0: type of the message

class YSLego
{
public:
 YSLego();
 ~YSLego();
 int iConnect;

 int USBconnect(void);
 int BTconnect(void);
 int isConnect(void);
 int makeMessage(char *);
 int sendMessage(int *);

 nFANTOM100::tStatus status;
 nFANTOM100::iNXTIterator * nxtIteratorPtr;
 nFANTOM100::iNXT * nxtPtr;
 nFANTOM100::iFile * filePtr;
 nFANTOM100::iFileIterator * fileIteratorPtr;

protected:
 

private:
 
 unsigned short ComndBytes[63];
 ViUInt8 ViComndBytes[63];
 ViUInt32 ResponseSize;

 //ViUInt8 btwait;

 bool RequestResponse;
 bool inputready;

 ViUInt8 ResponseBuffer[63];
 int ConvertBuffer[63];
 int numbytes;

 char infile[200];
 char outfile[200];
 char oldfile[200];
 char bufferin[500];

 bool  keyflag;

 bool StrCMP(ViChar *l, ViChar *r);

};

short int response2int(int *);

YSLegoCommon.h

cpp

// YSLegoCommon.h
// Include file for YSLegoExe


#pragma once
#pragma comment(lib, "winmm.lib")

//#include "targetver.h"

#include <stdio.h>
#include <tchar.h>

#include <iostream>
#include <sstream>
#include <iomanip>
#include <fstream>
#include <conio.h>
#include <time.h>
//#include <Mmsystem.h>
#include <Windows.h>

#include "iNXT.h"
#include "iNXTIterator.h"
#include "tStatus.h"

// Define NXT Direct Command Constants (new header)
#include "CommandConstant.h"

LegoM.cpp

cpp

#include "LegoM.h"
#include "YSLegoCommon.h"
#include "YSLego.h"
#include "TouchSensor.h"
#include "LightSensor.h"
#include "SoundSensor.h"
#include "UltrasonicSensor.h"
#include "LegoWheel.h"

using namespace std;

LegoM::LegoM() // Constructor
{
 pLego = new YSLego;
 pTouchSensor = NULL;
 pLightSensor = NULL;
 pSoundSensor = NULL;
 pUltrasonicSensor = NULL;  
 pL_LegoWheel = NULL;
 pR_LegoWheel = NULL;
}
LegoM::~LegoM() // Distructor
{
 Stop();
 if(TouchUnset()) cout << "Touch Sensor turns off!" << endl;
 if(LightUnset()) cout << "Light Sensor turns off!" << endl;
 if(SoundUnset()) cout << "Sound Sensor turns off!" << endl;
 if(UltrasonicUnset()) cout << "Ultrasonic Sensor turns off!" << endl;
 cout << "Now disconnecting...." <<endl;
 delete pLego; 
 delete pTouchSensor;
 delete pLightSensor;
 delete pSoundSensor;
 delete pUltrasonicSensor;
 delete pL_LegoWheel;
 delete pR_LegoWheel;
}
bool LegoM::Init()
{
 // Connect bluetooth
 if(!pLego->USBconnect())
  if(!pLego->BTconnect()) 
   return 0;
 
 // Sensors Setup (turn on)
 // "SETINPUTVALUE"  
 if(!TouchSetup()) return 0;
 else cout << "Touch Sensor turns on!" << endl;
 if(!LightSetup()) return 0;
 else cout << "Light Sensor turns on!" << endl;
 if(!SoundSetup()) return 0;
 else cout << "Sound Sensor turns on!" << endl;
 if(!UltrasonicSetup()) return 0;
 else cout << "Ultrasonic Sensor turns on!" << endl;
 // Left Wheel & Right Wheel Setup
 pL_LegoWheel = new LegoWheel(pLego,PORT_L_WHEEL);
 pR_LegoWheel = new LegoWheel(pLego,PORT_R_WHEEL);
 //pL_LegoWheel->MotorOn(this);
 //pR_LegoWheel->MotorOn(this);
 return 1;
}
bool LegoM::TouchSetup() // Touch Sensor Setup
{
 pTouchSensor = new TouchSensor(pLego);
 return pTouchSensor->SetInputMode(this);
}
bool LegoM::LightSetup() // Light Sensor Setup
{
 pLightSensor = new LightSensor(pLego);
 return pLightSensor->SetInputMode(this);
}
bool LegoM::SoundSetup() // Sound Sensor Setup
{
 pSoundSensor = new SoundSensor(pLego);
 return pSoundSensor->SetInputMode(this);
}
bool LegoM::UltrasonicSetup() // Ultrasonic Sensor Setup
{
 pUltrasonicSensor = new UltrasonicSensor(pLego);
 return pUltrasonicSensor->SetInputMode(this);
}
bool LegoM::TouchUnset() // Touch Sensor Unset
{
 pTouchSensor = new TouchSensor(pLego);
 return pTouchSensor->UnsetInputMode(this);
}
bool LegoM::LightUnset() // Light Sensor Unset
{
 pLightSensor = new LightSensor(pLego);
 return pLightSensor->UnsetInputMode(this);
}
bool LegoM::SoundUnset() // Sound Sensor Unset
{
 pSoundSensor = new SoundSensor(pLego);
 return pSoundSensor->UnsetInputMode(this);
}
bool LegoM::UltrasonicUnset() // Ultrasonic Sensor Unset
{
 pUltrasonicSensor = new UltrasonicSensor(pLego);
 return pUltrasonicSensor->UnsetInputMode(this);
}
bool LegoM::TouchToggle(bool Key) // return Touch Sensor's Status (on,off; show sensor's input value, too)
{
 return pTouchSensor->isToggle(this,Key);
}
int LegoM::LightResponse() // return Light Sensor's Input Value
{
 return pLightSensor->isResponse(this);
}
int LegoM::SoundResponse() // return Sound Sensor's Input Value
{
 return pSoundSensor->isResponse(this);
}
int LegoM::UltrasonicResponse() // return Ultrasonic Sensor's Input Value 
{
 //if(!pUltrasonicSensor->ResetInputMode(this)) {}//return -1;
 return pUltrasonicSensor->isResponse(this);
}


void LegoM::Move(int L_Speed,int R_Speed) // Control Both Wheels
{
 //while(!kbhit()){ test->Move(20,0,20,0,MODE_MOTORON,REGULATION_MODE_MOTOR_SYNC,MOTOR_RUN_STATE_RUNNING,0);}
 //pL_LegoWheel->Move(this,L_Speed,0,mode,rmode,runstate,forever);
 //bool nf1=0;
 //bool nf2=0;
 //if(!L_Speed) nf1 = 1;
 //if(!R_Speed) nf2 = 1;
 pL_LegoWheel->Move(this,L_Speed);
 pR_LegoWheel->Move(this,R_Speed);
}  
void LegoM::TurnLeft()
{
 Move(-80,0);
 Wait(750);
 Stop();
}
void LegoM::TurnRight()
{ 
 Move(0,-80);
 Wait(750);
 Stop();
}
void LegoM::Stop()
{
 Move(0,0);
}
void LegoM::GoForward()
{
 Move(-100,-100);
}
void LegoM::GoBackward()
{
 Move(80,80);
}
void LegoM::Wait(int t)
{
 /*
 int s_time;
 s_time = timeGetTime() + t;
 while(s_time > timeGetTime()) {}
 */
 
 Sleep(t);

}

int LegoM::response2int(int *resp) // return sensor's value
{
 int sensor_result;
 sensor_result = (int)resp[10];
 sensor_result = sensor_result << 8;
 sensor_result += (int)resp[9];
 return sensor_result;
}
char* LegoM::hexstr(const int CommandValue,char* temp) // Convert NXT Direct Command Constant to short String
{

 if(CommandValue>=16)
 {
  if(CommandValue/16 >= 10) temp[0]=CommandValue/16 +'A'-10;
  else temp[0]=CommandValue/16 + '0';

  if(CommandValue%16 >= 10) temp[0]=CommandValue%16 + 'A'-10;
  else temp[1]=CommandValue%16 + '0';

  return temp;

 } else {
 
  temp[0]='0';
  if(CommandValue%16 >= 10) temp[1]=CommandValue%16 + 'A'-10;
  else temp[1]=CommandValue%16 + '0';

  return temp;

 }

}
char* LegoM::CreateCommand(const int CommandType
         ,const int CommandValue1
         ,const int CommandValue2) // Create NXT Direct Command String         
{

 char temp[3];
 //char c_Value[40];
 for(int i =0;i<40;i++)
  c_Value[i]=NULL;

 
 //"00 07 00"
 if(CommandType == GETINPUTVALUE)
 {
  //char c_Value[9];
  c_Value[0] = '0';
  c_Value[1] = '0';
  c_Value[2] = ' ';
  c_Value[3] = '0';
  c_Value[4] = '7';
  c_Value[5] = ' ';
  hexstr(CommandValue1, temp);  // convert int to hex
  c_Value[6] = temp[0];
  c_Value[7] = temp[1];
 }
 if(CommandType == SETINPUTMODE)
 {
  c_Value[0] = '0';
  c_Value[1] = '0';
  c_Value[2] = ' ';
  c_Value[3] = '0';
  c_Value[4] = '5';
  c_Value[5] = ' ';
  hexstr(CommandValue1, temp);  // convert int to hex
  c_Value[6] = temp[0];
  c_Value[7] = temp[1];
  c_Value[8] = ' ';
  hexstr(CommandValue2, temp);  // convert int to hex
  c_Value[9] = temp[0];
  c_Value[10] = temp[1];
  c_Value[11] = ' ';
  c_Value[12] = '0';
  c_Value[13] = '0';
 }
 if(CommandType == SETOUTPUTSTATE)
 {

  if(CommandValue2>=0)
  {
  // "00 04 02 50 01 02 64 20 00 00 00 00 00" // move left wheel
  c_Value[0] = '0';
  c_Value[1] = '0';
  c_Value[2] = ' ';
  c_Value[3] = '0';
  c_Value[4] = '4';
  c_Value[5] = ' ';
  hexstr(CommandValue1, temp);  // convert int to hex
  c_Value[6] = temp[0];
  c_Value[7] = temp[1];
  c_Value[8] = ' ';
  hexstr(CommandValue2, temp);   // convert int to hex
  c_Value[9] = temp[0];
  c_Value[10] = temp[1];
  c_Value[11] = ' ';
  c_Value[12] = '0';
  c_Value[13] = '1';
  c_Value[14] = ' ';
  c_Value[15] = '0';
  c_Value[16] = '2';
  c_Value[17] = ' ';
  c_Value[18] = '0';
  c_Value[19] = '0';
  c_Value[20] = ' ';
  c_Value[21] = '2';
  c_Value[22] = '0';
  c_Value[23] = ' ';
  c_Value[24] = '0';
  c_Value[25] = '0';
  c_Value[26] = ' ';
  c_Value[27] = '0';
  c_Value[28] = '0';
  c_Value[29] = ' ';
  c_Value[30] = '0';
  c_Value[31] = '0';
  c_Value[32] = ' ';
  c_Value[33] = '0';
  c_Value[34] = '0';
  c_Value[35] = ' ';
  c_Value[36] = '0';
  c_Value[37] = '0';
  } else {
  // "00 04 02 50 01 02 64 20 00 00 00 00 00" // move left wheel
  c_Value[0] = '0';
  c_Value[1] = '0';
  c_Value[2] = ' ';
  c_Value[3] = '0';
  c_Value[4] = '4';
  c_Value[5] = ' ';
  hexstr(CommandValue1, temp);  // convert int to hex
  c_Value[6] = temp[0];
  c_Value[7] = temp[1];
  c_Value[8] = ' ';
  c_Value[9] = '-';
  hexstr(-CommandValue2, temp);   // convert int to hex
  c_Value[10] = temp[0];
  c_Value[11] = temp[1];
  c_Value[12] = ' ';
  c_Value[13] = '0';
  c_Value[14] = '1';
  c_Value[15] = ' ';
  c_Value[16] = '0';
  c_Value[17] = '2';
  c_Value[18] = ' ';
  c_Value[19] = '0';
  c_Value[20] = '0';
  c_Value[21] = ' ';
  c_Value[22] = '2';
  c_Value[23] = '0';
  c_Value[24] = ' ';
  c_Value[25] = '0';
  c_Value[26] = '0';
  c_Value[27] = ' ';
  c_Value[28] = '0';
  c_Value[29] = '0';
  c_Value[30] = ' ';
  c_Value[31] = '0';
  c_Value[32] = '0';
  c_Value[33] = ' ';
  c_Value[34] = '0';
  c_Value[35] = '0';
  c_Value[36] = ' ';
  c_Value[37] = '0';
  c_Value[38] = '0';
  }
 }
 // test command line
 //cout<<c_Value<<endl;
 return c_Value;
}

LegoWheel.cpp

cpp

#include "LegoWheel.h"
#include "LegoM.h"
#include "YSLegoCommon.h"
#include "YSLego.h"

using namespace std;

LegoWheel::LegoWheel(YSLego* aLego,int vPort)
{
 pLego = aLego;
 mPort = vPort;
}
LegoWheel::~LegoWheel()
{
}
void LegoWheel::MotorOn(LegoM* aLegoM)
{
 pLego->makeMessage(aLegoM->CreateCommand(SETOUTPUTSTATE,mPort,0)); // "SETOUTPUTMODE" 
}
void LegoWheel::Move(LegoM* aLegoM,int Speed)
{
 pLego->makeMessage(aLegoM->CreateCommand(SETOUTPUTSTATE,mPort,Speed)); // "SETOUTPUTMODE" 
 aLegoM->response_length = pLego->sendMessage(aLegoM->response);
 if(aLegoM->response_length != -1)
 {
  cout << "Motor Activate!" << endl;
 }
}

LightSensor.cpp

cpp

#include "LightSensor.h"
#include "LegoM.h"
#include "YSLegoCommon.h"
#include "YSLego.h"

using namespace std;

LightSensor::LightSensor(YSLego* aLego)
{
 pLego = aLego;
}
LightSensor::~LightSensor()
{
}
bool LightSensor::SetInputMode(LegoM* aLegoM)
{
 //"00 05 00 05 00"
 pLego->makeMessage(aLegoM->CreateCommand(SETINPUTMODE,PORT_LIGHT,TYPE_LIGHT_ACTIVE)); // "SETINPUTMODE" , light Sensor
 aLegoM->response_length = pLego->sendMessage(aLegoM->response);
 if(aLegoM->response[0] == 5 && aLegoM->response_length != -1) return 1;
 else return 0;
}
bool LightSensor::UnsetInputMode(LegoM* aLegoM)
{
 //"00 05 00 05 00"
 pLego->makeMessage(aLegoM->CreateCommand(SETINPUTMODE,PORT_LIGHT,TYPE_NO_SENSOR)); // "SETINPUTMODE" , light Sensor
 aLegoM->response_length = pLego->sendMessage(aLegoM->response);
 if(aLegoM->response[0] == 5 && aLegoM->response_length != -1) return 1;
 else return 0;
}
int LightSensor::isResponse(LegoM* aLegoM) // test version
{
 int sensor_result;
 pLego->makeMessage (aLegoM->CreateCommand(GETINPUTVALUE,PORT_LIGHT,0)); // getting touch sensor information
 aLegoM->response_length = pLego->sendMessage(aLegoM->response);
 if(aLegoM->response[0] == 7 && aLegoM->response_length != -1)
 {
  
  sensor_result = aLegoM->response2int(aLegoM->response);
  sensor_result = (sensor_result*100)/1023;
  cout << "Light Sensor Information: " << sensor_result <<endl; // << "[" << aLegoM->response[9] << "]"<<endl;
  //printf("[%4d][%d]\n", sensor_result, aLegoM->response[8]);
  
 }

 return sensor_result;
 
}

SoundSensor.cpp

cpp

#include "SoundSensor.h"
#include "LegoM.h"
#include "YSLegoCommon.h"
#include "YSLego.h"

using namespace std;

SoundSensor::SoundSensor(YSLego* aLego)
{
 pLego = aLego;
}
SoundSensor::~SoundSensor()
{
}
bool SoundSensor::SetInputMode(LegoM* aLegoM)
{
 //"00 05 00 05 00"
 pLego->makeMessage(aLegoM->CreateCommand(SETINPUTMODE,PORT_SOUND,TYPE_SOUND_DB)); // "SETINPUTMODE" , Sound Sensor
 aLegoM->response_length = pLego->sendMessage(aLegoM->response);
 if(aLegoM->response[0] == 5 && aLegoM->response_length != -1) return 1;
 else return 0;
}
bool SoundSensor::UnsetInputMode(LegoM* aLegoM)
{
 //"00 05 00 05 00"
 pLego->makeMessage(aLegoM->CreateCommand(SETINPUTMODE,PORT_SOUND,TYPE_NO_SENSOR)); // "SETINPUTMODE" , Sound Sensor
 aLegoM->response_length = pLego->sendMessage(aLegoM->response);
 if(aLegoM->response[0] == 5 && aLegoM->response_length != -1) return 1;
 else return 0;
}
int SoundSensor::isResponse(LegoM* aLegoM) // test version
{
 int sensor_result;
 pLego->makeMessage (aLegoM->CreateCommand(GETINPUTVALUE,PORT_SOUND,0)); // getting touch sensor information
 aLegoM->response_length = pLego->sendMessage(aLegoM->response);
 if(aLegoM->response[0] == 7 && aLegoM->response_length != -1)
 {
  
  sensor_result = aLegoM->response2int(aLegoM->response);
  sensor_result = (sensor_result*100)/1023;
  cout << "Sound Sensor Information: " << sensor_result <<endl; // << "[" << aLegoM->response[9] << "]"<<endl;
  //printf("[%4d][%d]\n", sensor_result, aLegoM->response[8]);
  
 }

 return sensor_result;
 
}

TouchSensor.cpp

cpp

#include "TouchSensor.h"
#include "LegoM.h"
#include "YSLegoCommon.h"
#include "YSLego.h"

using namespace std;

TouchSensor::TouchSensor(YSLego* aLego)
{
 pLego = aLego;
}
TouchSensor::~TouchSensor()
{
}
bool TouchSensor::SetInputMode(LegoM* aLegoM)
{
 //"00 05 00 01 00"
 pLego->makeMessage(aLegoM->CreateCommand(SETINPUTMODE,PORT_TOUCH,TYPE_SWITCH)); // "SETINPUTMODE" , touch Sensor
 aLegoM->response_length = pLego->sendMessage(aLegoM->response);
 if(aLegoM->response[0] == 5 && aLegoM->response_length != -1) return 1;
 else return 0;
}
bool TouchSensor::UnsetInputMode(LegoM* aLegoM)
{
 //"00 05 00 01 00"
 pLego->makeMessage(aLegoM->CreateCommand(SETINPUTMODE,PORT_TOUCH,TYPE_NO_SENSOR)); // "SETINPUTMODE" , touch Sensor
 aLegoM->response_length = pLego->sendMessage(aLegoM->response);
 if(aLegoM->response[0] == 5 && aLegoM->response_length != -1) return 1;
 else return 0;
}
bool TouchSensor::isToggle(LegoM* aLegoM,bool vkey) // test version
{
 int sensor_result;
 pLego->makeMessage (aLegoM->CreateCommand(GETINPUTVALUE,PORT_TOUCH,0)); // getting touch sensor information
 aLegoM->response_length = pLego->sendMessage(aLegoM->response);
 if(aLegoM->response[0] == 7 && aLegoM->response_length != -1)
 {
  
  sensor_result = aLegoM->response2int(aLegoM->response);
  //cout << "Touch Sensor Information: " << sensor_result <<endl; // << "[" << aLegoM->response[9] << "]"<<endl;
  //printf("[%4d][%d]\n", sensor_result, aLegoM->response[8]);
  

  if(aLegoM->response[8]==3) 
  { 
   if(vkey==1) cout<<"off"<<endl;
   return 0;
  } else {
   if(vkey==0) cout<<"on"<<endl;
   return 1;
  }
  
 }

 return 0;
 
}

UltrasonicSensor.cpp

cpp

#include "UltrasonicSensor.h"
#include "LegoM.h"
#include "YSLegoCommon.h"
#include "YSLego.h"

using namespace std;

UltrasonicSensor::UltrasonicSensor(YSLego* aLego)
{
 pLego = aLego;
}
UltrasonicSensor::~UltrasonicSensor()
{
}
bool UltrasonicSensor::SetInputMode(LegoM* aLegoM)
{
 //"00 05 00 05 00"
 pLego->makeMessage(aLegoM->CreateCommand(SETINPUTMODE,PORT_ULTRASONIC,TYPE_LOWSPEED)); // "SETINPUTMODE" , Sound Sensor
 aLegoM->response_length = pLego->sendMessage(aLegoM->response);
 if(aLegoM->response[0] == 5 && aLegoM->response_length != -1) return 1;
 else return 0;
 
}
bool UltrasonicSensor::UnsetInputMode(LegoM* aLegoM)
{
 //"00 05 00 05 00"
 pLego->makeMessage(aLegoM->CreateCommand(SETINPUTMODE,PORT_ULTRASONIC,TYPE_NO_SENSOR)); // "SETINPUTMODE" , Sound Sensor
 aLegoM->response_length = pLego->sendMessage(aLegoM->response);
 if(aLegoM->response[0] == 5 && aLegoM->response_length != -1) return 1;
 else return 0;
 
}
bool UltrasonicSensor::ResetInputMode(LegoM* aLegoM)
{
 pLego->makeMessage("00 0F 03 03 00 02 41 00");  
 aLegoM->response_length = pLego->sendMessage(aLegoM->response);
 pLego->makeMessage("00 0F 03 03 00 02 41 02"); 
 aLegoM->response_length = pLego->sendMessage(aLegoM->response);
 pLego->makeMessage("00 10 03"); 
 aLegoM->response_length = pLego->sendMessage(aLegoM->response);
 if(aLegoM->response[0] == 5 && aLegoM->response_length != -1) return 1;
 else return 0;

}
int UltrasonicSensor::isResponse(LegoM* aLegoM) // test version
{ 
 mDistance=0;
 //int sensor_result;
 while(mDistance==0) {
 pLego->makeMessage("00 0F 03 02 01 02 42"); 
 aLegoM->response_length = pLego->sendMessage(aLegoM->response);
 pLego->makeMessage("00 0E 03");   
 aLegoM->response_length = pLego->sendMessage(aLegoM->response);
 if( aLegoM->response[2] > 0 ) cout << "UltraSensor Activate! - ";
 pLego->makeMessage("00 10 03"); 
 aLegoM->response_length = pLego->sendMessage(aLegoM->response);
 mDistance = aLegoM->response[3];
 cout << "Distance = " << mDistance << endl;
 // pLego->makeMessage (aLegoM->CreateCommand(GETINPUTVALUE,PORT_ULTRASONIC,0)); // getting touch sensor information
 //aLegoM->response_length = pLego->sendMessage(aLegoM->response);
 //if(aLegoM->response[0] == 7 && aLegoM->response_length != -1)
 //{
  
 // sensor_result = aLegoM->response2int(aLegoM->response);
 // cout << "Ultrasonic Sensor Information: " << sensor_result <<endl; // << "[" << aLegoM->response[9] << "]"<<endl;
  //printf("[%4d][%d]\n", sensor_result, aLegoM->response[8]);
  
 //}
 }
 return mDistance;
 
}

YSLego.cpp

cpp

#include "YSLegoCommon.h"
#include "YSLego.h"

using namespace std;

YSLego::YSLego()
{ 
 //constructor
 iConnect = 0;
 nxtIteratorPtr = NULL;
 nxtPtr = NULL;
 filePtr = NULL;
 fileIteratorPtr = NULL;
 inputready = false;
 keyflag=false;
}

YSLego::~YSLego()
{ 
 //destructor
 nFANTOM100::iNXT::destroyNXT(nxtPtr);    
 cout << "Bye Bye" << endl;
 status.clear();
}

bool YSLego::StrCMP(ViChar *l, ViChar *r)
{
 int i;

 for(i=0; l[i] == r[i];i++) {
  if(l[i] == '\0') 
   return true;
 }
 if(r[i] == '\0')
  return true;

 return false;
}


int YSLego::BTconnect(void)
{
 ViChar name[256];

 cout << "Connecting to NXT with Bluetooth..." << endl;
 nxtIteratorPtr = nFANTOM100::iNXT::createNXTIterator(true, 10, status);
 if(status.isNotFatal())
 {
  nxtPtr = nxtIteratorPtr->getNXT(status);
  
  while(nxtPtr != NULL) {
   nxtIteratorPtr->getName(name, status);
   if(StrCMP(name, "BTH::NXT::"))
    break;

   //if there's other bluetooth on the list, 
   //this should work. but could not test it properly.
   //so be aware that this might not work.
   nxtIteratorPtr->advance(status);
   nxtPtr = nxtIteratorPtr->getNXT(status);

  }
  if(nxtPtr == NULL) {
   cout << "NXT Bluetooth connection: failed" << endl;
   return 0;
  }
  
  nFANTOM100::iNXT::destroyNXTIterator(nxtIteratorPtr);
  cout << "NXT Bluetooth connection: success" << endl;
 }
 else
 {
  cout << "NXT Bluetooth connection: failed" << endl;
  return 0;
 }
 return 1;
}

int YSLego::USBconnect(void)
{
 cout << "Connecting to NXT with USB..." << endl;
 nxtIteratorPtr = nFANTOM100::iNXT::createNXTIterator(false , 0, status); // USB Connect Check
 if(status.isNotFatal())
 {
  nxtPtr = nxtIteratorPtr->getNXT(status);
  if(nxtPtr == NULL) {
   cout << "NXT USB connection: failed" << endl;
   return 0;
  }

  nFANTOM100::iNXT::destroyNXTIterator(nxtIteratorPtr);
  cout << "NXT USB connection: success" << endl;
 }
 else
 {
  cout << "NXT USB connection: failed" << endl;
  return 0;
 }
 return 1;
}

int YSLego::makeMessage(char* input_txt)
{
 std::stringstream ss2;
 std::string charbuff;
 std::string buf;
// char *pch;
 char *tempchar;
 int count;

 ViUInt32 ResponseSizeArray[20] = { 2, 2, 2, 2, 2, 2, 24, 15, 2, 2, 2, 4, 2, 6, 3, 2, 19, 22, 0, 63 };
 
 keyflag=true;

 std::stringstream ss1(input_txt);
 count = 0;

 // convert buf into ComandByte array
 while(ss1 >> buf)
 {                                         
  count++;
  ss2.str("");
  ss2.clear();
  ss2 << setbase(16) << buf;

  // These are the command bytes
  ss2 >> ComndBytes[count]; 


  // Now convert back and display 
  ss2.str("");
  ss2.clear();

  ss2 << setbase(16) << ComndBytes[count];   
  
  // backflips here to get the right form 
  charbuff = ss2.str();             
  tempchar = &charbuff[0];

  // Conver to upper case 
  strupr(tempchar);                 

  // Now output for the user, show them what we read in
  //if(strlen(tempchar) == 1) 
  // cout << "0" << tempchar << " ";  
  //else
  // cout << tempchar << " ";
 }

 //cout << endl;

 numbytes = count-1;

 // Command Inputs, converted to ViUInt8 req by fantom
 for(count = 1; count < numbytes+1; count++) {
  ViComndBytes[count - 1] = (ViUInt8)ComndBytes[count+1];
 }        // Strips out the first byte 

 // Hex 0x80 (Dec 128) means Suppress Response
 if(ComndBytes[1] == 0x80)                
 {
  // Check the commnad line argument and pick if a response is called for
  RequestResponse = false;   
  ResponseSize = 0;
  inputready = true;
 }
 // Hex 0x00 means request response
 else if(ComndBytes[1] == 0)             
 {
  RequestResponse = true;

  // Selects the number of output bytes for a response
  ResponseSize = ResponseSizeArray[ComndBytes[2]];        
  inputready = true;
 }
 return 1;
}

int YSLego::sendMessage(int response_value[255])
{
 unsigned int            count;
 char          charbuff[10];
 stringstream  ss;      

 if(keyflag == true)
 {
  if(RequestResponse == true)
  {
   nxtPtr->sendDirectCommand
    (
    RequestResponse, // (true) get a response for this direct command 
    ViComndBytes,    // ViComndBytes array holding byte commands 
    numbytes,      // number of bytes in command 
    ResponseBuffer,  // Response buffer 
    ResponseSize,    // Response buffer size 
    status);         // Status 
  }
  else
  {
   nxtPtr->sendDirectCommand
    (
    RequestResponse, // (true) get a response for this direct command 
    ViComndBytes,    // ViComndBytes array holding byte commands 
    numbytes,        // number of bytes in command 
    NULL,            // Response buffer 
    0,               // Response buffer size 
    status);         // Status 
  }

  
  //cout << "Rcvd: ";
  for(count = 0; count < ResponseSize; count++)
  {
   ConvertBuffer[count] = ResponseBuffer[count];
   itoa(ConvertBuffer[count], charbuff, 16);
   strupr(charbuff);
   
   //if(strlen(charbuff) == 1){
   // cout << "0" << charbuff << " ";  // write to the display
   //}
   //else
   //{ 
   // cout << charbuff << " ";
   //}

  }
  //cout << "\n\n";
  
  for(unsigned int z = 0 ; z < ResponseSize ; z++)
   response_value[z] = ResponseBuffer[z];

  keyflag = false;
  return ResponseSize;
 }
 else
  return -1;
}

레고 로봇으로 미로 탈출하기 (Escape the maze by LEGO NXT) ​

Notes ​

Source Code ​

YSLegoExe.cpp (inclulde main function) ​

CommandConstant.h ​

LegoM.h ​

LegoWheel.h ​

LightSensor.h ​

SoundSensor.h ​

TouchSensor.h ​

UltrasonicSensor.h ​

YSLego.h ​

YSLegoCommon.h ​

LegoM.cpp ​

LegoWheel.cpp ​

LightSensor.cpp ​

SoundSensor.cpp ​

TouchSensor.cpp ​

UltrasonicSensor.cpp ​

YSLego.cpp ​

레고 로봇으로 미로 탈출하기 (Escape the maze by LEGO NXT)

Notes

Source Code

YSLegoExe.cpp (inclulde main function)

CommandConstant.h

LegoM.h

LegoWheel.h

LightSensor.h

SoundSensor.h

TouchSensor.h

UltrasonicSensor.h

YSLego.h

YSLegoCommon.h

LegoM.cpp

LegoWheel.cpp

LightSensor.cpp

SoundSensor.cpp

TouchSensor.cpp

UltrasonicSensor.cpp

YSLego.cpp