void Arduilink::send(unsigned int _id, const char* _msg) {
SensorItem* sensor = getSensor(_id);
if (sensor == NULL) return; // TODO return error
if (sensor->writter != NULL)
sensor->writter(_msg);
// TODO else return warning
}
void TwoDModelWidget::reinitSensor(RobotItem *robotItem, const PortInfo &port)
{
robotItem->removeSensor(port);
RobotModel &robotModel = robotItem->robotModel();
const DeviceInfo &device = robotModel.configuration().type(port);
if (device.isNull() || (
/// @todo: Add supported by 2D model sensors here
!device.isA<TouchSensor>()
&& !device.isA<ColorSensor>()
&& !device.isA<LightSensor>()
&& !device.isA<RangeSensor>()
/// @todo For working with line sensor from TRIK. Actually this information shall be loaded from plugins.
&& !device.isA<VectorSensor>()
))
{
return;
}
SensorItem *sensor = device.isA<RangeSensor>()
? new SonarSensorItem(mModel.worldModel(), robotModel.configuration()
, port
, robotModel.info().sensorImagePath(device)
, robotModel.info().sensorImageRect(device)
)
: new SensorItem(robotModel.configuration()
, port
, robotModel.info().sensorImagePath(device)
, robotModel.info().sensorImageRect(device)
);
sensor->setEditable(!mSensorsReadOnly);
robotItem->addSensor(port, sensor);
}
RobotItem::RobotItem(const QString &robotImageFileName, model::RobotModel &robotModel)
: RotateItem()
, mImage(QImage(robotImageFileName))
, mBeepItem(new BeepItem)
, mRectangleImpl()
, mRobotModel(robotModel)
{
connect(&mRobotModel, &model::RobotModel::robotRided, this, &RobotItem::ride);
connect(&mRobotModel, &model::RobotModel::positionChanged, this, &RobotItem::setPos);
connect(&mRobotModel, &model::RobotModel::rotationChanged, this, &RobotItem::setRotation);
connect(&mRobotModel, &model::RobotModel::playingSoundChanged, this, &RobotItem::setNeededBeep);
connect(&mRobotModel.configuration(), &model::SensorsConfiguration::deviceRemoved, this, &RobotItem::removeSensor);
connect(&mRobotModel.configuration(), &model::SensorsConfiguration::positionChanged
, this, &RobotItem::updateSensorPosition);
connect(&mRobotModel.configuration(), &model::SensorsConfiguration::rotationChanged
, this, &RobotItem::updateSensorRotation);
setAcceptHoverEvents(true);
setAcceptDrops(true);
setCursor(QCursor(Qt::PointingHandCursor));
setZValue(1);
setX2(x1() + robotWidth);
setY2(y1() + robotHeight);
mMarkerPoint = QPointF(0, y2() / 2); // Marker is situated behind the robot
QPen pen(this->pen());
pen.setWidth(defaultTraceWidth);
setPen(pen);
setTransformOriginPoint(rotatePoint);
mBeepItem->setParentItem(this);
mBeepItem->setPos((robotWidth - beepWavesSize) / 2, (robotHeight - beepWavesSize) / 2);
mBeepItem->setVisible(false);
RotateItem::init();
QHash<kitBase::robotModel::PortInfo, kitBase::robotModel::DeviceInfo> sensors = robotModel.info().specialDevices();
for (const kitBase::robotModel::PortInfo &port : sensors.keys()) {
const kitBase::robotModel::DeviceInfo device = sensors[port];
SensorItem *sensorItem = new SensorItem(robotModel.configuration(), port
, robotModel.info().sensorImagePath(device), robotModel.info().sensorImageRect(device));
addSensor(port, sensorItem);
const QPair<QPoint, qreal> configuration(robotModel.info().specialDeviceConfiguration(port));
QPoint position(configuration.first.x() * boundingRect().width() / 2
, configuration.first.y() * boundingRect().height() / 2);
sensorItem->setPos(position + boundingRect().center());
sensorItem->setRotation(configuration.second);
}
}
void SensorWidget::drawChannels()
{
if(m_pGraphicsScene)
{
m_pGraphicsScene->clear();
for(qint32 i = 0; i < m_pSensorModel->rowCount(); ++i)
{
QString dispChName = m_pSensorModel->data(i, 0).toString();
QString fullChName = m_pSensorModel->data(i, 1).toString();
QPointF loc = m_pSensorModel->data(i, 2).toPointF();
qint32 chNum = m_pSensorModel->getNameIdMap()[fullChName];
SensorItem *item = new SensorItem(dispChName, chNum, loc, QSizeF(28, 16));
item->setSelected(m_pSensorModel->data(i, 3).toBool());
item->setPos(loc);
connect(item, &SensorItem::itemChanged, m_pSensorModel, &SensorModel::updateChannelState);
m_pGraphicsScene->addItem(item);
}
}
}
int Arduilink::handleInput() {
while (Serial.available() > 0) {
while (write) {}
write = true;
String str = Serial.readString();
char buf[str.length() + 1];
str.toCharArray(buf, str.length() + 1);
char* opcode = NULL;
int node = -1;
int sensorId = -1;
char* pch;
pch = strtok(buf, ";");
while (pch != NULL)
{
if (opcode == NULL) {
if (strcmp(pch, "PRESENT") == 0) {
printSensors();
write = false;
return 0;
}
if (strcmp(pch, "GET") != 0 && strcmp(pch, "INFO") != 0 && strcmp(pch, "SET") != 0) {
Serial.print("400;OPCODE;");
Serial.println(pch);
write = false;
return 2;
}
opcode = pch;
}
else if (node == -1) {
node = atoi(pch);
}
else if (sensorId == -1) {
sensorId = atoi(pch);
break;
}
pch = strtok(NULL, ";");
}
/*Serial.print("Opcode: ");
Serial.print(opcode);
Serial.print(" NodeId: ");
Serial.print(node);
Serial.print(" SensorId: ");
Serial.println(sensorId);*/
if (nodeId != node) {
Serial.print("404;NODE;");
Serial.println(node);
write = false;
return 3;
}
SensorItem* sensor = getSensor(sensorId);
if (sensor == NULL) {
Serial.print("404;SENSOR;");
Serial.println(sensorId);
write = false;
return 4;
}
// INFO - Récupérer la description d'un capteur
if (strcmp(opcode, "INFO") == 0) {
printSensor(sensor, node);
}
// GET - Récupérer la valeur d'un capteur
if (strcmp(opcode, "GET") == 0) {
char buff[256];
sprintf(buff, "200;%d;%d;", node, sensor->id);
Serial.print(buff);
Serial.println(sensor->value);
}
// SET - Modifier un attribut d'un capteur
else if (strcmp(opcode, "SET") == 0) {
// Choose attribute mode
pch = strtok(NULL, ";");
int mode = 0;
if (strcmp(pch, "VERBOSE") == 0) mode = 1;
else if (strcmp(pch, "VAL") == 0) mode = 2;
else {
Serial.print("400;ATTR;");
Serial.println(pch);
write = false;
return 5;
}
// Ack
pch = strtok(NULL, ";");
bool ack = strcmp(pch, "1") == 0;
// Value
pch = strtok(NULL, ";");
// Handle Set Verbose
if (mode == 1) {
if (strcmp(pch, "1") == 0) {
if (ack) {
Serial.println("201;VERBOSE;1");
Serial.flush();
}
sensor->verbose = true;
}
else if (strcmp(pch, "0") == 0) {
sensor->verbose = false;
if (ack) {
Serial.println("201;VERBOSE;0"); // TODO Ameliorer avec ids
Serial.flush();
}
}
else {
Serial.print("400;OPT;VERBOSE;"); // TODO Ameliorer avec ids
Serial.println(pch);
write = false;
return 6;
}
}
// Handle Set Value
else if (mode == 2) {
sensor->value = pch;
if (sensor->writter != NULL)
sensor->writter(pch);
if (ack) {
Serial.print("201;SET;");
Serial.print(node);
Serial.print(";");
Serial.print(sensor->id);
Serial.print(";VAL;");
Serial.println(pch);
}
}
}
Serial.flush();
write = false;
return 0;
}
return 1;
}
