// TODO at the moment, this only works well for NORTH, SOUTH, EAST, WEST
// We'll try to sort it out for NEAST, SEAST, NWEST, SWEST later.
void Robot::moveTillPoint(int motorSpeed, bool forward, bool useBothSensors)
{
wheelEnc.getCountsAndResetM1();
wheelEnc.getCountsAndResetM2();
//Moves forward or backward based on the boolean 'forward' until a line intersection.
//unsigned int sensors[8];
//gridSensors.readLine(sensors, QTR_EMITTERS_ON, true);
int error;
//bool atPoint = false;
while(!reachedPoint(useBothSensors)/*!atPoint*/)
{
/* gridSensors.readLine(sensors, QTR_EMITTERS_ON, true);
if(reachedPoint(sensors, useBothSensors))
{
delay(5);
gridSensors.readLine(sensors, QTR_EMITTERS_ON, true);
if(reachedPoint(sensors, useBothSensors))
atPoint = true;
}*/
error = errorAdjustment();
if (forward)
motorForward(motorSpeed, error);
else
motorBackward(motorSpeed, error);
}
if(!useBothSensors)
moveTicks(5,STRAIGHTSPEED);
}
void runNode(void *data) {
MoveNode *currNode = (MoveNode *)data;
MoveNode *currChild = currNode->child;
if (currNode->nodeId == NULL) {
return;
}
for (int i = 0; i < pairMap[currNode->nPairId].numSensors; i++) {
Sensor *sensor = &pairMap[currNode->nPairId].sensors[i];
// createSensor(sensor);
/*if (sensor->type == SHAFT_ENCODER) {
sensor->enc = encoderInit(sensor->port, sensor->port + 1, false);
}*/
startSensor(sensor);
sensor = NULL;
}
signed char *saveState = NULL;
printDebug("Started node.");
// printf("Started node %d.\n\r", currNode->nodeId);
// printf("Node's child: %d\n\r", currChild->nodeId);
int nextPoint = 0;
setMotorSpeeds(currNode, nextPoint);
nextPoint++;
while (nextPoint < currNode->numPoints) {
// printf("DEBUG: %d\n\r", nextPoint);
while (joystickGetDigital(1, 5, JOY_UP)) { // pause
if (saveState == NULL) {
saveState = malloc(pairMap[currNode->nPairId].numPorts * sizeof(*(saveState)));
if (saveState == NULL) {
return; // break completely
}
for (int i = 0; i < pairMap[currNode->nPairId].numPorts; i++) {
saveState[i] = motorGet(pairMap[currNode->nPairId].motorPorts[i]);
motorStop(pairMap[currNode->nPairId].motorPorts[i]);
}
for (int i = 0; i < pairMap[currNode->nPairId].numSensors; i++) {
if (pairMap[currNode->nPairId].sensors[i].type == TIME) {
pauseTimer(pairMap[currNode->nPairId].sensors[i].port, true);
}
}
printDebug("Paused!");
}
if (joystickGetDigital(1, 8, JOY_DOWN)) { // stop it entirely
printDebug("Stopping!");
for (int i = 0; i < pairMap[currNode->nPairId].numSensors; i++) {
if (pairMap[currNode->nPairId].sensors[i].type == TIME) {
resumeTimer(pairMap[currNode->nPairId].sensors[i].port, true);
}
}
free(saveState);
saveState = NULL;
return;
}
delay(20);
}
if (outOfMemory) {
return;
}
if (saveState != NULL) {
for (int i = 0; i < pairMap[currNode->nPairId].numPorts; i++) {
motorSet(pairMap[currNode->nPairId].motorPorts[i], saveState[i]);
}
for (int i = 0; i < pairMap[currNode->nPairId].numSensors; i++) {
if (pairMap[currNode->nPairId].sensors[i].type == TIME) {
resumeTimer(pairMap[currNode->nPairId].sensors[i].port, true);
}
}
free(saveState);
saveState = NULL;
}
if (reachedPoint(currNode, currNode->points[nextPoint].endSensorVal, currNode->points[nextPoint - 1].endSensorVal)) {
setMotorSpeeds(currNode, nextPoint);
nextPoint++;
}
if (currChild != NULL) {
// printf("DEBUG: %d %d %d\n\r", currChild->nodeId, nextPoint, currChild->startPoint);
if (nextPoint + 1 >= currChild->startPoint && needToStart(currNode, currChild)) {
void *param = (void *)currChild;
taskCreate(runNode, TASK_DEFAULT_STACK_SIZE / 2, param, TASK_PRIORITY_DEFAULT);
currChild = currChild->sibling;
}
}
delay(5);
}
printDebug("Finished node.");
// printf("Finished node %d.", currNode->nodeId);
if (findParent(currNode)->nodeId == rootNode->nodeId) {
while (inMotion()) {
delay(20);
}
if (currNode->sibling != NULL) {
delay(currNode->sibling->startVal[0]);
runNode(currNode->sibling);
} else {
printDebug("Done.");
delay(500);
}
}
}
