int serialPortShutdown(void)
{
int i;
for(i=0; i < MAX_SERIAL_PORTS; i++)
serialPortClose(i);
return 1;
}
int serialPortShutdown() {
int i;
for(i=0;i<MAX_PORTS;i++) {
if (serialPortIsOpen(i))
serialPortClose(i);
}
return 1;
}
int serialPortCloseByName(const char *portName)
{
int portNum = portNumberForName(portName);
if (portNum < 0)
{ success(false);
return 0;
}
return serialPortClose(portNum);
}
EXPORT(sqInt) primitiveSerialPortClose(void) {
sqInt portNum;
portNum = interpreterProxy->stackIntegerValue(0);
if (interpreterProxy->failed()) {
return null;
}
serialPortClose(portNum);
if (interpreterProxy->failed()) {
return null;
}
interpreterProxy->pop(1);
return null;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "commander");
dynamic_reconfigure::Server<super_modified_servo::commanderConfig> srv;
dynamic_reconfigure::Server<super_modified_servo::commanderConfig>::CallbackType f;
f = boost::bind(&CommanderCallBack, _1, _2);
srv.setCallback(f);
ros::NodeHandle n;
ros::Publisher joint_pub = n.advertise<sensor_msgs::JointState>("/JointsState", 1000);
ros::Rate loop_rate(200);
fd = serialPortOpen("/dev/ttyUSB0", B115200);
sensor_msgs::JointState joint_state_msg;
char motor_name[] = "Motor";
joint_state_msg.header.stamp = ros::Time::now();
joint_state_msg.name.push_back(motor_name);
joint_state_msg.position.push_back(0.0);
joint_state_msg.velocity.push_back(0.0);
joint_state_msg.effort.push_back(0.0);
while(ros::ok())
{
if (start_flag == true)
{
if (getCommunicationSuccess() == false)
{
resetErrors(fd, motor_id);;
printf("[ERROR]: %d \n", getWarning());
}
joint_state_msg.header.stamp = ros::Time::now();
sprintf(motor_name, "ID:%d", motor_id);
joint_state_msg.name[0] = motor_name;
joint_state_msg.position[0] = ticks2deg(getPosition(fd, motor_id));
joint_state_msg.velocity[0] = ticks2deg(getVelocity(fd, motor_id));
joint_state_msg.effort[0] = getCurrent(fd, motor_id);
joint_pub.publish(joint_state_msg);
// profiledMoveToAbsolutePosition(fd, motor_id, deg2ticks(set_point));
// moveToAbsolutePosition(fd, motor_id, deg2ticks(set_point));
moveWithVelocity(fd, motor_id, deg2ticks(set_point));
}
ros::spinOnce();
loop_rate.sleep();
}
serialPortClose(fd);
return 0;
}
primitiveSerialPortClose(void)
{
sqInt portNum;
portNum = stackIntegerValue(0);
if (failed()) {
return null;
}
serialPortClose(portNum);
if (failed()) {
return null;
}
pop(1);
return null;
}
void MainWindow::actionMonitor() {
// Open close monitor
if (ui->widgetConsole->isVisible()) {
serialPortClose();
ui->actionMonitor->setChecked(false);
// Show main toolbar, hide monitor toolbar
ui->mainToolBar->setVisible(true);
ui->monitorToolBar->setVisible(false);
} else {
serialPortOpen();
ui->consoleEdit->setFocus();
ui->actionMonitor->setChecked(true);
// Hide main toolbar, show monitor toolbar
ui->mainToolBar->setVisible(false);
ui->monitorToolBar->setVisible(true);
}
}
MainWindow::MainWindow(QWidget *parent) :
QMainWindow(parent),
ui(new Ui::MainWindow)
{
ui->setupUi(this);
// Align last toolbar action to the right
QWidget *empty = new QWidget(this);
empty->setSizePolicy(QSizePolicy::Expanding,QSizePolicy::Preferred);
ui->mainToolBar->insertWidget(ui->actionMonitor, empty);
// Align last action to the right in the monitor toolbar
QWidget *emptyMonitor = new QWidget(this);
emptyMonitor->setSizePolicy(QSizePolicy::Expanding,QSizePolicy::Preferred);
ui->monitorToolBar->insertWidget(ui->actionMonitor, emptyMonitor);
// Hide monitor toolbar
ui->monitorToolBar->setVisible(false);
// Hide graphs widget
ui->graphsWidget->setVisible(false);
// Set monospaced font in the monitor
const QFont fixedFont = QFontDatabase::systemFont(QFontDatabase::FixedFont);
ui->consoleText->setFont(fixedFont);
// Set environment
settings = new SettingsStore(CONFIG_INI);
setArduinoBoard();
xmlFileName = "";
serial = NULL;
// Set zoom scale of the web view
float zoomScale = settings->zoomScale();
ui->webView->setZoomFactor(zoomScale);
// Hide messages
actionCloseMessages();
serialPortClose();
// Load blockly index
loadBlockly();
// Set timer to update list of available ports
updateSerialPorts();
QTimer *timer = new QTimer(this);
connect(timer, SIGNAL(timeout()), this, SLOT(updateSerialPorts()));
timer->start(5000);
ui->consoleText->document()->setMaximumBlockCount(100);
// Show/hide icon labels in the menu bar
iconLabels();
// Set process
process = new QProcess();
process->setProcessChannelMode(QProcess::MergedChannels);
connect(process,
SIGNAL(started()),
this,
SLOT(onProcessStarted()));
connect(process,
SIGNAL(readyReadStandardOutput()),
this,
SLOT(onProcessOutputUpdated()));
connect(process,
SIGNAL(finished(int)),
this,
SLOT(onProcessFinished(int)));
// Show opened file name in status bar
connect(statusBar(),
SIGNAL(messageChanged(QString)),
this,
SLOT(onStatusMessageChanged(QString)));
// Filter events to capture backspace key
ui->webView->installEventFilter(this);
}
