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

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


YSLegoExe.cpp (inclulde main function)

#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

#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

#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

#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

#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

#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

#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

#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

#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

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

#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

#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

#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

#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

#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

#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

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