segwayrmp_gui.cc
Go to the documentation of this file.
00001 #include "segwayrmp/segwayrmp.h"
00002 #include "segwayrmp/gui/segwayrmp_gui.h"
00003 #include "ui_segwayrmp_gui.h"
00004 #include "segwayrmp/impl/rmp_ftd2xx.h"
00005 
00006 #include <QtCore/QtConcurrentRun>
00007 #include <QtGui/QErrorMessage>
00008 #include <QtCore/QTimer>
00009 
00010 #define JOY_DEADBAND 3200
00011 #define JOY_MAX_VALUE 32768
00012 
00013 MainWindow::MainWindow(QWidget *parent) :
00014     QMainWindow(parent),
00015     ui(new Ui::MainWindow),
00016     connected_(false),
00017     interface_type_(segwayrmp::usb),
00018     rmp_type_(segwayrmp::rmp200),
00019     joystick_(0),
00020     running_(true)
00021 {
00022     // Init UI
00023     ui->setupUi(this);
00024     this->usb_devices_.push_back("No devices detected.");
00025     this->updateUSBList();
00026     // Init SDL
00027     SDL_Init(SDL_INIT_JOYSTICK);
00028     SDL_JoystickEventState(SDL_ENABLE);
00029     this->updateJoysticks();
00030     QTimer * timer = new QTimer(this);
00031 
00032     // Connection related signals and slots
00033     connect(ui->connect_button, SIGNAL(clicked()), this, SLOT(onConnectClicked()));
00034 
00035     connect(ui->connection_type, SIGNAL(currentIndexChanged(QString)), this, SLOT(connectionTypeChanged(QString)));
00036 
00037     connect(ui->rmp_type, SIGNAL(currentIndexChanged(QString)), this, SLOT(rmpTypeChanged(QString)));
00038 
00039     connect(ui->refresh_button, SIGNAL(clicked()), this, SLOT(updateUSBList()));
00040     connect(this, SIGNAL(USBUpdateComplete()), this, SLOT(USBListUpdated()));
00041 
00042     // Logging connections
00043     connect(this, SIGNAL(segwayLog(QString)), this, SLOT(handleSegwayLog(QString)));
00044 
00045     // Segway Status connection
00046     connect(this, SIGNAL(segwayStatus(QString)), this, SLOT(handleSegwayStatus(QString)));
00047 
00048     // Command button connections
00049     connect(ui->reset_integrators_button, SIGNAL(clicked()), this, SLOT(onResetIntegrators()));
00050     connect(ui->disable_motors_button, SIGNAL(clicked()), this, SLOT(onDisableMotors()));
00051     connect(ui->tractor_button, SIGNAL(clicked()), this, SLOT(onRequestTractor()));
00052     connect(ui->balance_button, SIGNAL(clicked()), this, SLOT(onRequestBalance()));
00053     connect(ui->power_down_button, SIGNAL(clicked()), this, SLOT(onRequestPowerDown()));
00054     connect(ui->balance_lockout_button, SIGNAL(clicked()), this, SLOT(onBalanceLockout()));
00055     connect(ui->balance_unlock_button, SIGNAL(clicked()), this, SLOT(onBalanceUnlock()));
00056     connect(ui->send_config_button, SIGNAL(clicked()), this, SLOT(onSendConfig()));
00057 
00058     // joystick stuff
00059     connect(ui->joy_list_cb, SIGNAL(activated(int)), this, SLOT(onJoystickChanged(int)));
00060     connect(timer, SIGNAL(timeout()), this, SLOT(commandPoll()));
00061     timer->start(1000.0/25.0);
00062     this->onJoystickChanged(0);
00063 }
00064 
00065 MainWindow::~MainWindow()
00066 {
00067     delete ui;
00068 }
00069 
00070 void MainWindow::closeEvent(QCloseEvent * e) {
00071     this->running_ = false;
00072 }
00073 
00074 void MainWindow::commandPoll() {
00075     this->joy_mutex_.lock();
00076     // If the joystick isn't there do nothing
00077     if (!this->joystick_) {
00078         this->joy_mutex_.unlock();
00079         return;
00080     }
00081     SDL_JoystickUpdate();
00082     int lv_i = SDL_JoystickGetAxis(this->joystick_, ui->lv_axis_cb->currentIndex());
00083     int av_i = SDL_JoystickGetAxis(this->joystick_, ui->av_axis_cb->currentIndex());
00084     this->joy_mutex_.unlock();
00085     // Deadband
00086     if (lv_i > -JOY_DEADBAND && lv_i < JOY_DEADBAND) {
00087         lv_i = 0;
00088     }
00089     if (av_i > -JOY_DEADBAND && av_i < JOY_DEADBAND) {
00090         av_i = 0;
00091     }
00092     // Normalize
00093     double lv = float(lv_i)/float(JOY_MAX_VALUE);
00094     lv *= -1; // Invert X
00095     double av = float(av_i)/float(JOY_MAX_VALUE);
00096     // Show in progress bars
00097     ui->lv_pb->setValue(int(lv*100));
00098     ui->av_pb->setValue(int(av*100));
00099     if (ui->send_chb->checkState() == Qt::Checked && this->rmp_) {
00100         try {
00101             double lv_s = ui->lv_scale_slider->value() / 100.0f;
00102             double av_s = ui->av_scale_slider->value() / 100.0f;
00103             av_s *= -1.0; // Invert turning for segway
00104             this->rmp_->moveCounts((short int)(lv*1176*lv_s), (short int)(av*1024*av_s));
00105         } catch (const std::exception &e) {
00106             qDebug() << "Error commanding base: " << e.what();
00107         }
00108     }
00109 }
00110 
00111 void MainWindow::onJoystickChanged(int index) {
00112     this->joy_mutex_.lock();
00113     if (this->joystick_) {
00114         SDL_JoystickClose(this->joystick_);
00115         this->joystick_ = 0;
00116     }
00117     if (SDL_NumJoysticks() > 0) {
00118         this->joystick_ = SDL_JoystickOpen(index);
00119         size_t num_axes = SDL_JoystickNumAxes(this->joystick_);
00120         ui->lv_axis_cb->clear();
00121         ui->lv_axis_cb->setEnabled(true);
00122         ui->av_axis_cb->clear();
00123         ui->av_axis_cb->setEnabled(true);
00124         ui->send_chb->setEnabled(true);
00125         if (num_axes == 0) {
00126             ui->lv_axis_cb->addItem("No axes.");
00127             ui->av_axis_cb->addItem("No axes.");
00128         }
00129         for (size_t i = 0; i < num_axes; ++i) {
00130             ui->lv_axis_cb->addItem(QString("Axis %1").arg(QString::number(i)));
00131             ui->av_axis_cb->addItem(QString("Axis %1").arg(QString::number(i)));
00132         }
00133         if (num_axes >= 2) {
00134             ui->lv_axis_cb->setCurrentIndex(1);
00135             ui->av_axis_cb->setCurrentIndex(0);
00136         }
00137     } else {
00138         ui->lv_axis_cb->clear();
00139         ui->lv_axis_cb->setEnabled(false);
00140         ui->av_axis_cb->clear();
00141         ui->av_axis_cb->setEnabled(false);
00142         ui->send_chb->setEnabled(false);
00143     }
00144     this->joy_mutex_.unlock();
00145 }
00146 
00147 void MainWindow::updateJoysticks() {
00148     size_t num_joysticks = SDL_NumJoysticks();
00149     ui->joy_list_cb->clear();
00150     if (num_joysticks == 0) {
00151         ui->joy_list_cb->addItem("No Joysticks Detected.");
00152     }
00153     for (size_t i = 0; i < num_joysticks; ++i) {
00154         ui->joy_list_cb->addItem(QString::fromAscii(SDL_JoystickName(i)));
00155     }
00156 }
00157 
00158 void MainWindow::onResetIntegrators() {
00159     if (this->connected_ and this->rmp_) {
00160         rmp_->resetAllIntegrators();
00161     }
00162 }
00163 
00164 void MainWindow::onDisableMotors() {
00165     if (this->connected_ and this->rmp_) {
00166         rmp_->shutdown();
00167     }
00168 }
00169 
00170 void MainWindow::onRequestTractor() {
00171     if (this->connected_ and this->rmp_) {
00172         rmp_->setOperationalMode(segwayrmp::tractor);
00173     }
00174 }
00175 
00176 void MainWindow::onRequestBalance() {
00177     if (this->connected_ and this->rmp_) {
00178         rmp_->setOperationalMode(segwayrmp::balanced);
00179     }
00180 }
00181 
00182 void MainWindow::onRequestPowerDown() {
00183     if (this->connected_ and this->rmp_) {
00184         rmp_->setOperationalMode(segwayrmp::power_down);
00185     }
00186 }
00187 
00188 void MainWindow::onBalanceLockout() {
00189     if (this->connected_ and this->rmp_) {
00190         rmp_->setBalanceModeLocking(true);
00191     }
00192 }
00193 
00194 void MainWindow::onBalanceUnlock() {
00195     if (this->connected_ and this->rmp_) {
00196         rmp_->setBalanceModeLocking(false);
00197     }
00198 }
00199 
00200 void MainWindow::onSendConfig() {
00201     if (this->connected_ and this->rmp_) {
00202         rmp_->setMaxVelocityScaleFactor(ui->max_vel_sb->value());
00203         rmp_->setMaxAccelerationScaleFactor(ui->max_accel_sb->value());
00204         rmp_->setMaxTurnScaleFactor(ui->max_turn_rate_sb->value());
00205         rmp_->setCurrentLimitScaleFactor(ui->current_limit_sb->value());
00206         QString gain_schedule = ui->gain_schedule_cb->currentText();
00207         if (gain_schedule == "Light") {
00208             rmp_->setControllerGainSchedule(segwayrmp::light);
00209         }
00210         if (gain_schedule == "Tall") {
00211             rmp_->setControllerGainSchedule(segwayrmp::tall);
00212         }
00213         if (gain_schedule == "Heavy") {
00214             rmp_->setControllerGainSchedule(segwayrmp::heavy);
00215         }
00216         ui->statusbar->showMessage("Configurations sent to the Segway RMP", 3000);
00217     }
00218 }
00219 
00220 void MainWindow::onSegwayLog(QString log_type, const std::string &msg) {
00221     emit segwayLog(QString("%1: %2").arg(log_type, QString::fromStdString(msg)));
00222 }
00223 
00224 void MainWindow::handleSegwayLog(QString log) {
00225     qDebug() << log;
00226     ui->statusbar->showMessage(log, 3000);
00227 }
00228 
00229 void MainWindow::onSegwayStatus(segwayrmp::SegwayStatus::Ptr ss) {
00230     QString qss = QString::fromStdString(ss->str());
00231     emit segwayStatus(QString("Time Stamp:\n%1").arg(qss));
00232 }
00233 
00234 void MainWindow::handleSegwayStatus(QString ss) {
00235     ui->status_label->setText(ss);
00236 }
00237 
00238 void MainWindow::onConnectClicked() {
00239     if (!this->connected_) {
00240         try {
00241             rmp_ = new segwayrmp::SegwayRMP(this->interface_type_, this->rmp_type_);
00242             if (this->interface_type_ == segwayrmp::usb) {
00243                 rmp_->configureUSBByIndex(ui->connection_id->currentIndex());
00244             } else if (this->interface_type_ == segwayrmp::serial) {
00245                 rmp_->configureSerial(ui->connection_id->currentText().toStdString());
00246             }
00247             rmp_->setLogMsgCallback("error", boost::bind(&MainWindow::onSegwayLog, this, "Error", _1));
00248             rmp_->setLogMsgCallback("info", boost::bind(&MainWindow::onSegwayLog, this, "Info", _1));
00249             rmp_->setLogMsgCallback("debug", boost::bind(&MainWindow::onSegwayLog, this, "Debug", _1));
00250             rmp_->setStatusCallback(boost::bind(&MainWindow::onSegwayStatus, this, _1));
00251             rmp_->connect();
00252             this->connected_ = true;
00253             ui->connect_button->setText("Disconnect");
00254             ui->connection_id->setEnabled(false);
00255             ui->connection_type->setEnabled(false);
00256             ui->rmp_type->setEnabled(false);
00257             ui->refresh_button->setEnabled(false);
00258         } catch (const std::exception &e) {
00259             QString error = QString::fromAscii(e.what());
00260             QErrorMessage error_msg;
00261             QString error_string = QString("Error connecting: %1").arg(error);
00262             qDebug() << "Error: " << error_string;
00263             error_msg.showMessage(error_string);
00264             error_msg.exec();
00265             delete rmp_;
00266             rmp_ = NULL;
00267             this->connected_ = false;
00268         }
00269     } else {
00270         delete rmp_;
00271         rmp_ = NULL;
00272         this->connected_ = false;
00273         ui->connect_button->setText("Connect");
00274         ui->connection_id->setEnabled(true);
00275         ui->connection_type->setEnabled(true);
00276         ui->rmp_type->setEnabled(true);
00277         ui->refresh_button->setEnabled(true);
00278     }
00279 }
00280 
00281 void MainWindow::connectionTypeChanged(QString mode) {
00282     if (mode == "USB") {
00283         ui->refresh_button->setEnabled(true);
00284         ui->connection_id->setEditable(false);
00285         this->updateUSBList();
00286         this->interface_type_ = segwayrmp::usb;
00287     } else if (mode == "Serial") {
00288         ui->refresh_button->setEnabled(false);
00289         ui->connection_id->setEditable(true);
00290         ui->connection_id->clear();
00291         ui->connection_id->addItem(QString("/dev/ttyUSB0"));
00292         this->interface_type_ = segwayrmp::serial;
00293     }
00294 }
00295 
00296 void MainWindow::rmpTypeChanged(QString type) {
00297     if (type == "rmp50") {
00298         this->rmp_type_ = segwayrmp::rmp50;
00299     }
00300     if (type == "rmp100") {
00301         this->rmp_type_ = segwayrmp::rmp100;
00302     }
00303     if (type == "rmp200") {
00304         this->rmp_type_ = segwayrmp::rmp200;
00305     }
00306     if (type == "rmp400") {
00307         this->rmp_type_ = segwayrmp::rmp400;
00308     }
00309 }
00310 
00311 void MainWindow::USBListUpdated() {
00312     ui->connection_id->clear();
00313     foreach (QString entry, this->usb_devices_) {
00314         ui->connection_id->addItem(entry);
00315     }
00316 }
00317 
00318 void MainWindow::updateUSBList() {
00319     QtConcurrent::run(this, &MainWindow::updateUSBList_);
00320 }
00321 
00322 void MainWindow::updateUSBList_() {
00323     std::vector<FT_DEVICE_LIST_INFO_NODE> devices = segwayrmp::enumerateUSBDevices();
00324     std::vector<FT_DEVICE_LIST_INFO_NODE>::iterator it;
00325     this->usb_devices_.clear();
00326     int i = 0;
00327     if (devices.size() == 0) {
00328         this->usb_devices_.push_back("No devices detected.");
00329     }
00330     for (it = devices.begin(); it != devices.end(); it++) {
00331         QString line = QString("%1 : %2 - %3").arg(QString::number(i), QString(it->Description), QString(it->SerialNumber));
00332         this->usb_devices_.push_back(line);
00333     }
00334     emit USBUpdateComplete();
00335 }
00336 


libsegwayrmp
Author(s): William Woodall
autogenerated on Fri Jan 3 2014 11:28:21