#include "CommandNXT.h"
// constructor
cCommandNXT::cCommandNXT()
{
isOpen = false;
}
// destructor
cCommandNXT::~cCommandNXT()
{
}
// Convert string type to int
int cCommandNXT::Str2Int(const string &str)
{
stringstream ss(str);
int n;
ss >> n;
return n;
}
// Convert int type to string
string cCommandNXT::Int2Str(int n)
{
stringstream ss;
ss << n;
return ss.str();
}
// Convert str type to bool
bool cCommandNXT::Str2Bool(const string &str)
{
stringstream ss(str);
bool boolen;
ss >> boolen;
return boolen;
}
// change string to lowercase
string cCommandNXT::ToLowerCase(string str)
{
for(int i = 0; i < str.length(); ++i)
str[i] = tolower(str[i]);
return str;
}
// Check output port
bool cCommandNXT::CheckOutPort(const string& port)
{
if(port == "out_a" || port == "out_b" || port == "out_c")
return true;
return false;
}
// Check input port
bool cCommandNXT::CheckInPort(const string& port)
{
if(port == "in_1" || port == "in_2" || port == "in_3" || port == "in_4")
return true;
return false;
}
// Check range of power
bool cCommandNXT::CheckPower(const string& power)
{
if(Str2Int(power) >= 0 && Str2Int(power) <= 100)
return true;
return false;
}
// Check boolean value
bool cCommandNXT::CheckBoolean(const string& boolen)
{
if(boolen == "true" || boolen == "false")
return true;
return false;
}
// Check tacho
bool cCommandNXT::CheckTacho(const string& tacho)
{
}
// Convert output port
int cCommandNXT::ConvertPortOut(const string& port)
{
if(port == "out_a")
return 0;
else if(port == "out_b")
return 1;
else if(port == "out_c")
return 2;
return -1;
}
// Convert input port
int cCommandNXT::ConvertPortIn(const string& port)
{
if(port == "in_1")
return 0;
else if(port == "in_2")
return 1;
else if(port == "in_3")
return 2;
else if(port == "in_4")
return 3;
return -1;
}
// choose the module and call appropriate
string cCommandNXT::RedirectCmd(vector<string>& cmds)
{
//motor module
if(cmds.at(0) == "motor")
{
return MotorCmd(cmds);
}
//sensor module
else if(cmds.at(0) == "sensor")
{
return SensorCmd(cmds);
}
//nxt module
else if(cmds.at(0) == "nxt")
{
return NXTCmd(cmds);
}
//unknown module
return "err: unknown_module: " + cmds.at(0);
}
// motor module
string cCommandNXT::MotorCmd(vector<string>& cmds)
{
string cmd = cmds.at(1);
string reply = "ok";
int countArgs = cmd.size();
if(isOpen == false)
return "err: not_enable_interface";
// GetRotationCount
if(cmd == "getrotationcount")
{
if(countArgs < 2 || !CheckOutPort( cmds.at(2) ))
return "err: wrong_params";
reply += ": " + Int2Str( NXT::Motor::GetRotationCount(ConvertPortOut(cmds.at(2))) );
}
// ResetRotationCount
else if(cmd == "resetrotationcount")
{
if(countArgs < 3 || !CheckOutPort( cmds.at(2) ) || !CheckBoolean(cmds.at(3)))
return "err: wrong_params";
NXT::Motor::ResetRotationCount(ConvertPortOut(cmds.at(2)), Str2Bool(cmds.at(3)));
}
// SetForward
else if(cmd == "setforward")
{
if(countArgs < 3 || !CheckOutPort( cmds.at(2) ) || !CheckPower(cmds.at(3)))
return "err: wrong_params";
NXT::Motor::SetForward(ConvertPortOut(cmds.at(2)), Str2Int(cmds.at(3)));
}
// SetReverse
else if(cmd == "setreverse")
{
if(countArgs < 3 || !CheckOutPort( cmds.at(2) ) || !CheckPower(cmds.at(3)))
return "err: wrong_params";
NXT::Motor::SetReverse(ConvertPortOut(cmds.at(2)), Str2Int(cmds.at(3)));
}
// Stop
else if(cmd == "stop")
{
if(countArgs < 3 || !CheckOutPort( cmds.at(2) ) || !CheckBoolean(cmds.at(3)))
return "err: wrong_params";
NXT::Motor::Stop(ConvertPortOut(cmds.at(2)), Str2Bool(cmds.at(3)));
}
// BrakeOn
else if(cmd == "brakeon")
{
if(countArgs < 2 || !CheckOutPort( cmds.at(2) ))
return "err: wrong_params";
NXT::Motor::BrakeOn(ConvertPortOut(cmds.at(2)));
}
// BrakeOff
else if(cmd == "brakeoff")
{
if(countArgs < 2 || !CheckOutPort( cmds.at(2) ))
return "err: wrong_params";
NXT::Motor::BrakeOff(ConvertPortOut(cmds.at(2)));
}
// GoTo
else if(cmd == "goto")
{
if(countArgs < 5 || !CheckOutPort( cmds.at(2) ) || !CheckPower(cmds.at(2)) || !CheckBoolean(cmds.at(5)))
return "err: wrong_params";
NXT::Motor::GoTo(ConvertPortOut(cmds.at(2)), Str2Int(cmds.at(3)), Str2Int(cmds.at(4)), Str2Bool( cmds.at(5) ));
}
// unknown command
else
{
return "err: unknown_command: >>" + cmd;
}
return reply;
}
// sensor module
string cCommandNXT::SensorCmd(vector<string>& cmds)
{
string cmd = cmds.at(1);
string reply = "ok";
int countArgs = cmd.size();
if(isOpen == false)
return "err: not_enable_interface";
// SetTouch
if(cmd == "settouch")
{
if(countArgs < 2 || !CheckInPort( cmds.at(2) ))
return "err: wrong_params";
NXT::Sensor::SetTouch(ConvertPortIn( cmds.at(2) ));
}
// SetSound
if(cmd == "setsound")
{
if(countArgs < 2 || !CheckInPort( cmds.at(2) ))
return "err: wrong_params";
NXT::Sensor::SetSound(ConvertPortIn( cmds.at(2) ));
}
// SetLight
if(cmd == "setlight")
{
if(countArgs < 3 || !CheckInPort( cmds.at(2) ) || !CheckBoolean( cmds.at(3) ))
return "err: wrong_params";
NXT::Sensor::SetLight(ConvertPortIn( cmds.at(2) ), Str2Bool(cmds.at(3) ));
}
// SetSonar
if(cmd == "setsonar")
{
if(countArgs < 2 || !CheckInPort( cmds.at(2) ))
return "err: wrong_params";
NXT::Sensor::SetSonar(ConvertPortIn( cmds.at(2) ));
}
// SetRaw
if(cmd == "setraw")
{
if(countArgs < 2 || !CheckInPort( cmds.at(2) ))
return "err: wrong_params";
NXT::Sensor::SetRaw(ConvertPortIn( cmds.at(2) ));
}
// GetValue
if(cmd == "getvalue")
{
if(countArgs < 2 || !CheckInPort( cmds.at(2) ))
return "err: wrong_params";
reply += ": " + NXT::Sensor::GetValue(Str2Int( cmds.at(2) ));
}
// LSGetStatus
if(cmd == "lsgetstatus")
{
if(countArgs < 2 || !CheckInPort( cmds.at(2) ))
return "err: wrong_params";
reply += ": " + NXT::Sensor::LSGetStatus(ConvertPortIn( cmds.at(2) ));
}
// GetSonarValue
if(cmd == "getsonarvalue")
{
if(countArgs < 2 || !CheckInPort( cmds.at(2) ))
return "err: wrong_params";
reply += ": " + NXT::Sensor::GetSonarValue(ConvertPortIn( cmds.at(2) ));
}
// SetSonarOff
if(cmd == "setsonaroff")
{
if(countArgs < 2 || !CheckInPort( cmds.at(2) ))
return "err: wrong_params";
NXT::Sensor::SetSonarOff(ConvertPortIn( cmds.at(2) ));
}
// SetSonarSingleShot
if(cmd == "setsonarsingleshot")
{
if(countArgs < 2 || !CheckInPort( cmds.at(2) ))
return "err: wrong_params";
NXT::Sensor::SetSonarSingleShot(ConvertPortIn( cmds.at(2) ));
}
// SetSonarContinuous
if(cmd == "setsonarcontinuous")
{
if(countArgs < 2 || !CheckInPort( cmds.at(2) ))
return "err: wrong_params";
NXT::Sensor::SetSonarContinuous(ConvertPortIn( cmds.at(2) ));
}
// SetSonarContinuousInterval
if(cmd == "setsonarcontinuousinterval")
{
if(countArgs < 3 || !CheckInPort( cmds.at(2) ))
return "err: wrong_params";
NXT::Sensor::SetSonarContinuousInterval(ConvertPortIn( cmds.at(2) ), Str2Int( cmds.at(3) ));
}
// unknown command
else
{
return "err: unknown_command: " + cmd;
}
return reply;
}
// nxt module
string cCommandNXT::NXTCmd(vector<string>& cmds)
{
string cmd = cmds.at(1);
string reply = "ok";
int countArgs = cmd.size();
if(isOpen == false && cmd != "open")
return "err: not_enable_interface";
// Open
if(cmd == "open")
{
if(!NXT::Open())
return "err";
isOpen = true;
}
// Close
else if(cmd == "close")
{
NXT::Close();
isOpen = false;
}
// BatteryLevel
else if(cmd == "batterylevel")
{
reply += ": " + Int2Str( NXT::BatteryLevel() );
}
// GetName
else if(cmd == "getname")
{
reply += ": " + NXT::GetName();
}
// StartProgram
else if(cmd == "startprogram")
{
if(countArgs < 2)
return "err: wrong_params";
NXT::StartProgram( cmds.at(2) );
}
// StopProgram
else if(cmd == "stopprogram")
{
NXT::StopProgram();
}
// PlayTone
else if(cmd == "playtone")
{
if(countArgs < 3)
return "err: wrong_params";
NXT::PlayTone(Str2Int( cmds.at(2) ), Str2Int( cmds.at(3) ));
}
// PlaySoundFile
else if(cmd == "playsoundfile")
{
if(countArgs < 3 || !CheckBoolean( cmds.at(3) ))
return "err: wrong_params";
NXT::PlaySoundFile( cmds.at(2), Str2Bool( cmds.at(3) ));
}
// StopSound
else if(cmd == "stopsound")
{
NXT::StopSound();
}
// unknown command
else
{
return "err: unknown_command: >>" + cmd;
}
return reply;
}
// parse the request and to operate it
string cCommandNXT::CallCmd(string& obj)
{
string str;
vector<string> cmds;
istringstream sstr(obj);
while(getline(sstr, str, ';'))
{
istringstream sstrSub(str);
sstrSub >> str;
cmds.push_back(ToLowerCase(str));
}
return RedirectCmd(cmds);
}