#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& 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& 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& 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& 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 cmds; istringstream sstr(obj); while(getline(sstr, str, ';')) { istringstream sstrSub(str); sstrSub >> str; cmds.push_back(ToLowerCase(str)); } return RedirectCmd(cmds); }