GT_STATUS sampleTest()
{
GT_U8 macAddr[6] = {0x0,0x23,0x45,0x67,0x89,0xab};
qdMultiDevStart();
enableTag();
sampleIsolatedCPUPort(qdMultiDev[0],macAddr);
crossChipTrunkSetup();
return GT_OK;
}
void handleSpecialEvents(short* enc){
static uint8_t oldTagValue = None;
static short elapsedEnc[2] = {0,0};
uint8_t resetAdjustSpeed = 0;
//Check for Error
if( tagValue & Error ){
//Report Error, Clear Flag, and Do Nothing
printw_err("Invalid RFID Tag!\n");
disableTag(Error);
lineFound = FALSE;
return;
}
//Depending on tag values adjust motor speed
if( tagValue != oldTagValue ){
lineFound = FALSE;
//Do first occurence actions
uint8_t diff = tagValue & ~oldTagValue;
//printw("diff = %X\n",diff);
if( diff & SpeedUp){
//Disable SPD DOWN
disableTag(SlowDown);
resetAdjustSpeed = 1;
}
if( diff & SlowDown ){
//Disable SPD UP
disableTag(SpeedUp);
resetAdjustSpeed = 1;
}
if( diff & GoLeft ){
//Disable RIGHT
disableTag(GoRight);
elapsedEnc[0] = 0;
elapsedEnc[1] = 0;
}
if( diff & GoRight ){
//Disable LEFT
disableTag(GoLeft);
elapsedEnc[0] = 0;
elapsedEnc[1] = 0;
}
if( diff & EndZone){
disableTag(SpeedUp);
disableTag(SlowDown);
disableTag(EndZone);
}
}
if( tagValue & SpeedUp ){ //SPD UP
setState("Go Fast");
//adjust to target
adjustSpeed(oldEnc[0], oldEnc[1], MAX_SPEED, resetAdjustSpeed);
}
if( tagValue & SlowDown ){ //SPD DOWN
setState("Go Slow");
//adjust to target
adjustSpeed(oldEnc[0], oldEnc[1], MIN_SPEED, resetAdjustSpeed);
}
if( tagValue & GoLeft ){ //LEFT
setState("Go Left");
//Update Encoder counts
elapsedEnc[0] += enc[0];
elapsedEnc[1] += enc[1];
//Check angle
float angle = enc2Ang(elapsedEnc[0], elapsedEnc[1]);
switch( doTurn(angle,MIN_TURN_ANGLE,MAX_TURN_ANGLE) ){
case 0: //Continue
setMotorData(&motorData,32,96); //Half Speed 0-Radius Left Turn
break;
case 1: //Done, Proceed
disableTag(GoLeft);
break;
case 2: //Done, Blocked
disableTag(GoLeft);
enableTag(GoRight);
//TODO: Handle stuck at corner situation
break;
}
}
if( tagValue & GoRight ){ //RIGHT
setState("Go Right");
//Update Encoder counts
elapsedEnc[0] += enc[0];
elapsedEnc[1] += enc[1];
//Check angle
float angle = -enc2Ang(elapsedEnc[0], elapsedEnc[1]);
switch( doTurn(angle,MIN_TURN_ANGLE,MAX_TURN_ANGLE) ){
case 0: //Continue
setMotorData(&motorData,96,32); //Half Speed 0-Radius Right Turn
break;
case 1: //Done, Proceed
disableTag(GoRight);
break;
case 2: //Done, Blocked
disableTag(GoRight);
enableTag(GoLeft);
//TODO: Handle stuck at corner situation
break;
}
}
if( tagValue & Finish ){ //FIN -- AT END TO ENSURE STOPPING IN EVENT OF OTHER TAGS
//Stop if found line
setMotorData(&motorData,64,64);
//else slow down?
}
oldTagValue = tagValue;
return;
}
