//---- Square Root ----
// Inputs: x is the the operand
// Outputs: the square root of x
// Calculates a square root
// Uses the fact that sqrt(A * B) = sqrt(A) * sqrt(B)
fixed32_3 sqrtFix(fixed32_3 n){
if(n<0){
return -1;
}else{
return udiv32_3_lhp(sqrtInt(n) * 1000, 31623);
}
}
int isPrime(size_t a) {
size_t tmp = sqrtInt(a);
static size_t Sqrt = 0;
static int* tab = NULL;
if( Sqrt < tmp) {
Sqrt = tmp;
int isComp = 0;
if( tab ) free(tab);
tab = calloc( Sqrt+1, sizeof *tab);
if( !tab ) return -1;
for( size_t i = 2 ; i <= Sqrt ; i++) {
if( tab[i] )
continue;
if ( a % i == 0) {
isComp = 1;
break;
}
for(int j = i+i ; j <= Sqrt ; j+=i)
tab[j] = 1;
}
return !isComp;
}
for(int i = 2 ; i < Sqrt ; i++ )
if( tab[i] )
continue;
else if( a % i == 0 )
return 0;
return 1;
}
int32 centroidLiteGetDistance() {
centroidValue centroidX, centroidY;
centroidDataGet(¢roidX, ¢roidY, ¢roidLite);
int16 iCentroidX = (int16) centroidX;
int16 iCentroidY = (int16) centroidY;
return sqrtInt(iCentroidX * iCentroidX + iCentroidY * iCentroidY);
}
// 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()
// 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);
}
}
