/** * \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; }