AIwiki

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