/**
* \brief Tells the robot to switch to the specified command mode
* \detailed Stops almost all tasks, resets the LCD, stops the wheels and starts the tasks required to execute in the specified command mode.
* \param commandOption - an integer representing a command option
*/
void setCommandMode(int commandOption) {
// Suspend & delete tasks + clear their handles
for (int i = 1; i < 10; i++) {
if (taskHandles[i] != NULL) {
vTaskSuspend(taskHandles[i]);
vTaskDelete(taskHandles[i]);
taskHandles[i] = NULL;
}
}
// shut off center servomotor when any task ends
motion_servo_stop(MOTION_SERVO_CENTER);
// Stop moving and clear LCD
setMovementType(STOP_MOVEMENT);
display("Command", "incoming");
/* Set tasks depending on command --------------------------- */
// Attachment mode
if (commandOption == -2) {
currentCommand = ATTACHMENT;
xTaskCreate(attachmentMode, (const portCHAR *)"Attachment Mode", 256, NULL /* parameters */,3, &taskHandles[3]);
xTaskCreate(getTemperatureTask, (const portCHAR *)"Get temperature data", 90, NULL /* parameters */,2, &taskHandles[2]);
xTaskCreate(outputToLCD, (const portCHAR *)"Display on LCD", 170, NULL /* parameters */,1, &taskHandles[1]);
// Command mode
} else if (commandOption == -1) {
// stop everything by not adding tasks
currentCommand = COMMAND_MODE;
display("Command mode", "Wait for cmds.");
// Command mode - movement type
} else if (0 <= commandOption && commandOption <= 4) {
currentCommand = MOVE;
setMovementType(commandOption);
xTaskCreate(getWheelData, (const portCHAR *)"Get wheel data", 256, NULL /* parameters */,2, &taskHandles[2]);
xTaskCreate(outputToLCD, (const portCHAR *)"Display on LCD", 150, NULL /* parameters */,1, &taskHandles[1]);
// Scan and show temperatures
} else if (commandOption == 5) {
currentCommand = TEMPERATURE;
xTaskCreate(getTemperatureTask, (const portCHAR *)"Get temperature data", 90, NULL /* parameters */,2, &taskHandles[3]);
xTaskCreate(moveHead, (const portCHAR *)"Rotate head", 120, NULL /* parameters */,3, &taskHandles[2]);
xTaskCreate(outputToLCD, (const portCHAR *)"Display on LCD", 150, NULL /* parameters */,1, &taskHandles[1]);
// Scan and show distance
} else if (commandOption == 6) {
currentCommand = DISTANCE;
xTaskCreate(distanceTask, (const portCHAR *)"Get distance", 120, NULL /* parameters */,1, &taskHandles[3]);
xTaskCreate(moveHead, (const portCHAR *)"Rotate head", 120, NULL /* parameters */,3, &taskHandles[2]);
xTaskCreate(outputToLCD, (const portCHAR *)"Display on LCD", 150, NULL /* parameters */,2, &taskHandles[1]);
}
/* ==============================--------------------------- */
}
/**
* \brief Entering the robot into the "Attachement mode"
* \detailed In this mode, the robot will look for a source of high heat and will follow it. If the robot doesn't find a source
* after a period of time, it goes into a panic mode, which is turning around itself forever until it detects a heat source and follows it.
* \param pvParameters - pointer to parameters
*/
static void attachmentMode(void * pvParameters) {
TickType_t xLastWakeTime = xTaskGetTickCount();
uint8_t distance = 0, panicModeCounter = 0, panicModeEnabledCount = 250;
uint16_t currentPulseWidthTicks = INITIAL_PULSE_WIDTH_TICKS;
// Initialize as search mode
ATTACHMENT_MODE = SEARCH_MODE;
// Head needs to be facing forward
motion_servo_start(MOTION_SERVO_CENTER);
rotateHead(¤tPulseWidthTicks);
while (1) {
// If we have new temperature data
if(hasNewValues == 1){
hasNewValues = 0;
// the highest temperature sensed is greater than the trigger temperature
if ((uint8_t) TRIGGER_TEMPERATURE < temperatures[indexOfHighestTemperature]) {
// We are tracking a heat source!
ATTACHMENT_MODE = TRACKING_MODE;
// reset panic mode counter
panicModeCounter = 0;
// get the distance to the heat source
distance = getDistance();
// if the highest heat source is not in the center
if (indexOfHighestTemperature != 4 || indexOfHighestTemperature != 5) {
// turn towards the heat source
turnToAngle(45 - 10 * indexOfHighestTemperature);
}
// get as close to target as possible
moveToTarget(distance);
// no temperature is higher than the trigger temperature
// increment the panic mode counter and check if we have reached the panic mode enabled count
} else if (panicModeEnabledCount <= panicModeCounter) {
ATTACHMENT_MODE = PANIC_MODE;
setMovementType(ROTATE_RIGHT_MOVEMENT);
} else {
++panicModeCounter;
// Stop movement because we're no longer tracking a heat source
setMovementType(STOP_MOVEMENT);
// No longer in tracking or panic mode
ATTACHMENT_MODE = SEARCH_MODE;
}
}
vTaskDelayUntil(&xLastWakeTime, ((30000 / panicModeEnabledCount) / portTICK_PERIOD_MS));
}
}
bool QLCInputChannel::loadXML(QXmlStreamReader &root)
{
if (root.isStartElement() == false || root.name() != KXMLQLCInputChannel)
{
qWarning() << Q_FUNC_INFO << "Channel node not found";
return false;
}
while (root.readNextStartElement())
{
if (root.name() == KXMLQLCInputChannelName)
{
setName(root.readElementText());
}
else if (root.name() == KXMLQLCInputChannelType)
{
setType(stringToType(root.readElementText()));
}
else if (root.name() == KXMLQLCInputChannelExtraPress)
{
root.readElementText();
setSendExtraPress(true);
}
else if (root.name() == KXMLQLCInputChannelMovement)
{
if (root.attributes().hasAttribute(KXMLQLCInputChannelSensitivity))
setMovementSensitivity(root.attributes().value(KXMLQLCInputChannelSensitivity).toString().toInt());
if (root.readElementText() == KXMLQLCInputChannelRelative)
setMovementType(Relative);
}
else
{
qWarning() << Q_FUNC_INFO << "Unknown input channel tag" << root.name();
root.skipCurrentElement();
}
}
return true;
}
/**
* \brief Adjusts the wheel speeds based on their current speeds
* \detailed Using the last tick counts for each wheel, try to meet a tick count. Alter wheel speed if target tick count not reached.
* \param ticCountLeftWheel - the last left wheel tick count
* \param ticCountRightWheel - the last right wheel tick count
*/
void adjustWheelSpeeds(uint32_t ticCountLeftWheel, uint32_t ticCountRightWheel) {
// If the left wheel is 3% greater or less than than the target, adjust its speed
if (targetTick * 1.03 < ticCountLeftWheel) {
if (currentMovementType == FORWARD_MOVEMENT
|| currentMovementType == ROTATE_RIGHT_MOVEMENT) {
leftForwardWidth -= pulseWidthChange;
} else { // BACKWARD OR ROTATE LEFT
leftBackwardWidth += pulseWidthChange;
}
} else if (ticCountLeftWheel < targetTick * 0.97) {
if (currentMovementType == FORWARD_MOVEMENT
|| currentMovementType == ROTATE_RIGHT_MOVEMENT) {
leftForwardWidth += pulseWidthChange;
} else { // BACKWARD OR ROTATE LEFT
leftBackwardWidth -= pulseWidthChange;
}
}
// If the right wheel is 3% greater or less than than the target, adjust its speed
if (targetTick * 1.03 < ticCountRightWheel) {
if (currentMovementType == FORWARD_MOVEMENT
|| currentMovementType == ROTATE_LEFT_MOVEMENT) {
rightForwardWidth += pulseWidthChange;
} else { // BACKWARD OR ROTATE RIGHT
rightBackwardWidth -= pulseWidthChange;
}
} else if (ticCountRightWheel < targetTick * 0.97) {
if (currentMovementType == FORWARD_MOVEMENT
|| currentMovementType == ROTATE_LEFT_MOVEMENT) {
rightForwardWidth -= pulseWidthChange;
} else { // BACKWARD OR ROTATE RIGHT
rightBackwardWidth += pulseWidthChange;
}
}
// Will change wheel speeds
setMovementType(currentMovementType);
}
bool QLCInputChannel::loadXML(QXmlStreamReader &root)
{
if (root.isStartElement() == false || root.name() != KXMLQLCInputChannel)
{
qWarning() << Q_FUNC_INFO << "Channel node not found";
return false;
}
while (root.readNextStartElement())
{
if (root.name() == KXMLQLCInputChannelName)
{
setName(root.readElementText());
}
else if (root.name() == KXMLQLCInputChannelType)
{
setType(stringToType(root.readElementText()));
}
else if (root.name() == KXMLQLCInputChannelMovement)
{
if (root.attributes().hasAttribute(KXMLQLCInputChannelSensitivity))
#if QT_VERSION < QT_VERSION_CHECK(5, 0, 0)
setMovementSensitivity(root.attributes().value(KXMLQLCInputChannelSensitivity).toString().toInt());
#else
setMovementSensitivity(root.attributes().value(KXMLQLCInputChannelSensitivity).toInt());
#endif
if (root.readElementText() == KXMLQLCInputChannelRelative)
setMovementType(Relative);
}
else
{
qWarning() << Q_FUNC_INFO << "Unknown input channel tag" << root.name();
}
}
return true;
}
