void placeBlockOnIr(DriveSys t, gyroSys g, ConvSys s, bool isRightPath, tSensors IR){
if(isRightPath){
if(HTIRS2readDCDir(IR) == 5){
turnToAngle(t, g, 90.0, 50);
//updateConvSys(s, 0, 100);
turnToAngle(t, g, -90.0, 50);
}
} else {
if(HTIRS2readDCDir(IR) == 5){
turnToAngle(t, g, -90.0, 50);
//conveyor system
turnToAngle(t, g, 90.0, 50);
}
}
}
task main(){
calibrateLight();
calibrateCompass();
ExploreStack stack;
Graph graph;
initStack(stack);
initGraph(graph);
Stub stub;
Edge lastEdge;
int currentNode = -1;
int canNode = -1;
//go to node
currentNode = exploreNewNode(graph, stack, currentNode, lastEdge);
if(detectCan()) {
canNode = currentNode;
announceCan(currentNode);
}
//while not empty
while(!empty(stack)){
pop(stack, stub);
motor[left] = 0;
motor[right] = 0;
goToNode(graph, currentNode, stub.node);
currentNode = stub.node;
turnToAngle(stub.angle, 15);
turnToLine();
nxtDisplayCenteredTextLine(4, "Exploring...");
int stackSize = stack.top;
FollowSegmentTilEnd(lastEdge);
currentNode = exploreNewNode(graph, stack, currentNode, lastEdge);
if(stackSize == stack.top && detectCan()) {
canNode = currentNode;
announceCan(currentNode);
}
}
motor[left] = 0;
motor[right] = 0;
eraseDisplay();
nxtDisplayCenteredTextLine(3, "%i nodes found", graph.nNodes);
wait10Msec(500);
if(canNode >= 0) {
goToNode(graph, currentNode, canNode);
motor[left] = 0;
motor[right] = 0;
eraseDisplay();
nxtDisplayCenteredTextLine(3, "Can was here");
wait10Msec(500);
} else {
motor[left] = 0;
motor[right] = 0;
eraseDisplay();
PlaySound(soundLowBuzz);
nxtDisplayCenteredTextLine(3, "There was no can");
wait10Msec(500);
}
}
task main()
{
waitForStart();
// raiseBucket();
int IRState; //Gets value from the IR
//StartTask (Play);
StartTask (update_gyroSensor);
goStraightForTime(4000, 40);
goStraightForTime(500, 20);
goStraightForTime(500, 10);
driveUntilIRNot(5, 40);//Adjust later
IRState = readIR();
driveUntilIR(0, 40);
driveUntilBumper(15);//Adjust later
if(IRState == 4) // if IR is 4 then Turn
{
turnToAngle( -40, -60, -90);
}
else
{
turnToAngle( 40, -60, 90);
}
if(IRState > 5)
{
driveUntilIR(1, 40);
turnToAngle(-40, -0, 0);
driveUntilBumper(15);
}
else if(IRState < 5)
{
driveUntilIR(9, 40);
turnToAngle(40, 0, 0);
driveUntilBumper(15);
}
else
{
driveUntilIR(0,40);
}
//turnToAngle(40, 60, 90);
//driveUntilIR();
}
/**
* \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));
}
}
/**
* Turns around until black is found
*/
void turnToLine(void) {
spinInDirection();
while(!isDark()) {
abortTimeslice();
}
int startDir = currentDirection();
while(isDark()) {
abortTimeslice();
}
motor[left] = 0;
motor[right] = 0;
int centre = startDir + (angleDifference(currentDirection(), startDir)/2);
nxtDisplayCenteredTextLine(6, "cen: %i", centre);
turnToAngle(centre, 0);
}