/*!
* @brief Thread handling the RTC
*/
void ToggleThread(void *data)
{
LEDs_Off(LED_BLUE);
LEDs_Off(LED_GREEN);
LEDs_Off(LED_YELLOW);
LEDs_Off(LED_ORANGE);
Touch_EnableScan(bTRUE);
uint8_t values = 0;
for (;;)
{
//At least 5s, so 501 * 10ms > 5s
if (Touch_Wait(501, &values) == OS_TIMEOUT)
{
break;
}
SetLeds(values);
}
LEDs_Off(LED_BLUE);
LEDs_Off(LED_GREEN);
LEDs_Off(LED_YELLOW);
LEDs_Off(LED_ORANGE);
Touch_EnableScan(bFALSE);
(*Callback)((void *)0);
OS_ERROR error = OS_ThreadDelete(OS_PRIORITY_SELF);
}
void ModeSelect::StartModeSelect(void)
{
mode_new = 0;
mode_act = 0;
blinkcnt = 0;
timecnt = 0;
EELIB_Command command;
EELIB_Result result;
uint8_t buf8;
button_state = ReadButton();
button_debounce = 0;
command[0] = EELIB_IAP_COMMAND_EEPROM_READ;
command[1] = 0; // Eeprom address
command[2] = (uint32_t) &buf8;
command[3] = 1; // Read length
command[4] = SystemCoreClock / 1000;
EELIB_entry(command, result);
if (result[0] != EELIB_IAP_STATUS_CMD_SUCCESS) {
buf8 = 0;
}
if (buf8 >= MODENUM)
buf8 = 0;
mode_new = buf8;
mode_act = buf8;
SetLeds();
}
void Robot::TeleopInit() {
double startTime = GetRTC();
printf("Robot: TeleopInit at %f seconds, %d ticks", startTime, ticks);
// If we'd previously run an autonomous, record how long it lasted
if (m_startTicks > 0) {
double elapsedTime = startTime - m_startTime;
int elapsedTicks = ticks - m_startTicks;
printf(", prior autonomous ran for %f seconds, %d ticks, %5.2f tps.",
elapsedTime, elapsedTicks, elapsedTicks/elapsedTime);
}
printf("\n");
m_startTicks = ticks;
m_startTime = startTime;
// uncomment only for calibrating vision
// CommandBase::navXSubsystem->zeroYaw(0);
if (mp_autonomousCommand != NULL) {
mp_autonomousCommand->Cancel();
}
// begin teleop "polling" commands
CommandBase::driveJoystickCommand->Start();
CommandBase::intakeRollerCommand->Start();
CommandBase::flapCommand->Start();
SetLeds(NORMAL);
}
void vParTestToggleLED( unsigned portBASE_TYPE uxLED )
{
/* Toggle the state of the requested LED. */
if (uxLED < partstNUM_LEDS)
{
ulLEDReg ^= ( 1 << uxLED );
SetLeds( ulLEDReg );
}
}
// alle 10 ms aufrufen
bool ModeSelect::DoModeSelect(void)
{
if (ButtonDebounce())
{
mode_new = mode_new+1;
if (mode_new >= MODENUM)
mode_new = 0;
blinkcnt = 7;
timecnt = MODELED_HPRD;
SetLeds();
} else if (blinkcnt != 0)
{
if (timecnt > 0)
timecnt--;
if (timecnt == 0)
{
blinkcnt--;
if (blinkcnt > 0)
{
timecnt = MODELED_HPRD;
SetSingleLed(mode_new, (blinkcnt & 1) != 0);
} else {
if (mode_act != mode_new)
{
mode_act = mode_new;
SetSingleLed(mode_new, true);
// Neuen Modus noch abspeichern.
uint32_t command[5], result[4];
uint8_t buf8 = mode_act;
command[0] = EELIB_IAP_COMMAND_EEPROM_WRITE;
command[1] = 0; // Eeprom address
command[2] = (uint32_t) &buf8;
command[3] = 1; // Write length
command[4] = SystemCoreClock / 1000;
EELIB_entry(command, result);
if (result[0] != EELIB_IAP_STATUS_CMD_SUCCESS) {
// doof das...
}
return true;
}
}
} else {
// nichts zu tun
}
} else {
// nichts zu tun
}
return false;
}
void vParTestSetLED( unsigned portBASE_TYPE uxLED, signed portBASE_TYPE xValue )
{
/* Switch an LED on or off as requested. */
if (uxLED < partstNUM_LEDS)
{
if( xValue )
{
ulLEDReg &= ~( 1 << uxLED );
}
else
{
ulLEDReg |= ( 1 << uxLED );
}
SetLeds( ulLEDReg );
}
}
void Robot::DisabledInit() {
double startTime = GetRTC();
printf("Robot: Disabled at %f seconds, %d ticks", startTime, ticks);
// If we'd previously run an autonomous, record how long it lasted
if (m_startTicks > 0) {
double elapsedTime = startTime - m_startTime;
int elapsedTicks = ticks - m_startTicks;
printf(", prior state ran for %f seconds, %d ticks, %5.2f tps.",
elapsedTime, elapsedTicks, elapsedTicks/elapsedTime);
}
printf("\n");
m_startTicks = ticks;
m_startTime = startTime;
if (mp_autonomousCommand != NULL) {
mp_autonomousCommand->Cancel();
}
printf("LEDs: Turning off\n");
SetLeds(OFF);
}
void Robot::AutonomousInit() {
m_startTicks = ticks;
m_startTime = GetRTC();
printf("Robot: AutononmousInit at %f seconds, %d ticks\n",
m_startTime, ticks);
int pos = m_fieldInfo.GetPosition();
int def = m_fieldInfo.GetDefense();
int target = m_fieldInfo.GetTarget();
bool slow = m_fieldInfo.IsCrossingSlowly();
printf("Autonomous: Position %d | Defense %d | Target %d, Speed %s\n",
pos, def, target, slow ? "SLOW" : "NORMAL");
// make sure we don't inadvertently leave the LED ring off
CommandBase::visionSubsystem->setLedRingState(true);
// if we've already run auto, delete the existing auto command
if (mp_autonomousCommand != NULL) {
delete mp_autonomousCommand;
mp_autonomousCommand = NULL;
}
// create a new dynamic auto command with the selected parameters from the SmartDashboard
mp_autonomousCommand = new AutonomousDriveAndShoot(pos, def, target);
mp_autonomousCommand->Start();
// reset our coordinate system to where the robot is currently located / oriented
// assume robot was set up correctly (there's nothing else we can do)
// Robot will start out wedges first, or intake first, depending on the defense
CommandBase::navXSubsystem->zeroYaw(m_fieldInfo.GetInitialOrientation());
CommandBase::navXSubsystem->ResetDisplacement();
CommandBase::driveSubsystem->SetInitialPosition(FieldInfo::startingLocations[pos].x,
FieldInfo::startingLocations[pos].y);
CommandBase::driveJoystickCommand->Cancel();
CommandBase::intakeRollerCommand->Cancel();
CommandBase::flapCommand->Cancel();
SetLeds(NORMAL);
}
