00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018 #ifndef HAND_BRIDGE_SDHX_H_
00019 #define HAND_BRIDGE_SDHX_H_
00020
00021 #include "serial_port.h"
00022 #include "boost/format.hpp"
00023 #include "boost/thread.hpp"
00024
00025 class SDHX {
00026 SerialPort serial;
00027 uint8_t rc;
00028 volatile bool initialized, reading;
00029 boost::mutex send_mutex;
00030 boost::mutex data_mutex;
00031 boost::thread read_thread;
00032 boost::chrono::steady_clock::time_point last_time;
00033
00034 void doRead(){
00035 const size_t MAX_LINE = 1024;
00036
00037 char buffer[MAX_LINE+1] = {0};
00038
00039 int a = 0, r = 1, offset = 0;
00040 while(a >= 0 && r > 0){
00041 if((a = serial.waitData(boost::chrono::milliseconds(1))) > 0 && (r = serial.read(buffer+offset, MAX_LINE-offset)) > 0){
00042 char * line = buffer;
00043 while(char * extra = strchr(line, '\n')){
00044 *extra=0;
00045 switch(line[0]){
00046 case 'r': tryParseRC(line); break;
00047 case 'P': tryReadValues(line, pos, "P=%hd,%hd", true); break;
00048 case 'V': tryReadValues(line, vel, "V=%hd,%hd"); break;
00049 case 'C': tryReadValues(line, cur, "C=%hd,%hd"); break;
00050 }
00051 line = extra+1;
00052 }
00053 offset = (buffer + offset + r - line);
00054 strncpy(buffer, line, offset);
00055 }
00056 }
00057 std::cerr << "stopped reading" << std::endl;
00058 boost::mutex::scoped_lock lock(data_mutex);
00059 reading = false;
00060 }
00061
00062 bool tryParseRC(const char *line){
00063 uint8_t res;
00064 if(sscanf(line, "rc=%hhx", &res) == 1){
00065 if(res != 0){
00066 boost::mutex::scoped_lock lock(data_mutex);
00067 rc = res;
00068 }
00069 return true;
00070 }
00071 return false;
00072 }
00073 template<typename T> bool tryReadValues(const char *line, T (&val)[2], const char *format, bool track_time = false) {
00074 T val1,val2;
00075 if(sscanf(line, format, &val1, &val2) == 2){
00076 boost::mutex::scoped_lock lock(data_mutex);
00077 val[0] = val1;
00078 val[1] = val2;
00079 if(track_time){
00080
00081 last_time = boost::chrono::steady_clock::now();
00082 }
00083 return true;
00084 }
00085 return false;
00086 }
00087
00088 bool send(const std::string &command){
00089 boost::mutex::scoped_lock lock(send_mutex);
00090 return serial.write(command);
00091 }
00092
00093 bool setPWM(const int16_t &val, const char *format) {
00094 return val == 0 || send(boost::str(boost::format(format) % val));
00095 }
00096
00097 int16_t pos[2], vel[2], cur[2];
00098 public:
00099 SDHX() : rc(0), initialized(false), reading(false) {
00100 }
00101 ~SDHX(){
00102 if(initialized && read_thread.joinable()){
00103 read_thread.interrupt();
00104 read_thread.join();
00105 }
00106 }
00107 bool init(const char* port, int16_t min_pwm0, int16_t min_pwm1, int16_t max_pwm0, int16_t max_pwm1){
00108 std::string dev(port);
00109 if(!serial.isOpen() && !serial.init(&dev[0])) return false;
00110
00111 boost::mutex::scoped_lock lock(data_mutex);
00112 if(!reading){
00113 reading = true;
00114 read_thread = boost::thread(boost::bind(&SDHX::doRead, this));
00115 }
00116
00117 lock.unlock();
00118 if(setPWM(min_pwm0, "set min_pwm0 %1%\r\n") &&
00119 setPWM(min_pwm1, "set min_pwm1 %1%\r\n") &&
00120 setPWM(max_pwm0, "set max_pwm0 %1%\r\n") &&
00121 setPWM(max_pwm1, "set max_pwm1 %1%\r\n")){
00122 lock.lock();
00123 rc = 0;
00124 initialized = true;
00125 return true;
00126 }
00127 return false;
00128 }
00129 bool isInitialized() {
00130 return initialized;
00131 }
00132
00133 template <typename Dur> bool getData(int16_t (&p)[2], int16_t (&v)[2], int16_t (&c)[2], const Dur &max_age){
00134 boost::mutex::scoped_lock lock(data_mutex);
00135 if((boost::chrono::steady_clock::now() - last_time) > max_age) return false;
00136 if(!reading) return false;
00137
00138 p[0] = pos[0];
00139 p[1] = pos[1];
00140 v[0] = vel[0];
00141 v[1] = vel[1];
00142 c[0] = cur[0];
00143 c[1] = cur[1];
00144
00145 return true;
00146 }
00147 bool poll(){
00148 return initialized && send("p;v;c\r\n");
00149 }
00150 bool halt(){
00151 return initialized && send("s\r\n");
00152 }
00153 bool move(const int16_t (&p)[2], const int16_t (&v)[2], const int16_t (&c)[2]){
00154 static boost::format command("m %hd,%hd %hd,%hd %hd,%hd\r\n");
00155 return initialized && send(boost::str(command % p[0] % p[1] % v[0] % v[1] % c[0] % c[1]));
00156 }
00157
00158
00159
00160
00161
00162 uint8_t getRC(){
00163 boost::mutex::scoped_lock lock(data_mutex);
00164 return rc;
00165 };
00166 };
00167 #endif // HAND_BRIDGE_SDHX_H_