int Kernel::Execute() {
LOGGER.Write(LOG_APP, "Kernel executing.");
while (taskList.size()) {
Task *t;
std::list< MMPtr<Task> >::iterator it;
for (it = taskList.begin(); it != taskList.end(); ) {
t = (*it);
it++;
if (!t->canKill) t->Update();
}
for (it = taskList.begin(); it != taskList.end(); ) {
t = (*it);
it++;
if (t->canKill) {
t->Stop();
taskList.erase(t->pos);
std::string rslt = "Task ID: " + std::to_string(t->getId()) + " removed.";
LOGGER.Write(LOG_APP, rslt.c_str());
t = 0;
}
}
//TODO: Move this to a garbage collection task.
MMObject::collectGarbage();
}
return 0;
}
void OperatorControl(void)
{
myRobot->SetSafetyEnabled(false);
LEDLights (true); //turn camera lights on
shooterspeedTask->Start((UINT32)this); //start counting shooter speed
kickerTask->Start((UINT32)this); //turns on the kicker task
kicker_in_motion = false;
while (IsOperatorControl() && !IsDisabled())
{
if (ControllBox->GetDigital(3)) //turn tracking on with switch 3 on controll box
{
tracking(ControllBox->GetDigital(7));
}
else
{
myRobot->TankDrive(leftstick, rightstick); //if tracking is off, enable human drivers
Wait(0.005); // wait for a motor update time
}
Shooter_onoff=ControllBox->GetDigital(4); //shoot if switch 4 is on
ballgatherer(ControllBox->GetDigital(5), rightstick->GetRawButton(10));
kicker_onoff=lonelystick->GetRawButton(1);
bridgeboot(ControllBox->GetDigital(6));
kicker_cancel=lonelystick->GetRawButton(2);
//kicker_down=rightstick->GetRawButton(11));
}
LEDLights (false);
shooterspeedTask->Stop();
kickerTask->Stop();
ballgatherer(false, false);
kickermotor->Set(Relay::kOff);
}
/**
* Drive left & right motors for 2 seconds then stop
*/
void Autonomous(void)
{
myRobot->SetSafetyEnabled(false);
//shooter on
kickerTask->Start((UINT32)this);
shooterspeedTask->Start((UINT32)this);
Shooter_onoff=true;
//if (speederror < 10);
//track+adjust
LEDLights(true);
//turn tracking on
//while (tracking(false) == false) {
// }
//if while returns true, then shoot
//might have to wait for encoder once capabilities have been enabled
Wait(2.0);
AutonomousShooting(true);
AutonomousShooting(true);
AutonomousShooting(true);
//load
/*
Wait(1.0);
kicker_onoff = true;
while(kicker_in_motion == false) {
Wait(0.005);
printf ("kicker_onoff is false, kicker_onoff is true\n");
}
kicker_onoff = false;
while (kicker_in_motion == true) {
Wait(0.005);
printf ("kicker_in_motion is true, kicker_onoff is false\n");
}
//shoot, by itself, because the shooter motor was already on.
//gather
ballgatherer(true, false);
Wait(4.0);
printf ("ballgatherer on\n");
ballgatherer(false, false);
printf ("ballgatherer off\n");
//load
kicker_onoff = true;
while(kicker_in_motion == false) {
Wait(0.005);
printf ("kicker_in_motion is false, kicker_onoff is true\n");
}
kicker_onoff = false;
while (kicker_in_motion == true) {
Wait(0.005);
printf ("kicker_in_motion is true, kicker_onoff is false \n");
}
//shoot *does by itself because the shooter is already on, AGAIN! :D
ballgatherer(true, false);
Wait(4.0);
printf ("ballgatherer on\n");
ballgatherer(false, false);
printf ("ballgatherer off\n");
kicker_onoff = true;
while(kicker_in_motion == false) {
Wait(0.005);
printf ("kicker_onoff is false, kicker_onoff is true\n");
}
kicker_onoff = false;
while (kicker_in_motion == true) {
Wait(0.005);
printf ("kicker_in_motion is true, kicker_onoff is false\n");
}
//shoot, by itself, because the shooter motor was already on.
//gather
*/
kickerTask->Stop();
shooterspeedTask->Stop();
Shooter_onoff=false;
LEDLights(false);
}
~RobotSystem() {
debug("Deleting robot");
updateCAN.Stop();
cameraTask.Stop();
}
