void NetworkWidget::checkNetwork()
{
// check if we have received any packets since last time.
if (stats_packets == 0 || stats_packets > stats_packets_old) {
// Restart the Timer
NetworkTimer->stop();
NetworkTimer->start( networkFailureTime );
stats_packets_old = stats_packets; // Update the value
return;
}
if (stats_packets == stats_packets_old) {
// Network timeout has occured.
STATE = NET_FAILURE;
stats_network_failures++;
ui->lblStatus->setText("Network Failure!");
ui->lblNetworkFailures->setText(QString::number(stats_network_failures, 10));
NetworkTimer->stop();
// Dump queue
emptyQueue();
if (ui->chkDefaultsOnNetworkFail->isChecked()) {
sendDefaults();
}
}
}
void StreamlineSetup::handleRequest(char* xml) {
mxml_node_t *tree, *node;
const char * attr = NULL;
tree = mxmlLoadString(NULL, xml, MXML_NO_CALLBACK);
node = mxmlFindElement(tree, tree, TAG_REQUEST, ATTR_TYPE, NULL, MXML_DESCEND_FIRST);
if (node) {
attr = mxmlElementGetAttr(node, ATTR_TYPE);
}
if (attr && strcmp(attr, VALUE_EVENTS) == 0) {
sendEvents();
logg.logMessage("Sent events xml response");
} else if (attr && strcmp(attr, VALUE_CONFIGURATION) == 0) {
sendConfiguration();
logg.logMessage("Sent configuration xml response");
} else if (attr && strcmp(attr, VALUE_COUNTERS) == 0) {
sendCounters();
logg.logMessage("Sent counters xml response");
} else if (attr && strcmp(attr, VALUE_CAPTURED) == 0) {
CapturedXML capturedXML;
char* capturedText = capturedXML.getXML(false);
sendData(capturedText, strlen(capturedText), RESPONSE_XML);
free(capturedText);
logg.logMessage("Sent captured xml response");
} else if (attr && strcmp(attr, VALUE_DEFAULTS) == 0) {
sendDefaults();
logg.logMessage("Sent default configuration xml response");
} else {
char error[] = "Unknown request";
sendData(error, strlen(error), RESPONSE_NAK);
logg.logMessage("Received unknown request:\n%s", xml);
}
mxmlDelete(tree);
}
/*
Get the datagrams and store them in a queue.
We have a QTimer that is running and taking them out of the queue.
*/
void NetworkWidget::processPendingDatagrams()
{
while (udpSocket->hasPendingDatagrams())
{
QByteArray datagram;
datagram.resize(udpSocket->pendingDatagramSize());
udpSocket->readDatagram(datagram.data(), datagram.size());
if (STATE == EMERG_STOP || (STATE == NET_FAILURE && ui->chkStopOnNetworkFailure->isChecked())) {
// Discard all packets
return;
}
// STOP CONDITION
if ( datagram.size() && datagram[0]==(char)0 ) {
STATE = EMERG_STOP;
// output whatever is need for stop.
sendDefaults();
// Stop the network timer
NetworkTimer->stop();
emptyQueue();
ui->btnClearEmergencyStop->setEnabled(true);
ui->lblEmergencyStatus->setText("EMERGENCY STOP!");
QPalette red_text;
red_text.setColor(QPalette::WindowText, Qt::red);
ui->lblEmergencyStatus->setPalette(red_text);
ui->lblEmergencyStatus->setFrameStyle(QFrame::Box);
ui->lblStatus->setText("EMERGENCY STOP!");
return;
}
// Increment the received packet count
stats_packets++;
// check if valid packet
if (validPacket(datagram.constData())) {
queue.enqueue(datagram);
} else {
stats_invalid++;
}
}
// Update the Statistics
ui->lblPackets->setText(QString::number(stats_packets,10));
ui->lblInvalid->setText(QString::number(stats_invalid,10));
// Check if a network failure occured.
if (STATE == NET_FAILURE && !ui->chkStopOnNetworkFailure->isChecked()) {
STATE = LISTENING;
NetworkTimer->start( networkFailureTime );
}
ui->lblStatus->setText("Receiving Packets...");
}
// Output data to the ServoBoard
void NetworkWidget::ouputServoData()
{
if (STATE == NET_FAILURE || STATE == EMERG_STOP) {
emptyQueue();
if (!ui->chkDefaultsOnNetworkFail->isChecked()) {
return;
}
sendDefaults();
return;
}
if (STATE == COM_FAILURE || !this->serial || !this->serial->isOpen()){
// communications failure
STATE = COM_FAILURE;
// Should we try to get the COM port back?
return;
}
if (queue.isEmpty())
return;
// handle items that have been received from the network.
QByteArray message, data(2,0);
unsigned char pairs, current_servo;
int error; float p_term, d_term;
// Update the statistics on the Max Queue size
bool convert;
int max = ui->lblQueueMax->text().toInt(&convert, 10);
if (convert && max < queue.size())
ui->lblQueueMax->setText(QString::number(queue.size(), 10));
do {
message = queue.dequeue();
} while (message.size() < 1);
if (ui->chkSendMostRecent->isChecked()) {
// dump the queue
emptyQueue();
}
gain_p = 0.26f;
gain_d = 0.04f;
gain_i = 0.015f;
pairs = (message[0] * 2);
unsigned int i = 1;
while (i < pairs) {
current_servo = message[i] - 1; // Store the current servo number. (range of current_servo: 0-11)
data[0] = 144 + current_servo + 1; // Output to Servo Board. (range of data: 1-12)
if (ui->chkPIDcontrol->isChecked()) {
// Basic PID control
error = message[i+1] - servo[current_servo]; // error = new_value - old_value
p_term = gain_p * error;
servo_integral[current_servo] += gain_i * error;
d_term = gain_d * (error - servo_error[current_servo])/(outputTime/100);
servo[current_servo] = p_term + d_term + servo_integral[current_servo] + servo[current_servo];
servo_error[current_servo] = error;
// Maximum range exceeded?
if (servo[current_servo] > 97) servo[current_servo] = 97;
if (servo[current_servo] < 1) servo[current_servo] = 1;
data[1] = servo[current_servo];
} else {
// No control
data[1] = servo[current_servo] = message[i+1]; // servo value
}
// Update the Current value on the form.
currentServoValue[current_servo]->setText(QString::number(servo[current_servo],10));
// Send off to the Servo board.
if (this->serial->write( data ) == -1) {
STATE = COM_FAILURE;
return;
}
i += 2; // Go to the next servo pair
}
}
