void LEDHandler::setRightEye(LEDRequest& ledRequest)
{
//right eye -> groundContact ? role : role -> blinking
// + penalty shootout: native_{striker,keeper} ? {striker,keeper} : off
LEDRequest::LEDState state = theBehaviorLEDRequest.modifiers[BehaviorLEDRequest::rightEye];
//no groundContact
if(!theGroundContactState.contact/* && (theFrameInfo.time & 512)*/)
setEyeColor(ledRequest, false, BehaviorLEDRequest::blue, state);
//overwrite
else if(theBehaviorLEDRequest.rightEyeColor != BehaviorLEDRequest::defaultColor)
setEyeColor(ledRequest, false, theBehaviorLEDRequest.rightEyeColor, state);
else
{
switch(theBehaviorControlOutput.behaviorStatus.role)
{
case Role::keeper:
setEyeColor(ledRequest, false, BehaviorLEDRequest::blue, state);
break;
case Role::defender:
setEyeColor(ledRequest, false, BehaviorLEDRequest::white, state);
break;
case Role::striker:
setEyeColor(ledRequest, false, BehaviorLEDRequest::red, state);
break;
case Role::undefined:
case Role::none:
//off
break;
default:
ASSERT(false);
}
}
}
void LEDHandler::setLeftEye(LEDRequest& ledRequest)
{
//left eye -> groundContact ? ballSeen and GoalSeen : blue
LEDRequest::LEDState state = theBehaviorLEDRequest.modifiers[BehaviorLEDRequest::leftEye];
//no groundContact
if(!theGroundContactState.contact/* && (theFrameInfo.time & 512)*/)
setEyeColor(ledRequest, true, BehaviorLEDRequest::blue, state);
//overwrite
else if(theBehaviorLEDRequest.leftEyeColor != BehaviorLEDRequest::defaultColor)
//blue
setEyeColor(ledRequest, true, theBehaviorLEDRequest.leftEyeColor, state);
//default
else
{
bool ballSeen = theFrameInfo.getTimeSince(theBallModel.timeWhenLastSeen) < 250;
// TODO: this used to be seeing a whole goal, now it's only at least one post
bool goalSeen = theFrameInfo.getTimeSince(theGoalPercept.timeWhenCompleteGoalLastSeen) < 250;
if(ballSeen && goalSeen)
//red
setEyeColor(ledRequest, true, BehaviorLEDRequest::red, state);
else if(ballSeen)
//white
setEyeColor(ledRequest, true, BehaviorLEDRequest::white, state);
else if(goalSeen)
//green
setEyeColor(ledRequest, true, BehaviorLEDRequest::green, state);
}
}
// executes one of Dash's autonomous behaviors
void DashBot::executeAutoMode(void){
auto_flag = 1; // indicates an autonomous mode is on, if an all_stop is called, resets to zero to end auto behavior
switch (receivedRadioPacket[1]) {
case DASH_TEST:
dashTest();
Serial1.write('3');
case DASH_CIRCLE:
dashCircle();
Serial1.write('3');
break;
case DASH_FIG_8:
dashFig8();
Serial1.write('3');
break;
case DASH_DANCE:
dashDance();
Serial1.write('3');
break;
case DASH_BUMP:
dashBump();
Serial1.write('3');
break;
default:
setEyeColor(0,0,0);
}
}
void DashBot::executeRadioCommand(void)
{
/*
switch (receivedRadioPacket[0]) {
case ALL_STOP:
mode = STOP_MODE;
allStop();
clearRadioPacket();
auto_flag = 0;
break;
case SET_NAME:
setName();
break;
case JOYSTICK_DRIVE:
// joystick drive
mode = JOYSTICK_MODE;
directDrive(receivedRadioPacket[1], receivedRadioPacket[2], receivedRadioPacket[3], receivedRadioPacket[4]);
clearRadioPacket();
break;
case GYRO_DRIVE:
// gyro-assisted driving
stabilizedDrive();
break;
case SET_EYE_COLOR:
setEyeColor(receivedRadioPacket[1], receivedRadioPacket[2], receivedRadioPacket[3]);
clearRadioPacket();
break;
case SEND_INFO_PACKET:
// send name and color to iOS device
sendInfoPacket();
break;
case SET_INFO_PACKET:
// set whether a name/color or sensor packet is being sent
setInfoPacketMode();
break;
case EXECUTE_AUTO_MODE:
executeAutoMode();
clearRadioPacket();
break;
default:
debugBlinkOn();
clearRadioPacket();
*/
switch (receivedRadioPacket[0]){
case 1:
setEyeColor(0,100,0); //green
clearRadioPacket();
break;
case 2:
setEyeColor(0,0,100); //blue
clearRadioPacket();
break;
case 3:
setEyeColor(100,0,0); //red
clearRadioPacket();
break;
case 4:
setEyeColor(0,0,0); //clear
clearRadioPacket();
break;
case 5:
setMessage();
break;
case 6:
setInfoPacketMode();
break;
case 7:
motorL_pwm = receivedRadioPacket[1];
motorR_pwm = receivedRadioPacket[2];
duration = (receivedRadioPacket[3] << 16) + (receivedRadioPacket[4] << 8) + receivedRadioPacket[5];
if (motorL_pwm > 100){
motorL_pwm = motorL_pwm - 100;
motorL_pwm = -1* motorL_pwm;
}
if (motorR_pwm > 100){
motorR_pwm = motorR_pwm - 100;
motorR_pwm = -1* motorR_pwm;
}
driveForTime(motorL_pwm, motorR_pwm, duration);
break;
case 8:
delay_between_sensor_emissions = receivedRadioPacket[1];
break;
case 9:
switch (receivedRadioPacket[3]){
case 1:
servo1.write(receivedRadioPacket[1], receivedRadioPacket[2], false);
break;
case 2:
servo2.write(receivedRadioPacket[1], receivedRadioPacket[2], false);
break;
case 3:
servo3.write(receivedRadioPacket[1], receivedRadioPacket[2], false);
break;
default:
setEyeColor(100,0,100); //purple
clearRadioPacket();
}
break;
/*case 10:
//servo1.write(180, 30, false);
servo2.write(receivedRadioPacket[1], receivedRadioPacket[2], false);
break;*/
case 11:
servo1port = receivedRadioPacket[1];
servo2port = receivedRadioPacket[2];
servo3port = receivedRadioPacket[3];
servo1.attach(servo1port); // attaches the servo on pin 9 to the servo object
servo2.attach(servo2port);
servo3.attach(servo3port);
break;
case 12:
motor_pwm = receivedRadioPacket[1];
if (motor_pwm > 100){
motor_pwm = motor_pwm - 100;
motor_pwm = -1*motor_pwm;
}
motorDriveR(motor_pwm);
break;
case 13:
motor_pwm = receivedRadioPacket[1];
if (motor_pwm > 100){
motor_pwm = motor_pwm - 100;
motor_pwm = -1*motor_pwm;
}
motorDriveL(motor_pwm);
break;
case 14:
servo3.write(receivedRadioPacket[1], receivedRadioPacket[2], false);
break;
default:
setEyeColor(100,0,100); //purple
clearRadioPacket();
}
}
