void AutonomousInit(){
SmartDashboard::PutString("AutonomousInit", "Autonomous Init Reached");
autonomous->setSelected(*((std::string*)chooser->GetSelected()));
//autoSelected = SmartDashboard::GetString("Auto Selector", *((std::string*)chooser->GetSelected()));
autoSelected = *((std::string*)chooser->GetSelected());
SmartDashboard::PutString("Auto Selected", autoSelected);
//testSelected= SmartDashboard::GetString("Auto Selector", *((std::string*)chooser2->GetSelected()));
testSelected = *((std::string*)chooser2->GetSelected());
Drive->AutonomousInit();
autonomous->startTimer();
autonomous->reset();
}
/*Initializes objects necessary for teleop
* Adds options to sendable chooser on SmartDashboard
*/
void RobotInit(){
/*if (fork()==0){
system("/home/lvuser/start_vision &");
}*/
//creates DriveTrain and sendable choosers
Drive= new DriveTrain();
chooser = new SendableChooser();
chooser2 = new SendableChooser();
//adds options to both
chooser->AddDefault(DoNothing, (void*)&DoNothing);
chooser->AddObject(autoNameLeft, (void*)&autoNameLeft);
chooser->AddObject(autoNameRight, (void*)&autoNameRight);
chooser->AddObject(autoNameOver, (void*)&autoNameOver);
chooser->AddObject(autoNameP1, (void*)&autoNameP1);
chooser->AddObject(autoNameP2, (void*)&autoNameP2);
chooser->AddObject(autoNameP3, (void*)&autoNameP3);
chooser->AddObject(autoNameP4, (void*)&autoNameP4);
chooser->AddObject(autoNameP5, (void*)&autoNameP5);
chooser->AddObject(autoNameRW, (void*)&autoNameRW);
SmartDashboard::PutData("Auto Modes", chooser);
chooser2->AddDefault(autoNameTest1, (void*)&autoNameTest1);
chooser2->AddObject(autoNameTest2, (void*)&autoNameTest2);
chooser2->AddObject(autoNameTest3, (void*)&autoNameTest3);
chooser2->AddObject(autoNameTest4, (void*)&autoNameTest4);
SmartDashboard::PutData("Auto Modes 2", chooser2);
Drive->AutonomousInit();
autonomous = new Autonomous(Drive, *((std::string*)chooser->GetSelected()));
//passes driveTrain and sendable chooser option to Autonomous class and creates it!
}