Beh* behLight(Beh* lightPtr, int32 light_value[],int ErrorlightPtr[],boolean *TaskDone, boolean *IntegralWindup) {
// this function compute control law (tv, rv) to move robot toward light and sets TaskDone to 1 if the robot is at the light
boolean done = FALSE;
//assumption is fl is 0, fr is 1
// commented out for adding controller
//if (abs(diff) < 10)
//sensor_offset = -diff;
//else
//sensor_offset = 0;
// end comment
int32 tv;
int32 rv;
//static int32 integralError = 0;
int32 diff;
if ((abs(light_value[0]) < 1200) && (abs(light_value[1]) < 1200 ) && (abs(light_value[2]) < 1200 ) && (abs(light_value[3]) < 1200 )) {
//ledsSetPattern(LED_BLUE, LED_PATTERN_ON, LED_BRIGHTNESS_LOW, LED_RATE_MED);
//uint32 deltaTicks = encoder_delta_ticks(currentTime, prevTime);
//if (max ( light_value[2] , light_value[3] ) > max(light_value[1],light_value[0])){
//if ((light_value[2]> light_value[1]) || (light_value[3],light_value[0])){
if ( min(light_value[2],light_value[3]) > (100 * max (light_value[1],light_value[0]) /100)){
//if (light_value[2]>min(light_value[2],light_value[3]) ){
// diff = (max ( light_value[2] , light_value[3] ) - max(light_value[1],light_value[0]));
diff = MILLIRAD_PI /4;}
else{
diff = light_value[1] - light_value[0];}
//behPIController(lightPtr, diff, ErrorlightPtr,IntegralWindup);
behPController(lightPtr, diff);
//behPIDController(lightPtr, diff,ErrorlightPtr, IntegralWindup);
// #### commented out for adding controller
//lightPtr->tv = SPEED;
//lightPtr->rv = (sensor_offset + diff) * K;
// #### end comment
*TaskDone = 0;
}
else {
ledsSetPattern(LED_GREEN, LED_PATTERN_ON, LED_BRIGHTNESS_LOW, LED_RATE_MED);
*TaskDone =1;
lightPtr->tv = 0;
lightPtr->rv = 0;
}
return lightPtr;
}
void task2(void* parameters)
{
int count = 0;
while(TRUE)
{
if(count < 10)
{
serial_send_string_mutex((char*) parameters);
++count;
}
ledsSetPattern(LED_GREEN, LED_PATTERN_CIRCLE, LED_BRIGHTNESS_MED, LED_RATE_MED);
}
}
void behaviorTask(void* parameters) {
/******** Variables *********/
uint32 lastWakeTime = osTaskGetTickCount();
uint32 neighborRound = 0;
uint8 manipulationRobotsCounter = 0;
/******** Variables Declared by JJF ********/
//BroadcastMessage broadcastMessage;
BroadcastMessage treeArray[MRM_ROBOT_COUNT_MAX]; // changed type
nbrDataRobotList robotList[MRM_ROBOT_COUNT_MAX];
/******** Initializations ********/
radioCommandSetSubnet(2);
neighborsInit(NEIGHBOR_ROUND_PERIOD);
//TODO yo uonly need to call this function once
irCommsSetXmitPower(MRM_IR_XMIT_COMMS_XMIT_POWER);
systemPrintStartup();
// make a robot list to sort all the robots
robotListCreate(robotList, MRM_ROBOT_COUNT_MAX); // generate a robot list
/* create all the trees, and set them to not be sources */
for (i = 0; i < MRM_ROBOT_COUNT_MAX; ++i) {
broadcastMsgCreate(&treeArray[i], MRM_TREE_MAX_HOP); // generate a broadcast tree
//broadcastMsgSetSource(&tree_array[i], FALSE); // I'm not the course yet myself as the source of the broadcastMessage
}
// int i; // used as loop counter
// for (i = 1; i < MAX_ROBOT_NUM; i++) {
// broadcastMsgCreate(&tree_array[i], maxHop)
// }
/* add one manipultaion robot for ourself */
manipulationRobotsCounter = 1;
/******** Behavior **************/
for (;;) {
if (rprintfIsHost()) {
ledsSetPattern(LED_BLUE, LED_PATTERN_CIRCLE, LED_BRIGHTNESS_LOW, LED_RATE_MED);
continue;
} //end if host
else { // infinite loop starts from here
BroadcastMessage tempTree;
boolean treeExist = FALSE;
NbrList nbrList;
neighborsGetMutex();
printNow = neighborsNewRoundCheck(&neighborRound);
//cprintf("print %d \n",printNow);
nbrListCreate(&nbrList);
uint8 i;
for (i = 0; i < manipulationRobotsCounter; i++){
broadcastMsgUpdate(&treeArray[i], &nbrList);
}
//nbrListPrintHops(&nbrList, &broadcastMessage, "OMG");
// uint8 tempSourceID = broadcastMessage.msgSourceID.value;
// uint8 tempMsgHops = broadcastMessage.msgHops.value;
// uint8 tempSenderID = broadcastMessage.senderID;
//
// treeUpdate(&tree_array[0], tempSourceID,
// tempSenderID, tempMsgHops); // update my tree, it will be at the top of the array initially
// // create a data wrapper that contains my broadcastMsg
// nbrDataBroadcastMsgCreate(&broadcastMsgData, "broadcastMsg");
for (i = 0; i < nbrListGetSize(&nbrList); i++) {
nbrPtr = nbrListGetNbr(&nbrList, i);
if (nbrPtr != NULL) {
int j;
uint8 tempSourceID;
uint8 tempMsgHops;
uint8 tempSenderID;
for (j = 0; j < manipulationRobotsCounter; j++) {
if (treeArray[j].msgSourceID.value == roneID) {
tempSourceID = broadcastMsgGetSourceIDNbr(&treeArray[j], nbrPtr);
tempMsgHops = broadcastMsgGetHopsNbr(&treeArray[j], nbrPtr);
tempSenderID = broadcastMsgGetSenderIDNbr(&treeArray[j], nbrPtr);
cprintf("In my message, my sourceID is %d, MsgHops is %d, SenderID is %d, tempSourceID is %d, tempMsgHop"
"s is %d, tempSenderID is %d.\n",
treeArray[j].msgSourceID.value, treeArray[j].msgHops.value, treeArray[j].senderID, tempSourceID, tempMsgHops, tempSenderID);
break;
}
}
if ( tempSourceID != 0) {
// /* Update my broadcastMsg with tempSouceId, tempMsgHops, and tempSenderID */
// broadcastMessage.senderID = tempSenderID;
// broadcastMessage.msgSourceID.value = tempSourceID;
// broadcastMessage.msgHops.value = tempMsgHops;
// if (tempSourceID == roneID)
// broadcastMessage.isSource == TRUE;
// else
// broadcastMessage.isSource == FALSE;
//
// broadcastMsgUpdate(&broadcastMessage, &nbrList);
for (j = 0; j < manipulationRobotsCounter; j++) {
if (treeArray[j].msgSourceID.value == tempSourceID) {
broadcastMsgUpdate(&treeArray[j], &nbrList);
//treeUpdate(&tree_array[j], broadcastMessage.msgSourceID.value, broadcastMessage.senderID, broadcastMessage.msgHops.value);
treeExist = TRUE;
break;
}
}
if (!treeExist) {
cprintf("I am making a tree %d.\n", manipulationRobotsCounter);
cprintf("after making a treebb, my sourceID is %d, senderID is %d, hopNum is %d.\n", treeArray[manipulationRobotsCounter].msgSourceID.value, treeArray[manipulationRobotsCounter].senderID, treeArray[manipulationRobotsCounter].msgHops.value);
cprintf("msgUpdate nbrlist's length %d\n", nbrListGetSize(&nbrList));
cprintf("robot no.%d Num hops: %d\n", treeArray[manipulationRobotsCounter-1].msgSourceID.value, treeArray[manipulationRobotsCounter-1].msgHops.value);
broadcastMsgUpdate(&treeArray[manipulationRobotsCounter], &nbrList);
cprintf("after making a tree, my sourceID is %d, senderID is %d, hopNum is %d.\n", treeArray[manipulationRobotsCounter].msgSourceID.value, treeArray[manipulationRobotsCounter].senderID, treeArray[manipulationRobotsCounter].msgHops.value);
//treeUpdate(&tree_array[robot_counter], broadcastMessage.msgSourceID.value, broadcastMessage.senderID, broadcastMessage.msgHops.value);
// sort the tree array;
int loop_counter = manipulationRobotsCounter;
while(loop_counter > 0) {
if (treeArray[loop_counter].msgSourceID.value < treeArray[loop_counter-1].msgSourceID.value) {
tempTree = treeArray[loop_counter];
treeArray[loop_counter] = treeArray[loop_counter-1];
treeArray[loop_counter-1] = tempTree;
}
loop_counter--;
}
manipulationRobotsCounter++; // we increment robot_counter because we've updated a new tree in array
} else {
treeExist = FALSE;
}
}
}
}
printTreeArray(&treeArray, manipulationRobotsCounter);
///////////////////////////////////////////////////////////
neighborsPutMutex(); // commented
osTaskDelayUntil(&lastWakeTime, BEHAVIOR_TASK_PERIOD);
} // END of else , if the robot is not Host
} //forever for loop
} //behaviorTask()
void behaviorTask(void* parameters) {
//rprintfSetSleepTime(500);
uint32 lastWakeTime = osTaskGetTickCount();
uint8 navigationMode = MODE_IDLE;
Beh behOutput;
boolean printNow;
uint32 neighborRound = 0;
NbrList nbrList;
int i;
Nbr* nbrPtr;
BroadcastMessage broadcastMessage;
broadcastMsgCreate(&broadcastMessage, 20);
systemPrintStartup();
systemPrintMemUsage();
neighborsInit(NEIGHBOR_ROUND_PERIOD);
radioCommandSetSubnet(1);
NbrData guessWeight;
NbrData guessX_1,guessX_2,guessX_3,guessX_4;
NbrData guessY_1,guessY_2,guessY_3,guessY_4;
nbrDataCreate(&(guessWeight), "guessWeight", 8, 0);
nbrDataCreate32(&guessX_1,&guessX_2,&guessX_3,&guessX_4, "guessX_1", "guessX_2","guessX_3","guessX_4",0); //in milliradians
nbrDataCreate32(&guessY_1,&guessY_2,&guessY_3,&guessY_4, "guessY_1", "guessY_2","guessY_3","guessY_4",0); //in millimeters
uint32 x,y, xprime, yprime, nbrOrient,nbrBear, nbrWeight, weightTot, xtot, ytot, xave, yave;
uint8 roundWeight;
//uint16 IRXmitPower = IR_COMMS_POWER_MAX/4;
//GlobalRobotList globalRobotListPtr;
//globalRobotListCreate(&globalRobotListPtr);
for (;;) {
if (rprintfIsHost()) {
ledsSetPattern(LED_BLUE, LED_PATTERN_CIRCLE, LED_BRIGHTNESS_LOW, LED_RATE_MED);
continue;
}else{
/*** INIT STUFF ***/
behOutput = behInactive;
neighborsGetMutex();
printNow = neighborsNewRoundCheck(&neighborRound);
//irCommsSetXmitPower(IRXmitPower);
nbrListCreate(&nbrList);
broadcastMsgUpdate(&broadcastMessage, &nbrList);
//globalRobotListUpdate(&globalRobotListPtr, &nbrList);
if(printNow){
// globalRobotListPrintAllTree(&globalRobotListPtr, &nbrList);
}
/*** READ BUTTONS ***/
if (buttonsGet(BUTTON_RED)) {
navigationMode = GUESSCOM;
}else if (buttonsGet(BUTTON_GREEN)) {
} else if (buttonsGet(BUTTON_BLUE)) {
}
/** STATES MACHINE **/
switch (navigationMode) {
case GUESSCOM: {
ledsSetPattern(LED_RED, LED_PATTERN_CIRCLE, LED_BRIGHTNESS_LOW, LED_RATE_MED);
break;
}
case MODE_IDLE:
default: {
ledsSetPattern(LED_GREEN, LED_PATTERN_CIRCLE, LED_BRIGHTNESS_LOW, LED_RATE_MED);
break;
}
}
if(navigationMode == GUESSCOM){
roundWeight = 0;
xtot = 0;
ytot = 0;
weightTot = 0;
for (i = 0; i < nbrListGetSize(&nbrList); i++) {
nbrPtr = nbrListGetNbr(&nbrList, i);
nbrOrient = nbrGetOrientation(nbrPtr) + PI;
nbrBear = nbrGetBearing(nbrPtr) + PI;
x = nbrDataGetNbr32(&guessX_1,&guessX_2,&guessX_3,&guessX_4,nbrPtr);
y = nbrDataGetNbr32(&guessY_1,&guessY_2,&guessY_3,&guessY_4,nbrPtr);
if(printNow){
cprintf("NBR: B%d O%d X%d Y%d ",nbrOrient,nbrOrient,x,y);
}
xprime = x*cosMilliRad(nbrOrient)/1000 - y*sinMilliRad(nbrOrient)/1000;
yprime = x*sinMilliRad(nbrOrient)/1000 + y*cosMilliRad(nbrOrient)/1000;
x = xprime;
y = yprime + nbrEdgeDis;
if(printNow){
cprintf("MATH: X'%d Y'%d X''%d Y''%d ",xprime,xprime,x,y);
}
xprime = x*cosMilliRad(nbrBear)/1000 - y*sinMilliRad(nbrBear)/1000;
yprime = x*sinMilliRad(nbrBear)/1000 + y*cosMilliRad(nbrBear)/1000;
if(printNow){
cprintf("X'''%d Y'''%d\n",xprime,xprime);
}
nbrWeight = nbrDataGetNbr(&guessWeight,nbrPtr);
weightTot += nbrWeight;
xtot += nbrWeight * xprime;
ytot += nbrWeight * yprime;
roundWeight++;
}
xave = xtot / weightTot;
yave = ytot / weightTot;
nbrDataSet32(&guessX_1,&guessX_2,&guessX_3,&guessX_4,xave);
nbrDataSet32(&guessY_1,&guessY_2,&guessY_3,&guessY_4,yave);
nbrDataSet(&guessWeight,roundWeight);
if(printNow){
cprintf("Guess: X %d Y%d Weight %d\n",xave,yave,weightTot);
}
}
/*** FINAL STUFF ***/
motorSetBeh(&behOutput);
neighborsPutMutex();
// delay until the next behavior period
osTaskDelayUntil(&lastWakeTime, BEHAVIOR_TASK_PERIOD);
lastWakeTime = osTaskGetTickCount();
}//end not host
} // end for loop
} //end behavior function
void
behaviorTask(void* parameters)
{
int x, y;
int i, dead;
int state = STATE_IDLE;
NbrList nbrList;
uint32 c = 0;
uint32 neighborsCheck;
uint8 buttonRed, buttonGreen, buttonBlue;
int16 bearing, orientation, newRV;
int32 tv, rv;
int32 tvR, rvR = 0;
boolean foundObj = 0;
Beh behU = behInactive;
uint32 lastWakeTime = osTaskGetTickCount();
char* RXmsg;
radioCommandSetSubnet(2);
/* Set LEDs to idle state pattern */
ledsSetPattern(LED_GREEN,
LED_PATTERN_PULSE, LED_BRIGHTNESS_MED, LED_RATE_MED);
/* Neighbor data sets for TV and RV */
NbrData moving;
/* Initialize nbrs and data */
neighborsInit(200);
nbrDataCreate(&moving, "moving", 8, 0);
for (;;) {
/* Receive user input and set state */
buttonRed = buttonsGet(BUTTON_RED);
buttonGreen = buttonsGet(BUTTON_GREEN);
buttonBlue = buttonsGet(BUTTON_BLUE);
if (buttonRed) {
state = STATE_RECIEVE;
ledsSetPattern(LED_RED,
LED_PATTERN_PULSE, LED_BRIGHTNESS_MED, LED_RATE_MED);
} else if (buttonBlue) {
dead = 0;
state = STATE_SEND;
ledsSetPattern(LED_BLUE,
LED_PATTERN_PULSE, LED_BRIGHTNESS_MED, LED_RATE_MED);
} else if (buttonGreen) {
state = STATE_IDLE;
ledsSetPattern(LED_GREEN,
LED_PATTERN_PULSE, LED_BRIGHTNESS_MED, LED_RATE_MED);
}
neighborsGetMutex();
switch (state) {
/* Receiving data from traveling robot */
case STATE_RECIEVE: {
nbrDataSet(&moving, STATE_RECIEVE);
/* Only print on a new nbr round */
if (neighborsNewRoundCheck(&neighborsCheck)) {
nbrListCreate(&nbrList);
if(radioCommandReceive(&radioCmdRemoteControl, &radioMessageRX, 0))
RXmsg = radioCommandGetDataPtr(&radioMessageRX);
for (i = 0; i < nbrList.size; i++) {
if (nbrDataGetNbr(&moving, nbrList.nbrs[i]) == STATE_SEND) {
/* cprintf("%d,%d,%s",
nbrList.nbrs[i]->orientation,
nbrList.nbrs[i]->bearing,
RXmsg); */
rprintf("%d,%d,%s",
nbrList.nbrs[i]->orientation,
nbrList.nbrs[i]->bearing,
RXmsg);
c += 1;
}
}
}
break;
}
/* Drive in a U shaped path around neighbor */
case STATE_SEND: {
nbrDataSet(&moving, STATE_SEND);
nbrListCreate(&nbrList);
/*if (nbrList.size > 0) {
bearing = nbrList.nbrs[0]->bearing;
// If we are still driving towards the nbr
if (abs(bearing) < PI2) {
behSetTvRv(&behU, TV_SPEED, 0);
//Orbiting the nbr
} else {
bearing -= PI2;
if (bearing < 0) {
newRV = (bearing - PI2) / 1.5;
behSetTvRv(&behU, TV_SPEED, newRV);
} else {
newRV = (bearing + PI2) / 1.5;
behSetTvRv(&behU, TV_SPEED, newRV);
}
if (abs(newRV) < 200)
behSetTvRv(&behU, TV_SPEED, 0);
}
// If we have completed a half circle, just go forward
orientation = nbrList.nbrs[0]->orientation;
if (orientation > 2900)
behSetTvRv(&behU, TV_SPEED, 0);
// Go to idle if we lose the nbr
} else {
dead++;
if (dead > 5) {
state = STATE_IDLE;
ledsSetPattern(LED_GREEN,
LED_PATTERN_PULSE, LED_BRIGHTNESS_MED, LED_RATE_MED);
break;
}
}*/
if(irObstaclesGetBearing() || foundObj){
foundObj = 1;
behWallMove(&behU,TV_SPEED, WALL_FOLLOW_SIDE);
} else{
behSetTvRv(&behU,TV_SPEED,0);
}
/* Transmit tv and rv */
motorGetTVRV(&tv, &rv);
if (neighborsNewRoundCheck(&neighborsCheck)) {
//cprintf("%d,%d\n", tv, rv);
rprintf("%d,%d\n", x++, y++);
x = x % 198;
y = y % 173;
}
//sprintf(radioMessageTX.command.data, "%ld,%ld\n", tv, rv);
//radioCommandXmit(&radioCmdRemoteControl, ROBOT_ID_ALL, &radioMessageTX);
break;
}
/* Idle until active */
case STATE_IDLE:
default: {
foundObj == 0;
nbrDataSet(&moving, STATE_IDLE);
behU = behInactive;
break;
}
}
neighborsPutMutex();
motorSetBeh(&behU);
osTaskDelayUntil(&lastWakeTime, BEHAVIOR_TASK_PERIOD);
}
}
// behaviors run every 50ms. They should be designed to be short, and terminate quickly.
// they are used for robot control. Watch this space for a simple behavior abstraction
// to appear.
//
void behaviorTask(void* parameters)
{
/******** Variables *********/
uint32 lastWakeTime = osTaskGetTickCount();
uint32 neighborRoundPrev = 0;
uint32 neighborRound = 0;
uint32 alpha = 0;
uint8 i = 0;
uint16 IRXmitPower = IR_COMMS_POWER_MAX;
boolean newSensorData;
int32 tv, rv;
uint16 leader = 0;
uint16 mode = INACTIVE;
uint16 nbrMode = INACTIVE;
uint16 nbrLeader = 0;
uint16 wait = 0;
uint32 rotateNum = 0;
uint32 translateNum = 0;
NbrData msgLeader;
NbrData msgBearing;
NbrData msgMode;
NbrList nbrList;
Nbr* nbrPtr;
Nbr* leaderPtr;
nbrDataCreate(&msgLeader, "leader", 1, 0);
nbrDataCreate(&msgBearing, "bearing", 7, 0);
nbrDataCreate(&msgMode, "mode", 2, 0);
Beh behOutput;
/******** Initializations ********/
radioCommandSetSubnet(1);
neighborsInit(NEIGHBOR_ROUND_PERIOD);
broadcastMsgCreate(&broadcastMessage, 20);
systemPrintStartup();
/******** Behavior **************/
for (;;)
{
behOutput = behInactive;
neighborsGetMutex();
printNow = neighborsNewRoundCheck(&neighborRound);
cprintf("print %d \n",printNow);
irCommsSetXmitPower(IRXmitPower);
nbrListCreate(&nbrList);
broadcastMsgUpdate(&broadcastMessage, &nbrList);
//Check Buttons, won't start until leader selected
if (buttonsGet(BUTTON_RED)==1)
{
leader = 1;
mode = TRANSLATE;
}
// Checks Green button to stop behavior
if (buttonsGet(BUTTON_GREEN)==1)
{
leader = 1;
mode = ROTATE;
}
if (buttonsGet(BUTTON_BLUE)==1)
{
leader = 1;
mode = LEADERSTOP;
}
translateNum = 0;
rotateNum = 0;
uint8 foundLeader = 0;
uint32 interruptNum = 0;
if (leader ==1){
//do nothing
} else {
//check for mode change
for (i = 0; i < nbrList.size; ++i){
nbrPtr = nbrList.nbrs[i];
nbrMode = nbrDataGetNbr(&msgMode, nbrPtr);
nbrLeader = nbrDataGetNbr(&msgLeader, nbrPtr);
if (nbrLeader == 1){
foundLeader = 1;
mode = nbrMode;
leaderPtr = nbrPtr;
} else {
if (nbrMode != 0){
if(nbrMode == 1){
translateNum = translateNum +1;
} else if (nbrMode == 2){
rotateNum = rotateNum + 1;
} else if (nbrMode == 3){
interruptNum = interruptNum + 1;
}
}
}
}
if (foundLeader != 1){
if(translateNum > rotateNum && translateNum > interruptNum){
mode = TRANSLATE;
} else if (rotateNum > translateNum && rotateNum > interruptNum){
mode = ROTATE;
} else if (rotateNum == 0 && translateNum == 0 && interruptNum == 0){
//dont change from previous mode
} else if (interruptNum > translateNum && interruptNum > rotateNum){
mode = LEADERSTOP;
} else {
mode= TRANSLATE;
}
}
}
nbrDataSet(&msgLeader, leader);
nbrDataSet(&msgMode, mode);
if(printNow){
cprintf("found? %d leader %d, mode %d \n",foundLeader,leader, mode);
}
switch(mode){
case INACTIVE: {
behOutput = behInactive;
ledsSetPattern(LED_RED, LED_PATTERN_CIRCLE, LED_BRIGHTNESS_MED, LED_RATE_MED);
break;
}
case LEADERSTOP: {
behOutput = behInactive;
ledsSetPattern(LED_RED, LED_PATTERN_PULSE, LED_BRIGHTNESS_MED, LED_RATE_MED);
break;
}
case TRANSLATE: {
ledsSetPattern(LED_GREEN, LED_PATTERN_PULSE, LED_BRIGHTNESS_MED, LED_RATE_MED);
if(leader){
//move forward
behSetTv(&behOutput,50);
} else {
alpha = behFlockAngle(&nbrList);
if (abs(alpha) < 95){
wait = wait + 1;
behFlock(&behOutput, &nbrList, 50);
//if (wait > 10) {
// behFlock(&behOutput, &nbrList, 50);
//} else {
// behFlock(&behOutput, &nbrList, 0);
//}
} else {
wait = 0;
behFlock(&behOutput, &nbrList, 50);
}
}
break;
}
case ROTATE: {
ledsSetPattern(LED_BLUE, LED_PATTERN_PULSE, LED_BRIGHTNESS_MED, LED_RATE_MED);
if(leader){
behOutput = behInactive;
} else {
behFlockNormalToLeader(&behOutput,&nbrList,50);
//behOrbit(&behOutput,&nbrList,50);
}
break;
}
}//end mode switch
motorSetBeh(&behOutput);
neighborsPutMutex();
osTaskDelayUntil(&lastWakeTime, BEHAVIOR_TASK_PERIOD);
}//forever for loop
}//behaviorTask()
// behaviors run every 50ms. They should be designed to be short, and terminate quickly.
// they are used for robot control. Watch this space for a simple behavior abstraction
// to appear.
//
void behaviorTask(void* parameters) {
/******** Variables *********/
uint32 lastWakeTime = osTaskGetTickCount();
uint32 neighborRoundPrev = 0;
uint32 neighborRound = 0;
uint8 i = 0;
uint16 IRXmitPower = IR_COMMS_POWER_MAX;
boolean newSensorData;
int32 tv, rv;
uint16 leader = 0;
uint16 mode = MODE_INACTIVE;
uint16 nbrMode = MODE_INACTIVE;
uint16 nbrLeader = 0;
uint16 wait = 0;
NbrData msgLeaderPosXR;
NbrData msgLeaderPosYR;
NbrData msgLeaderPosXS;
NbrData msgLeaderPosYS;
Nbr* nbrPtr;
Nbr* leaderPtr;
int32 translatetime = 0;
int32 rotateTime = 0;
NbrList nbrList;
int32 nbrBearing = 0;
int32 nbrDist = 0;
int32 alpha;
int32 flockAngleIntegral = 0;
int32 flockAngleIntegral2 = 0;
uint32 accelCounter = 0;
int32 leaderPosY = 0;
int32 leaderPosX = 0;
Beh behOutput;
/******** Initializations ********/
radioCommandSetSubnet(1);
neighborsInit(NEIGHBOR_ROUND_PERIOD);
nbrDataCreate(&msgLeaderPosXR, "positionx", 8, 0);
nbrDataCreate(&msgLeaderPosYR, "positiony", 8, 0);
nbrDataCreate(&msgLeaderPosXS, "positionx", 8, 0);
nbrDataCreate(&msgLeaderPosYS, "positiony", 8, 0);
broadcastMsgCreate(&broadcastMessage, 20);
broadcastMsgDataCreate(&broadcastData_Mode, &broadcastMessage, "mode",
MODE_INACTIVE);
broadcastMsgDataCreate(&broadcastData_DesiredHeading, &broadcastMessage,
"heading", 0);
uint32 senderID_seen = 0;
systemPrintStartup();
uint8 val = 0;
int32 t = 0;
int16 leaderBearing = 0;
uint32 leaderDist = 0;
int32 l1 = 0;
int32 l2 = 0;
/******** Behavior **************/
for (;;) {
behOutput = behInactive;
neighborsGetMutex();
printNow = neighborsNewRoundCheck(&neighborRound);
//cprintf("print %d \n",printNow);
//TODO yo uonly need to call this function once
irCommsSetXmitPower(IRXmitPower);
nbrListCreate(&nbrList);
broadcastMsgUpdate(&broadcastMessage, &nbrList);
if (broadcastMessage.active == TRUE) {
if (broadcastMsgIsSource(&broadcastMessage)) {
nbrDataSet(&msgLeaderPosXS, 0);
nbrDataSet(&msgLeaderPosYS, 0);
//nbrDataSet(&msgLeaderPosXR, 0);
//nbrDataSet(&msgLeaderPosYR, 0);
if (printNow) {
cprintf("x: %d y: %d dist: %d bear: %d\n", 0, 0, 0, 0);
}
} else
{
int32 senderID = broadcastMsgGetSenderID(&broadcastMessage);
//behFlockNormalToLeader(&behOutput, &nbrList, nbrPtr,tv);
for (t = 0; t < nbrList.size; ++t) {
nbrPtr = nbrList.nbrs[t];
if (nbrPtr->ID == senderID) {
senderID_seen = 1;
nbrBearing = nbrGetBearing(nbrPtr);
nbrDist = nbrGetRange(nbrPtr);
if (printNow) {
cprintf("dist: %d bear: %d\n", nbrDist, nbrBearing);
//cprintf("posx: %d posy: %d \n",&msgLeaderPosX.value,&msgLeaderPosY.value);
}
l1 = ((int8) nbrDataGet(nbrPtr)) * LEADER_POS_SCALER;
l2 = ((int8) nbrDataGet(&msgLeaderPosYR))
* LEADER_POS_SCALER;
leaderPosX = l1
+ (int32)(
nbrDist * cosMilliRad(nbrBearing)
/ MILLIRAD_TRIG_SCALER);
leaderPosY = l2
+ (int32)(
nbrDist * sinMilliRad(nbrBearing)
/ MILLIRAD_TRIG_SCALER);
leaderPosX = boundAbs(leaderPosX, LEADER_POS_BOUND);
leaderPosY = boundAbs(leaderPosY, LEADER_POS_BOUND);
nbrDataSet(&msgLeaderPosXS,
(int8)(leaderPosX / LEADER_POS_SCALER));
nbrDataSet(&msgLeaderPosYS,
(int8)(leaderPosY / LEADER_POS_SCALER));
int32 dist = leaderPosX * leaderPosX
+ leaderPosY * leaderPosY;
leaderDist = sqrtInt((uint32) dist);
leaderBearing = atan2MilliRad(leaderPosY, leaderPosX);
/*
* Problems:
* 1- leader bearing not normalized
* 2- distance never changes
*/
leaderBearing = normalizeAngleMilliRad2(leaderBearing);
}
}
/*if (printNow) {
cprintf("x: %d y: %d dist: %d bear: %d\n", leaderPosX,
leaderPosY, leaderDist, leaderBearing);
//cprintf("posx: %d posy: %d \n",&msgLeaderPosX.value,&msgLeaderPosY.value);
cprintf("posx: %d posy: %d \n", l1, l2);
}*/
}
}
//Check Buttons, won't start until leader selected
if(buttonsGet(BUTTON_RED)&&buttonsGet(BUTTON_GREEN)){
broadcastMsgSetSource(&broadcastMessage, TRUE);
broadcastMsgDataSet(&broadcastData_Mode, MODE_COM);
translatetime = 0;
rotateTime = 0;
val = MODE_COM;
}else if (buttonsGet(BUTTON_RED)) {
broadcastMsgSetSource(&broadcastMessage, TRUE);
broadcastMsgDataSet(&broadcastData_Mode, MODE_TRANSLATE);
val = MODE_TRANSLATE;
translatetime = 0;
rotateTime = 0;
} else if (buttonsGet(BUTTON_GREEN)) {
// Checks Green button to stop behavior
broadcastMsgSetSource(&broadcastMessage, TRUE);
broadcastMsgDataSet(&broadcastData_Mode, MODE_ROTATE);
translatetime = 0;
rotateTime = 0;
val = MODE_ROTATE;
} else if (buttonsGet(BUTTON_BLUE)) {
broadcastMsgSetSource(&broadcastMessage, TRUE);
broadcastMsgDataSet(&broadcastData_Mode, MODE_LEADERSTOP);
translatetime = 0;
rotateTime = 0;
val = MODE_LEADERSTOP;
}
if (printNow)
cprintf("hops, mode = %d,%d\n",
broadcastMsgGetHops(&broadcastMessage),
broadcastMsgDataGet(&broadcastData_Mode));
// uint32 translateNum = 0;
// uint32 rotateNum = 0;
// uint8 foundLeader = 0;
// uint32 interruptNum = 0;
// leaderPtr = NULL;
// if (broadcastMsgIsSource(&broadcastMessage)){
// //do nothing
// } else {
// //check for mode change
// for (i = 0; i < nbrList.size; ++i){
// nbrPtr = nbrList.nbrs[i];
// nbrMode = nbrDataGetNbr(&msgMode, nbrPtr);
// nbrLeader = nbrDataGetNbr(&msgLeader, nbrPtr);
// if (nbrLeader == 1){
// foundLeader = 1;
// mode = nbrMode;
// leaderPtr = nbrPtr;
// if(mode == MODE_ROTATE){
// translatetime = 0;
// }
// if(mode == MODE_TRANSLATE){
// rotateTime = 0;
// }
// } else {
// if (nbrMode != 0){
// if(nbrMode == 1){
// translateNum = translateNum +1;
// } else if (nbrMode == 2){
// rotateNum = rotateNum + 1;
// } else if (nbrMode == 3){
// interruptNum = interruptNum + 1;
// }
// }
// }
// }
// if (foundLeader != 1){
// if(translateNum > rotateNum && translateNum > interruptNum){
// mode = MODE_TRANSLATE;
// rotateTime = 0;
// } else if (rotateNum > translateNum && rotateNum > interruptNum){
// mode = MODE_ROTATE;
// translatetime = 0;
// } else if (rotateNum == 0 && translateNum == 0 && interruptNum == 0){
// //dont change from previous mode
// } else if (interruptNum > translateNum && interruptNum > rotateNum){
// mode = MODE_LEADERSTOP;
// translatetime = 0;
// rotateTime = 0;
// } else {
// mode= MODE_TRANSLATE;
// rotateTime = 0;
// }
// }
// }
if (printNow) {
cprintf("sender: %d\n", broadcastMsgGetSenderID(&broadcastMessage));
} else {
val = broadcastMsgDataGet(&broadcastData_Mode);
//cprintf("val: %d\n", val);
broadcastMsgDataSet(&broadcastData_Mode, val);
}
switch (val) {
case MODE_INACTIVE: {
behOutput = behInactive;
//cprintf("inactive \n");
ledsSetPattern(LED_RED, LED_PATTERN_CIRCLE, LED_BRIGHTNESS_MED,
LED_RATE_MED);
break;
}
case MODE_LEADERSTOP: {
behOutput = behInactive;
ledsSetPattern(LED_RED, LED_PATTERN_PULSE, LED_BRIGHTNESS_MED,
LED_RATE_MED);
break;
}
case MODE_TRANSLATE: {
translatetime = translatetime + 1;
ledsSetPattern(LED_GREEN, LED_PATTERN_PULSE, LED_BRIGHTNESS_MED,
LED_RATE_MED);
if (broadcastMsgIsSource(&broadcastMessage)) {
//Pulsing Green LEDS indicate waiting to translate
//Circling Green LEDS indicate translation has begun
if (translatetime <= DELAY_TRANSLATE) { // set the time delay before start translating
behSetTvRv(&behOutput, 0, 0);
ledsSetPattern(LED_GREEN, LED_PATTERN_PULSE,
LED_BRIGHTNESS_LOW, LED_RATE_MED);
} else {
// TODO : add a timer for the duration of translation : if (translatetime <= TRANSLATE_ PERIOD)
behSetTvRv(&behOutput, TV, 0);
ledsSetPattern(LED_GREEN, LED_PATTERN_CIRCLE,
LED_BRIGHTNESS_LOW, LED_RATE_MED);
// TODO : else { behSetTvRv(&behOutput, TV, 0);
// // mode = MODE_INACTIVE }
}
} else {
int32 tvgain, error;
// either push or flock, depending on the experiment
// alpha = behFlockAngle(&nbrList);
// rvBearingController(&behMove, alpha, 90);
// error = behFlockAngleMax(&nbrList)/ 100;
// if(abs(error) > 3) {
// //behMove.tv = 0;
// if(error > 0) {
// motorSetPWM(MOTOR_LEFT, -PHOTOTAXIS_PWM);
// motorSetPWM(MOTOR_RIGHT, PHOTOTAXIS_PWM);
// } else {
// motorSetPWM(MOTOR_LEFT, PHOTOTAXIS_PWM);
// motorSetPWM(MOTOR_RIGHT, -PHOTOTAXIS_PWM);
// }
// cprintf("rotate error % 4d tv% 4d size% 4d\n", error, behMove.tv, nbrList.size );
// } else {
//// behMove.tv = FLOCK_TV;
// motorSetPWM(MOTOR_LEFT, PHOTOTAXIS_PWM);
// motorSetPWM(MOTOR_RIGHT, PHOTOTAXIS_PWM);
// cprintf("translate error % 4d tv% 4d size% 4d\n", error, behMove.tv, nbrList.size );
// }
alpha = behFlockAngle(&nbrList); // consensus, be aligned with the leader
// start PI controller
error = alpha / 100;
flockAngleIntegral = flockAngleIntegral * FLOCK_INTEGRAL_DECAY / 1000;
flockAngleIntegral += (error * error) * (error > 0 ? 1 : -1);
flockAngleIntegral = bound(flockAngleIntegral,
-FLOCK_INTEGRAL_MAX, FLOCK_INTEGRAL_MAX);
tvgain = 100 - (abs(flockAngleIntegral) * 100) / FLOCK_INTEGRAL_MAX;
//tvgain = 100 - ((flockAngleIntegral*flockAngleIntegral) * 100)/(FLOCK_INTEGRAL_MAX*FLOCK_INTEGRAL_MAX);
//behMove.rv = flockAngleIntegral;
int32 rv_I = flockAngleIntegral * K_I / 100; //WEIGHTED TEST WORKS WITH 2
int32 rv_P = error * K_P / 100; //WEIGHTED TEST WORKS WITH 2
if (printNow) {
cprintf(
"error % 4d flockAngleIntegral% 5d rv_I% 4d rv_P% 4d \n",
alpha, flockAngleIntegral, rv_I, rv_P);
}
behOutput.rv = (rv_I + rv_P);
//rv = ((100 - (networkSize*100)/4) * rv) / 100;
behOutput.active = TRUE;
//cprintf("error % 4d flockAngleIntegral% 5d tvgain% 4d tv% 4d nbr% 4d\n", alpha, flockAngleIntegral, tvgain, behMove.tv ,nbrGetID(nbrList ->nbrs));
if (translatetime <= DELAY_TRANSLATE) { // set the time delay before start translating
behOutput.tv = 0;
ledsSetPattern(LED_GREEN, LED_PATTERN_PULSE,
LED_BRIGHTNESS_LOW, LED_RATE_MED);
} else {
// TODO : add a timer for the duration of translation : if (translatetime <= TRANSLATE_ PERIOD)
//behOutput.tv = TV;
behOutput.tv = TV_GAIN * FLOCK_TV * tvgain / 100;
ledsSetPattern(LED_GREEN, LED_PATTERN_CIRCLE,
LED_BRIGHTNESS_LOW, LED_RATE_MED);
// TODO : else { behSetTvRv(&behOutput, 0, 0);
// mode = MODE_INACTIVE }
}
}
//motorSetBeh(&behOutput);
break;
}
case MODE_ROTATE: {
translatetime = 0;
rotateTime = rotateTime + 1;
if (rotateTime <= DELAY_ROTATE) {
behOutput.tv = 0;
//behSetTvRv(&behOutput, 0, 0);
ledsSetPattern(LED_GREEN, LED_PATTERN_PULSE, LED_BRIGHTNESS_LOW,
LED_RATE_MED);
} else {
if (broadcastMsgIsSource(&broadcastMessage)) {
//if (rotateTime<=100){
// ledsSetPattern(LED_BLUE, LED_PATTERN_PULSE, LED_BRIGHTNESS_MED, LED_RATE_SNAIL);
//behOutput = behInactive;
//} else {
ledsSetPattern(LED_BLUE, LED_PATTERN_CIRCLE,
LED_BRIGHTNESS_MED, LED_RATE_MED);
behOutput = behInactive;
//}
} else {
//uint16 speed = 0;
// if (rotateTime <=100){
// tv = 0;
// speed = LED_RATE_SNAIL;
// } else {
// tv = TV;
uint16 speed = LED_RATE_MED;
// }
// if(broadcastMsgIsSource(&broadcastMessage))
// {
// ledsSetPattern(LED_BLUE, LED_PATTERN_CIRCLE, LED_BRIGHTNESS_MED, speed);
// behOutput = behInactive;
// nbrDataSet(&msgLeaderPosX , 0);
// nbrDataSet(&msgLeaderPosY , 0);
// } else {
ledsSetPattern(LED_BLUE, LED_PATTERN_PULSE,
LED_BRIGHTNESS_MED, speed);
int32 tv_gain, alpha;
tv = 0;
rv = 0;
tv_gain = 0;
behOutput.active = TRUE;
//tv= TV_GAIN* nbrPtr->range/10;
//alpha = behAngleNormalToLeader(leaderBearing);
int32 error = alpha / 100;
flockAngleIntegral2 = flockAngleIntegral2
* FLOCK_INTEGRAL_DECAY / 1000;
flockAngleIntegral2 += 1 * (error * error)
* (error > 0 ? 1 : -1);
flockAngleIntegral2 = bound(flockAngleIntegral2,
-FLOCK_INTEGRAL_MAX, FLOCK_INTEGRAL_MAX);
tv_gain = 100
- (abs(flockAngleIntegral2) * 100)
/ FLOCK_INTEGRAL_MAX;
//leaderBearing = atan2MilliRad(leaderPosY , leaderPosX);
//leaderDist = sqrtInt((uint32)dist) ;
if (leaderBearing > 0) // turn to be normal to the leader
alpha = normalizeAngleMilliRad2(
leaderBearing - (int32) MILLIRAD_DEG_90);
else
alpha = normalizeAngleMilliRad2(
leaderBearing + (int32) MILLIRAD_DEG_90);
//uint32 leaderDist =dist/80 ;
//alpha = MILLIRAD_DEG_30;
//int32 FLOCK_RV_GAIN = 50;
//rv = alpha * FLOCK_RV_GAIN / 100;
int32 rv_I = 0 * flockAngleIntegral2 * K_I / 100; //WEIGHTED TEST WORKS WITH 2
int32 rv_P = error * K_P; //WEIGHTED TEST WORKS WITH 2
// if (printNow) {
// cprintf(
// "error % 4d flockAngleIntegral% 5d rv_I% 4d rv_P% 4d \n",
// alpha, flockAngleIntegral, rv_I, rv_P);
// }
behOutput.rv = (rv_I + rv_P);
//tv_gain = 1;
tv = tv_gain * leaderDist / 4300; // set tv based on the distance to the leader
//behOutput.rv = rv;
behOutput.tv = tv;
//cprintf("error % 4d flockAngleIntegral% 5d tvgain% 4d tv% 4d nbr% 4d\n", alpha, flockAngleIntegral, tvgain, behMove.tv ,nbrGetID(nbrList ->nbrs));
//if (translatetime<=100){
// behOutput.tv = 0;
// ledsSetPattern(LED_GREEN, LED_PATTERN_PULSE, LED_BRIGHTNESS_LOW, LED_RATE_MED);
//}
// else {
//behOutput.tv = TV;0
// }
// motorSetBeh(&behOutput);
ledsSetPattern(LED_GREEN, LED_PATTERN_CIRCLE,
LED_BRIGHTNESS_LOW, LED_RATE_MED);
/* if (senderID_seen == 1)
{
behOutput.rv = rv;
behOutput.tv = tv;
} else {
behOutput.rv = 0;
behOutput.tv = 0;
}
*/
}
/*
for (i = 0; i < nbrList.size; ++i){
nbrPtr = nbrList.nbrs[i];
nbrLeader = nbrDataGetNbr(&msgLeader, nbrPtr);
//if (nbrLeader == 1)
if(broadcastMsgIsSource(&broadcastMessage))
{
tv= TV_GAIN* nbrPtr->range/10;
behFlockNormalToLeader(&behOutput, &nbrList, nbrPtr,tv);
}
}
if(leaderPtr != NULL){
ledsSetPattern(LED_BLUE, LED_PATTERN_CIRCLE, LED_BRIGHTNESS_MED, speed);
motorSetBeh(&behOutput);
//behOrbitRange(&behOutput,leaderPtr,tv,2);
} else {
ledsSetPattern(LED_BLUE, LED_PATTERN_PULSE, LED_BRIGHTNESS_MED, speed);
}
*/
//}
}
break;
}//end rotate
case MODE_COM: {
ledsSetPattern(LED_BLUE,LED_PATTERN_CLAW,LED_BRIGHTNESS_MED,LED_RATE_MED);
break;
}
} //end mode switch
//motorSetBeh(&behOutput);
motorSetBeh(&behOutput);
neighborsPutMutex();
osTaskDelayUntil(&lastWakeTime, BEHAVIOR_TASK_PERIOD);
} //forever for loop
} //behaviorTask()
void behaviorTask(void* parameters) {
//rprintfSetSleepTime(500);
uint32 lastWakeTime = osTaskGetTickCount();
uint8 navigationMode = MODE_IDLE;
Beh behOutput;
uint8 i;
boolean printNow, WAITING;
uint32 neighborRound = 0;
int RV;
BroadcastMessage broadcastMessage;
broadcastMsgCreate(&broadcastMessage, 20);
systemPrintStartup();
systemPrintMemUsage();
neighborsInit(NEIGHBOR_ROUND_PERIOD);
radioCommandSetSubnet(2);
//uint16 IRXmitPower = IR_COMMS_POWER_MAX/4;
boolean bounceGreen = 0;
boolean bounceBlue = 0;
boolean bounceRed = 0;
uint8 redCount = 0;
uint8 blueCount = 0;
//GRIPPER INIT
gripperBoardInit();
uint8 gripPos = REST;
gripperGripRelax();
for (;;) {
if (rprintfIsHost()) {
ledsSetPattern(LED_BLUE, LED_PATTERN_CIRCLE, LED_BRIGHTNESS_LOW, LED_RATE_MED);
continue;
}//end if host
else{
/*** INIT STUFF ***/
behOutput = behInactive;
neighborsGetMutex();
printNow = neighborsNewRoundCheck(&neighborRound);
/*** READ BUTTONS ***/
if (buttonsGet(BUTTON_RED)) {
navigationMode = ROT_LEFT;
if(bounceRed){
redCount++;
if(redCount > 5){
redCount = 0;
}
}
bounceRed = 0;
}
if (buttonsGet(BUTTON_GREEN)) {
gripPos = 0;
navigationMode = MODE_IDLE;
}
if (buttonsGet(BUTTON_BLUE)) {
navigationMode = ROT_RIGHT;
if(bounceBlue){
blueCount++;
cprintf("1\n");
if(blueCount > 5){
blueCount = 0;
}
}
bounceBlue = 0;
}
if (!buttonsGet(BUTTON_RED)) {
bounceRed = 1;
}
if (!buttonsGet(BUTTON_GREEN)) {
bounceGreen = 1;
}
if (!buttonsGet(BUTTON_BLUE)) {
bounceBlue = 1;
}
/** STATES MACHINE **/
switch (navigationMode) {
case MODE_IDLE: {
//Start Menu
ledsSetPattern(LED_GREEN, LED_PATTERN_CIRCLE, LED_BRIGHTNESS_MED, LED_RATE_SLOW);
behOutput = behInactive;
if(gripPos != ATTEMPTING){
gripperGripUntilGripped();
gripPos = ATTEMPTING;
}
cprintf("Idle");
break;
}
case ROT_LEFT: {
//Choose lowest robot amongst themselves to be controlled by remote
navigationLEDsSet(redCount,0,blueCount, 0);
RV = redCount*300;
behSetTvRv(&behOutput, 0, RV);
cprintf("Left RV %d ",RV);
break;
}
case ROT_RIGHT: {
//Robot to be controled by remote
navigationLEDsSet(redCount,0,blueCount, 0);
RV = blueCount*300;
behSetTvRv(&behOutput, 0, -RV);
cprintf("Right RV %d ",RV);
break;
}
}
cprintf("BUMP %d A X %d A Y %d A Z %d G X %d G Y %d G Z %d\n", bumpSensorsGetBearing(),
accelerometerGetValue(ACCELEROMETER_X), accelerometerGetValue(ACCELEROMETER_Y), accelerometerGetValue(ACCELEROMETER_Z),
gyroGetValue(GYRO_X_AXIS), gyroGetValue(GYRO_Y_AXIS), gyroGetValue(GYRO_Z_AXIS));
/*** FINAL STUFF ***/
motorSetBeh(&behOutput);
neighborsPutMutex();
// delay until the next behavior period
osTaskDelayUntil(&lastWakeTime, BEHAVIOR_TASK_PERIOD);
lastWakeTime = osTaskGetTickCount();
}//end not host
} // end for loop
} //end behavior function
void behaviorTask(void* parameters) {
//initialization
uint32 lastWakeTime = osTaskGetTickCount();
uint32 neighborRoundPrev = 0;
uint32 neighborRound;
uint8 neighbors;
uint32 currentTime = osTaskGetTickCount();
uint32 prevTime = 0;
boolean reachedLightSource = FALSE;
int32 integralError = 0;
boolean targetSeen = 0;
boolean lightActivated = 0;
boolean TaskDone = 0;
uint32 bumpSensorTimeOn = 0;
uint32 bumpSensorTimeOff = 0;
static boolean IntegralWindup = 0;
uint8 currentState = MODE_IDLE;
// search variables
uint32 segmentNum = 0;
uint32 TimeStart = currentTime;
uint32 TimeForThisSegment = 0;
static int ErrorLightPtr[2];
ErrorLightPtr[0] = 0;
ErrorLightPtr[1] = 0;
//int ErrorlightPtr =&LightError;
int32 light_value[4];
neighborsInit(NEIGHBOR_ROUND_PERIOD);
int32 light_value_init[4];
light_value_init[0] = lightSensorGetValue(0); // front right light sensor
light_value_init[1] = lightSensorGetValue(1); // front left light sensor
light_value_init[2] = lightSensorGetValue(2); // rear left light sensor
light_value_init[3] = lightSensorGetValue(3); // rear right sensor
for (;;) {
if (!rprintfIsHost()) {
NbrList nbrList;
Beh behMove = behInactive;
neighborsGetMutex();
nbrListCreate(&nbrList);
currentTime = osTaskGetTickCount();
buttonModeRGB(¤tState, MODE_FOLLOW_WALL_LEFT, MODE_IDLE, MODE_FOLLOW_WALL_RIGHT);
uint8 bumpBits = bumpSensorsGetBits();
if (bumpBits != 0 ) {
bumpSensorTimeOn++;
bumpSensorTimeOff = 0;
isBumped = 1;
}else{
bumpSensorTimeOn = 0;
bumpSensorTimeOff++;
}
light_value[0] = lightSensorGetValue(0) - FR_OFFSET ; // front right light sensor
light_value[1] = lightSensorGetValue(1); // front left light sensor
light_value[2] = lightSensorGetValue(2); // rear left light sensor
light_value[3] = lightSensorGetValue(3) - RR_OFFSET; // rear right sensor
//compute control law to move to light
switch (currentState){
case MODE_IDLE: {
ledsSetPattern(LED_GREEN, LED_PATTERN_CIRCLE, LED_BRIGHTNESS_LOW, LED_RATE_SLOW);
break;
}
case MODE_FOLLOW_WALL_LEFT: {
behWallMove(&behMove, MOTION_TV, WALL_FOLLOW_LEFT);
ledsSetPattern(LED_RED, LED_PATTERN_CIRCLE, LED_BRIGHTNESS_HIGH, LED_RATE_FAST);
break;
}
case MODE_FOLLOW_WALL_RIGHT: {
behWallMove(&behMove, MOTION_TV, WALL_FOLLOW_RIGHT);
ledsSetPattern(LED_BLUE, LED_PATTERN_CIRCLE, LED_BRIGHTNESS_HIGH, LED_RATE_FAST);
break;
}
case MODE_SEARCH_FOR_OBJECT:{
ledsSetPattern(LED_RED, LED_PATTERN_ON, LED_BRIGHTNESS_MED, LED_RATE_MED);
if(bumpSensorTimeOn > 5){
currentState = MODE_TRANSPORT_TO_LIGHT;
}
//TODO: spiral mode. For now, move straight
//behSetTv(&behMove, SPEED);
//behSetRv(&behMove, 0);
// have we completed this segment?
if( currentTime >= TimeStart+TimeForThisSegment ){
segmentNum++;
TimeStart = currentTime;
// we want segments of length diameter*(1,1,2,2,3,3,4,4,5,5,...) to make a spiral path
// = (number)*robot_diameter_in_mm*1000 ms/s / SPEED_in_mm/s.
TimeForThisSegment = ((segmentNum+1)/2)*110*1000/SPEED;
}
//Compute control law
behSetTv(&behMove, SPEED);
// we added 100 to the multiplication to make a tighter spiral.
behSetRv(&behMove, MILLIRAD_DEG_90*1100/TimeForThisSegment);
break;
}
case MODE_TRANSPORT_TO_LIGHT:{
ledsSetPattern(LED_BLUE, LED_PATTERN_ON, LED_BRIGHTNESS_MED, LED_RATE_MED);
/*if(bumpSensorTimeOff > 5){
currentState = SEARCH_FOR_OBJECT;
segmentNum = 0;
TimeStart = currentTime;
TimeForThisSegment = 0;
break;
}*/
/*if ( TaskDone == 1 ){
currentState = AT_LIGHT;
break;
}
*/
// move toward the light
behLight(&behMove , light_value, ErrorLightPtr,&TaskDone, &IntegralWindup );
//behSetTv(behPtr, behPtr->tv);
//behSetRv(behPtr, behPtr->rv);
break;
}
case MODE_AT_LIGHT:{
ledsSetPattern(LED_GREEN, LED_PATTERN_ON, LED_BRIGHTNESS_MED, LED_RATE_MED);
behSetTvRv(&behMove, 0, 0);
if ( TaskDone != 1 ){
currentState = MODE_TRANSPORT_TO_LIGHT;
}
break;
}
}
//cprintf("%d:%d,%d,%d,%d,%d,%d,%d\n", roneID, lightActivated, targetSeen, isBumped, bumpBits, light_value[0], light_value[1], TaskDone);
motorSetBeh(&behMove);
neighborsPutMutex();
osTaskDelayUntil(&lastWakeTime, BEHAVIOR_TASK_PERIOD);
//TODO: limit tv and rv. (100 is a good limit?)
}
}
return;
}
void behaviorTask(void* parameters) {
int state = STATE_IDLE;
int movementState;
int16 bumpBearing,rotateRV, currTv;
uint32 lastWakeTime = osTaskGetTickCount();
uint32 rotateTime, momentumTime;
NbrList nbrList;
Beh behOutput;
uint8 buttonRed, buttonGreen, buttonBlue;
boolean momementumActive, printNow;
uint32 neighborRound = 0;
int i;
uint32 nbrBearing;
Nbr* nbrPtr;
NbrData type;
NbrData velocity;
// Init nbr system
BroadcastMessage broadcastMessage;
broadcastMsgCreate(&broadcastMessage, 20);
radioCommandSetSubnet(1);
neighborsInit(300);
// Create nbr data
nbrDataCreate(&type, "type", 2, 0);
nbrDataCreate(&velocity, "velocity", 8, 0);
// Print startup message and thread memory usage
systemPrintStartup();
systemPrintMemUsage();
for (;;) {
behOutput = behInactive;
printNow = neighborsNewRoundCheck(&neighborRound);
nbrListCreate(&nbrList);
broadcastMsgUpdate(&broadcastMessage, &nbrList);
// Determine state from buttons
if (state == STATE_IDLE) {
if (buttonsGet(BUTTON_RED)) {
state = STATE_CUEBALL;
} else if (buttonsGet (BUTTON_GREEN)) {
state = STATE_POCKET;
} else if (buttonsGet (BUTTON_BLUE)) {
state = STATE_BALL;
}
}
// Set LEDs for each state
switch (state) {
case STATE_CUEBALL: {
ledsSetPattern(LED_ALL, LED_PATTERN_PULSE, LED_BRIGHTNESS_MED, LED_RATE_SLOW);
nbrDataSet(&type, STATE_CUEBALL);
break;
}
case STATE_POCKET: {
ledsSetPattern(LED_GREEN, LED_PATTERN_PULSE, LED_BRIGHTNESS_MED, LED_RATE_SLOW);
nbrDataSet(&type, STATE_POCKET);
nbrDataSet(&velocity, 0);
movementState = MOVE_IDLE;
break;
}
case STATE_BALL: {
nbrDataSet(&type, STATE_CUEBALL);
if (roneID % 2)
ledsSetPattern(LED_RED, LED_PATTERN_PULSE, LED_BRIGHTNESS_MED, LED_RATE_SLOW);
else
ledsSetPattern(LED_BLUE, LED_PATTERN_PULSE, LED_BRIGHTNESS_MED, LED_RATE_SLOW);
break;
}
case STATE_IDLE:
default: {
ledsSetPattern(LED_ALL, LED_PATTERN_CIRCLE, LED_BRIGHTNESS_MED, LED_RATE_SLOW);
movementState = MOVE_IDLE;
break;
}
}
//Movement State Machine
switch (movementState) {
case MOVE_IDLE: {
momementumActive = 0;
behSetTvRv(&behOutput, 0, 0);
nbrDataSet(&velocity, 0);
if (bumpSensorsGetBearing() != -1) {
movementState = MOVE_ROTATE;
rotateTime = osTaskGetTickCount();
bumpBearing = bumpSensorsGetBearing();
if (state == STATE_CUEBALL){
currTv = 150;
} else if(state == STATE_BALL){
//currTv = 75;
for (i = 0; i < nbrList.size; i++){
nbrPtr = nbrList.nbrs[i];
nbrBearing = nbrGetBearing(nbrPtr);
cprintf("NBR ID: %d, BB %d, NB %d\n",nbrGetID(nbrPtr),bumpBearing,nbrBearing);
if(abs(nbrBearing -bumpBearing) < 500){
currTv = nbrDataGetNbr(&velocity, nbrPtr);
cprintf("currTv %d")
}
}
}else{
currTv = 0;
}
if(bumpBearing > 0){
bumpBearing = PI - bumpBearing;
rotateRV = -1000;
} else{
bumpBearing = PI - abs(bumpBearing);
rotateRV = 1000;
}
//bearing = bearing * 1.25;
behSetTvRv(&behOutput, 0, rotateRV);
}
break;
}
// behaviors run every 50ms. They should be designed to be short, and terminate quickly.
// they are used for robot control. Watch this space for a simple behavior abstraction
// to appear.
//
void behaviorTask(void* parameters) {
/******** Variables *********/
uint32 lastWakeTime = osTaskGetTickCount();
uint32 neighborRoundPrev = 0;
uint32 neighborRound = 0;
uint8 i = 0;
uint16 IRXmitPower = IR_COMMS_POWER_MAX;
boolean newSensorData;
int32 tv, rv;
uint16 leader = 0;
uint16 mode = MODE_INACTIVE;
uint16 nbrMode = MODE_INACTIVE;
uint16 nbrLeader = 0;
uint16 wait = 0;
NbrData msgLeaderPosX;
NbrData msgLeaderPosY;
NbrData msgNbrType;
NbrDataFloat msgWeight;
Nbr* nbrPtr;
Nbr* leaderPtr = NULL;
Nbr* safestGuidePtr = NULL;
int32 translatetime = 0;
int32 rotateTime = 0;
NbrList nbrList;
int32 nbrBearing = 0;
int32 nbrDist = 0;
int32 alpha;
int32 flockAngleIntegral = 0;
int32 flockAngleIntegral2 = 0;
uint32 accelCounter = 0;
int32 leaderPosY = 0;
int32 leaderPosX = 0;
Beh behOutput;
/******** Initializations ********/
radioCommandSetSubnet(1);
neighborsInit(NEIGHBOR_ROUND_PERIOD);
nbrDataCreate(&msgLeaderPosX, "positionx", 8, 0);
nbrDataCreate(&msgLeaderPosY, "positiony", 8, 0);
nbrDataCreate(&msgNbrType, "type", 2, TRANSPORT);
nbrDataCreateFloat(&msgWeight, "weight");
broadcastMsgCreate(&broadcastMessage, 20);
broadcastMsgDataCreate(&broadcastData_Mode, &broadcastMessage, "mode",MODE_INACTIVE);
broadcastMsgDataCreate(&broadcastData_DesiredHeading, &broadcastMessage,"heading", 0);
uint32 senderID_seen = 0;
irCommsSetXmitPower(IRXmitPower);
systemPrintStartup();
uint8 state = 0;
int32 t = 0;
int16 leaderBearing = 0;
uint32 leaderDist = 0;
/******** Behavior **************/
for (;;) {
behOutput = behInactive;
neighborsGetMutex();
printNow = neighborsNewRoundCheck(&neighborRound);
nbrListCreate(&nbrList);
broadcastMsgUpdate(&broadcastMessage, &nbrList);
int32 senderID = broadcastMsgGetSenderID(&broadcastMessage);
/**** Calculate Position to All Neighbors ****/
for (t = 0; t < nbrList.size; ++t) {
nbrPtr = nbrList.nbrs[t];
if (nbrPtr->ID == senderID) {
senderID_seen = 1;
nbrBearing = nbrGetBearing(nbrPtr);
nbrDist = nbrGetRange(nbrPtr);
int32 l1 = ((int8)nbrDataGet(&msgLeaderPosX)) * LEADER_POS_SCALER;
int32 l2 = ((int8)nbrDataGet(&msgLeaderPosY)) * LEADER_POS_SCALER;
leaderPosX = l1 + (int32)(nbrDist * cosMilliRad(nbrBearing) / MILLIRAD_TRIG_SCALER);
leaderPosY = l2 + (int32)(nbrDist * sinMilliRad(nbrBearing) / MILLIRAD_TRIG_SCALER);
leaderPosX = boundAbs(leaderPosX, LEADER_POS_BOUND);
leaderPosY = boundAbs(leaderPosY, LEADER_POS_BOUND);
nbrDataSet(&msgLeaderPosX, (int8)(leaderPosX / LEADER_POS_SCALER));
nbrDataSet(&msgLeaderPosY, (int8)(leaderPosY / LEADER_POS_SCALER));
int32 dist = leaderPosX * leaderPosX + leaderPosY * leaderPosY;
leaderBearing = atan2MilliRad(leaderPosY, leaderPosX);
leaderDist = sqrtInt((uint32) dist);
}
}
/**** Check Button Input ****/
//Check Buttons, won't start until leader selected
if (buttonsGet(BUTTON_RED)) {
//Sets leader
broadcastMsgSetSource(&broadcastMessage, TRUE);
broadcastMsgDataSet(&broadcastData_Mode, MODE_INACTIVE);
state = MODE_LEADER;
translatetime = 0;
rotateTime = 0;
} else if (buttonsGet(BUTTON_GREEN)) {
//Starts navigation mode
broadcastMsgSetSource(&broadcastMessage, TRUE);
broadcastMsgDataSet(&broadcastData_Mode, MODE_ROTATE);
translatetime = 0;
rotateTime = 0;
state = MODE_ROTATE;
} else if (buttonsGet(BUTTON_BLUE)) {
//Stops all modes
broadcastMsgSetSource(&broadcastMessage, TRUE);
broadcastMsgDataSet(&broadcastData_Mode, MODE_LEADERSTOP);
translatetime = 0;
rotateTime = 0;
state = MODE_LEADERSTOP;
}
/*** Debugging Prints ***/
if (printNow){
cprintf("hops, mode = %d,%d\n",broadcastMsgGetHops(&broadcastMessage),broadcastMsgDataGet(&broadcastData_Mode));
}
if (printNow) {
cprintf("sender: %d\n", broadcastMsgGetSenderID(&broadcastMessage));
} else {
state = broadcastMsgDataGet(&broadcastData_Mode);
cprintf("val: %d\n", state);
broadcastMsgDataSet(&broadcastData_Mode, state);
}
/*** Determine Action ***/
switch (state) {
case MODE_LEADER: {
//Leader flashes all LEDS
ledsSetPattern(LED_ALL,LED_PATTERN_BLINK,LED_BRIGHTNESS_MED,LED_RATE_MED);
/*** Select Safest Guide ***/
Nbr* newSafestPtr = NULL;
NbrDataFloat msgWeight;
uint8 nbrType = UNSAFE;
for(t = 0; t < nbrList.size; ++t){
nbrPtr = nbrList.nbrs[t];
nbrType = nbrDataGetNbr(&msgNbrType,nbrPtr);
if(nbrType == TRANSPORT){
//Ignore transport neighbors for choosing parent robot
continue;
} else {
//Find minimum cost parent
if(newSafestPtr != NULL){
//If we have a guide selected, compare and determine if new should be selected
if(nbrDataGetNbrFloat(&msgWeight,nbrPtr) < nbrDataGetNbrFloat(&msgWeight,newSafestPtr)){
newSafestPtr = nbrPtr;
}
}
}
}
if(newSafestPtr != NULL){
//if we have a new safest guide with low weight, compare to current and switch or not
if(newSafestPtr->ID != safestGuidePtr->ID){
//switch guides
safestGuidePtr = newSafestPtr;
}
}
/*** Tell Others What to Do ***/
break;
}
case MODE_INACTIVE: {
behOutput = behInactive;
//cprintf("inactive \n");
ledsSetPattern(LED_RED, LED_PATTERN_CIRCLE, LED_BRIGHTNESS_MED,LED_RATE_MED);
break;
}
case MODE_LEADERSTOP: {
behOutput = behInactive;
ledsSetPattern(LED_RED, LED_PATTERN_PULSE, LED_BRIGHTNESS_MED,LED_RATE_MED);
break;
}
case MODE_TRANSLATE: {
translatetime = translatetime + 1;
if (broadcastMsgIsSource(&broadcastMessage)) {
//Pulsing Green LEDS indicate waiting to translate
//Circling Green LEDS indicate translation has begun
if (translatetime <= 100) {
behSetTvRv(&behOutput, 0, 0);
ledsSetPattern(LED_GREEN, LED_PATTERN_PULSE,LED_BRIGHTNESS_LOW, LED_RATE_MED);
} else {
behSetTvRv(&behOutput, TV, 0);
ledsSetPattern(LED_GREEN, LED_PATTERN_CIRCLE,LED_BRIGHTNESS_LOW, LED_RATE_MED);
}
} else {
int32 tvgain, error;
alpha = behFlockAngle(&nbrList);
error = alpha / 100;
flockAngleIntegral = flockAngleIntegral * FLOCK_INTEGRAL_DECAY / 1000;
flockAngleIntegral += (error * error) * (error > 0 ? 1 : -1);
flockAngleIntegral = bound(flockAngleIntegral,-FLOCK_INTEGRAL_MAX, FLOCK_INTEGRAL_MAX);
tvgain = 100 - (abs(flockAngleIntegral) * 100) / FLOCK_INTEGRAL_MAX;
int32 rv_I = flockAngleIntegral * K_I / 100; //WEIGHTED TEST WORKS WITH 2
int32 rv_P = error * K_P / 100; //WEIGHTED TEST WORKS WITH 2
if (printNow) {
cprintf(
"error % 4d flockAngleIntegral% 5d rv_I% 4d rv_P% 4d \n",
alpha, flockAngleIntegral, rv_I, rv_P);
}
behOutput.rv = (rv_I + rv_P);
behOutput.active = TRUE;
if (translatetime <= 100) {
//If waiting to start translating, pulse green LEDS
behOutput.tv = 0;
ledsSetPattern(LED_GREEN, LED_PATTERN_PULSE,LED_BRIGHTNESS_LOW, LED_RATE_MED);
} else {
//If translating, circling green LEDS
behOutput.tv = 2 * FLOCK_TV * tvgain / 100;
ledsSetPattern(LED_GREEN, LED_PATTERN_CIRCLE,LED_BRIGHTNESS_LOW, LED_RATE_MED);
}
}
break;
}
case MODE_ROTATE: {
rotateTime = rotateTime + 1;
if (broadcastMsgIsSource(&broadcastMessage)) {
ledsSetPattern(LED_BLUE, LED_PATTERN_PULSE, LED_BRIGHTNESS_MED,LED_RATE_MED);
behOutput = behInactive;
nbrDataSet(&msgLeaderPosX, 0);
nbrDataSet(&msgLeaderPosY, 0);
} else {
uint16 speed = LED_RATE_MED;
ledsSetPattern(LED_BLUE, LED_PATTERN_CIRCLE, LED_BRIGHTNESS_MED,LED_RATE_MED);
int32 tv_gain, alpha;
tv = 0;
rv = 0;
tv_gain = 0;
behOutput.active = TRUE;
if (leaderBearing > 0)
alpha = normalizeAngleMilliRad2(leaderBearing - (int32) MILLIRAD_DEG_90);
else
alpha = normalizeAngleMilliRad2(leaderBearing + (int32) MILLIRAD_DEG_90);
int32 FLOCK_RV_GAIN = 50;
rv = alpha * FLOCK_RV_GAIN / 100;
tv_gain = 1;
tv = tv_gain * leaderDist / 20;
if (senderID_seen == 1){
behOutput.rv = rv;
behOutput.tv = tv;
} else {
behOutput.rv = 0;
behOutput.tv = 0;
}
}
break;
}
} //end mode switch
//Set behavior and finish this task period
motorSetBeh(&behOutput);
neighborsPutMutex();
osTaskDelayUntil(&lastWakeTime, BEHAVIOR_TASK_PERIOD);
} //forever for loop
} //behaviorTask()
// behaviors run every 50ms. They should be designed to be short, and terminate quickly.
// they are used for robot control. Watch this space for a simple behavior abstraction
// to appear.
void behaviorTask(void* parameters) {
uint32 lastWakeTime = osTaskGetTickCount();
Beh behOutput;
uint32 neighborRound = 0;
boolean printNow;
uint8 mode = MODE_SENSE;
char string[18];
static uint32 printTime = 0;
uint32 buttonTime = 0;
int32 i;
uint8 tileIDOld;
neighborsInit(300);
// wait for a sec to get initial magnetometer values
osTaskDelay(500);
magSetOffset();
// init the sound task
playworksSoundInit();
tileIDOld = bumpSensorsGetBits();
while (TRUE) {
behOutput = behInactive;
neighborsGetMutex();
if ((osTaskGetTickCount() - printTime) > PRINT_TIME) {
printTime += PRINT_TIME;
printNow = TRUE;
} else {
printNow = FALSE;
}
switch (mode) {
case MODE_PROGRAM: {
//ledsSetPattern(LED_RED, LED_PATTERN_PULSE, LED_BRIGHTNESS_LOW, LED_RATE_MED);
if (buttonsGetEdge(BUTTON_RED)) {
// add a left turn
waypointListAdd(COMMAND_LEFT);
ledsSetPattern(LED_RED, LED_PATTERN_ON, LED_BRIGHTNESS_HIGH, LED_RATE_MED);
buttonTime = osTaskGetTickCount();
}
if (buttonsGetEdge(BUTTON_GREEN)) {
// add a forward motion
waypointListAdd(COMMAND_FORWARD);
ledsSetPattern(LED_GREEN, LED_PATTERN_ON, LED_BRIGHTNESS_HIGH, LED_RATE_MED);
buttonTime = osTaskGetTickCount();
}
if (buttonsGetEdge(BUTTON_BLUE)) {
// add a right turn
waypointListAdd(COMMAND_RIGHT);
ledsSetPattern(LED_BLUE, LED_PATTERN_ON, LED_BRIGHTNESS_HIGH, LED_RATE_MED);
buttonTime = osTaskGetTickCount();
}
if ((osTaskGetTickCount() - buttonTime) > BUTTON_PRESS_TIME) {
ledsSetPattern(LED_ALL, LED_PATTERN_CIRCLE, LED_BRIGHTNESS_LOW, LED_RATE_MED);
}
if (((osTaskGetTickCount() - buttonTime) > COMMAND_EXECUTE_TIME) &&
(waypointListWriteIdx > 0)) {
waypointListReadIdx = 0;
mode = MODE_MOVE;
currentCommand = waypointReadCommand();
}
break;
}
case MODE_MOVE: {
// read the waypoint list and move to the next waypoint
switch (currentCommand) {
case COMMAND_FORWARD: {
uint32 distance = encoderGetOdometer() - motionOdometerStart;
uint32 distanceToGoal = TILE_WIDTH - distance;
int32 tv = computeVelRamp(MOTION_TV, MOTION_TV_MIN, MOTION_TV_RAMP_DISTANCE, distance, distanceToGoal);
if (distance > TILE_WIDTH) {
cprintf(",done\n");
currentCommand = waypointReadCommand();
} else {
if (printNow) cprintf(",%d", distance);
behSetTvRv(&behOutput, tv, 0);
}
ledsSetPattern(LED_GREEN, LED_PATTERN_PULSE, LED_BRIGHTNESS_HIGH, LED_RATE_FAST);
break;
}
case COMMAND_LEFT: {
Pose pose;
encoderGetPose(&pose);
int32 angle = abs(smallestAngleDifference(motionPoseStart.theta, pose.theta));
int32 angleToGoal = MILLIRAD_HALF_PI - angle;
int32 rv = computeVelRamp(MOTION_RV, MOTION_RV_MIN, MOTION_RV_RAMP_DISTANCE, angle, angleToGoal);
if (angle > MILLIRAD_HALF_PI) {
cprintf(",done\n");
currentCommand = waypointReadCommand();
} else {
if (printNow) cprintf(",%d", angle);
behSetTvRv(&behOutput, 0, rv);
}
ledsSetPattern(LED_RED, LED_PATTERN_PULSE, LED_BRIGHTNESS_HIGH, LED_RATE_FAST);
break;
}
case COMMAND_RIGHT: {
Pose pose;
encoderGetPose(&pose);
int32 angle = abs(smallestAngleDifference(motionPoseStart.theta, pose.theta));
int32 angleToGoal = MILLIRAD_HALF_PI - angle;
int32 rv = computeVelRamp(MOTION_RV, MOTION_RV_MIN, MOTION_RV_RAMP_DISTANCE, angle, angleToGoal);
if (angle > MILLIRAD_HALF_PI) {
cprintf(",done\n");
currentCommand = waypointReadCommand();
} else {
if (printNow) cprintf(",%d", angle);
behSetTvRv(&behOutput, 0, -rv);
}
ledsSetPattern(LED_BLUE, LED_PATTERN_PULSE, LED_BRIGHTNESS_HIGH, LED_RATE_FAST);
break;
}
case COMMAND_DONE: {
cprintf(",done\n");
waypointListWriteIdx = 0;
waypointListReadIdx = 0;
mode = MODE_PROGRAM;
}
default:
break;
}
break;
}
default:
case MODE_SENSE:
case MODE_LINE_FOLLOW: {
static int32 magMagFilteredLeft = 0;
static int32 magMagFilteredRight = 0;
static uint8 turningDirection = ROTATE_RIGHT;
static int32 rvLineFollow = 0;
static int32 tvLineFollow = 0;
static uint32 rvDirectionChangeTime = 0;
char rvDirChange[] = " ";
if (buttonsGet(BUTTON_RED)) {
magSetOffset();
}
if (buttonsGet(BUTTON_GREEN)) {
mode = MODE_LINE_FOLLOW;
}
int32 magValues[MAG_AXES];
int32 x, y, z;
for (i = 0; i < MAG_AXES; i++) {
magValues[i] = magGetValueLeftRaw(i) - magOffsetValuesLeft[i];
}
x = magValues[MAG_X_AXIS];
y = magValues[MAG_X_AXIS];
z = magValues[MAG_X_AXIS];
int32 magMagLeft = sqrtInt(x*x + y*y + z*z);
magMagLeft = magMagLeft * 100 / 570;
int32 magMagFilteredPrevLeft = magMagFilteredLeft;
magMagFilteredLeft = filterIIR(magMagFilteredLeft, magMagLeft, MAG_IIR_TIME_CONSTANT);
for (i = 0; i < MAG_AXES; i++) {
magValues[i] = magGetValueRightRaw(i) - magOffsetValuesRight[i];
}
x = magValues[MAG_X_AXIS];
y = magValues[MAG_X_AXIS];
z = magValues[MAG_X_AXIS];
int32 magMagRight = sqrtInt(x*x + y*y + z*z);
magMagRight = magMagRight * 100 / 7565;
int32 magMagFilteredPrevRight = magMagFilteredRight;
magMagFilteredRight = filterIIR(magMagFilteredRight, magMagRight, MAG_IIR_TIME_CONSTANT);
//if (printNow) cprintf("mag,% 6d,% 6d,% 6d,% 6d,% 6d\n", x, y, z, magMag, magMagFiltered);
//if (printNow) cprintf("left % 6d,% 6d right % 6d,% 6d\n", magMagLeft, magMagFilteredLeft, magMagRight, magMagFilteredRight);
uint8 tileID = bumpSensorsGetBits();
if (tileIDOld != tileID) {
playTileSound(tileID);
tileIDOld = tileID;
}
if (printNow) cprintf("tile % 4d, left % 6d right % 6d\n", tileID, magMagFilteredLeft, magMagFilteredRight);
if (mode == MODE_LINE_FOLLOW) {
int32 diff = magMagFilteredLeft - magMagFilteredRight;
// int32 rvDiff = rvLineFollowNew - rvLineFollow;
// int32 tvLineFollowNew = 0;
// if (rvDiff > RV_MAX_DIFF) {
// rvLineFollow += RV_MAX_DIFF;
// tvLineFollowNew = MOTION_TV/6;
// ledsSetPattern(LED_RED, LED_PATTERN_ON, LED_BRIGHTNESS_HIGH, LED_RATE_FAST);
// }else if (rvDiff < -RV_MAX_DIFF) {
// rvLineFollow -= RV_MAX_DIFF;
// tvLineFollowNew = MOTION_TV/6;
// ledsSetPattern(LED_BLUE, LED_PATTERN_ON, LED_BRIGHTNESS_HIGH, LED_RATE_FAST);
// }else {
// rvLineFollow = rvLineFollowNew;
// tvLineFollowNew = MOTION_TV/2;
// ledsSetPattern(LED_GREEN, LED_PATTERN_PULSE, LED_BRIGHTNESS_HIGH, LED_RATE_FAST);
// }
int32 rvLineFollowNew = diff * RV_GAIN / 100;
int32 tvLineFollowNew = 0;
if (rvLineFollowNew > RV_MAX) {
//rvLineFollow += RV_MAX_DIFF;
//rvLineFollow = RV_MAX;
tvLineFollowNew = MOTION_TV/3;
ledsSetPattern(LED_RED, LED_PATTERN_ON, LED_BRIGHTNESS_HIGH, LED_RATE_FAST);
}else if (rvLineFollowNew < -RV_MAX) {
//rvLineFollow -= RV_MAX_DIFF;
//rvLineFollow = -RV_MAX;
tvLineFollowNew = MOTION_TV/3;
ledsSetPattern(LED_BLUE, LED_PATTERN_ON, LED_BRIGHTNESS_HIGH, LED_RATE_FAST);
} else {
//rvLineFollow = rvLineFollowNew;
tvLineFollowNew = MOTION_TV;
ledsSetPattern(LED_GREEN, LED_PATTERN_PULSE, LED_BRIGHTNESS_HIGH, LED_RATE_FAST);
}
rvLineFollow = filterIIR(rvLineFollow, rvLineFollowNew, 50);
tvLineFollowNew = MOTION_TV - MOTION_TV * abs(rvLineFollowNew) / RV_MAX;
if ((tvLineFollowNew - tvLineFollow) > TV_MAX_DIFF) {
tvLineFollow += TV_MAX_DIFF;
}else if ((tvLineFollowNew - tvLineFollow) < -TV_MAX_DIFF) {
tvLineFollow -= TV_MAX_DIFF;
} else {
tvLineFollow = tvLineFollowNew;
}
behSetTvRv(&behOutput, tvLineFollowNew, rvLineFollow);
if (printNow) cprintf("left % 6d | right % 6d | rv % 6d | tv % 6d\n", magMagFilteredLeft, magMagFilteredRight, rvLineFollow, tvLineFollow);
} else {
ledsSetPattern(LED_GREEN, LED_PATTERN_CIRCLE, LED_BRIGHTNESS_HIGH, LED_RATE_MED);
}
break;
}
}
//behSetTvRv(&behOutput, -200, 0);
motorSetBeh(&behOutput);
neighborsPutMutex();
osTaskDelayUntil(&lastWakeTime, 20);
}
}
