void AutonomousPeriodic(void) {
		m_autoPeriodicLoops++;
#if 0
		static int Clock=0;
		bool correct = DriveStick->GetButton(Joystick::kTriggerButton);
		bool Reset = DriveStick->GetButton (Joystick::kTopButton);
		ds->PrintfLine(DriverStationLCD::kUser_Line1, "%s %s",
				correct ? "correct on" : "correct off",
						Reset ? "Reset": "No Reset");
		//ds->PrintfLine(DriverStationLCD::kUser_Line6, "%d %c %c", Clock, correct? "C" : "c", Reset? "R" : "r");
		if (Reset)
			Clock=0, MyRobot.ResetCounters();
		else
		{
		    ++Clock;
		    if(Clock<=100)       MyRobot.DriveRobot(1.0,0.0, ds, correct);		// drive forward
		    else if (Clock<=200) MyRobot.DriveRobot(-1.0,0.0, ds, correct);  // stop
		    else if (Clock<=250) MyRobot.DriveRobot(-1.0,0.0, ds, correct);  // drive back halfway
		    else if (Clock<=300) MyRobot.DriveRobot(1.0,0.0, ds, correct);   // stop
		    else
		    {
		    	// Real teleop mode: use the JoySticks to drive
		    	MyRobot.DriveRobot(DriveStick->GetY(),(DriveStick->GetX()), ds);
		    }
		}
#endif
	}
	void TeleopPeriodic(void) {
		// increment the number of teleop periodic loops completed
		m_telePeriodicLoops++;
		static AutoDrive *autoDrive = NULL;
		bool autoButton = DriveStick->GetButton(Joystick::kTriggerButton);
		if (autoButton)
		{
			if (autoDrive == NULL)
				autoDrive = new AutoDrive(m_Configuration->GetValue( m_Constant[ cAutoDrive]) * 100);
			autoDrive->Periodic(MyRobot, ds);
			ds->PrintfLine (DriverStationLCD::kUser_Line5, "autoDrive on");
		}
		else
		{
			ds->PrintfLine (DriverStationLCD::kUser_Line5, "autoDrive off");
			if (autoDrive != NULL)
			{
				MyRobot.ResetCounters();
				delete autoDrive;
				autoDrive = NULL;
			}

			if( !m_Configuration)
			{
				printf( "Configuration Initialize");
				InitializeConfiguration();
			}
			
			m_Configuration->Execute( DriveStick->GetRawButton( 2), DriveStick->GetZ(), ds);
			
			if(DriveStick->GetRawButton( 3))
			{
				ds->PrintfLine(DriverStationLCD::kUser_Line6, "Calculating distance...");
				Vision *vision = new Vision();
				double distance = vision->TakeDistancePicture( ds, m_Configuration->GetValue( m_Constant[ cHBottom]), m_Configuration->GetValue( m_Constant[ cHTop]), m_Configuration->GetValue( m_Constant[ cSBottom]), m_Configuration->GetValue( m_Constant[ cSTop]), m_Configuration->GetValue( m_Constant[ cVBottom]), m_Configuration->GetValue( m_Constant[ cVTop]));
				if( distance < 0.000001)
					ds->PrintfLine(DriverStationLCD::kUser_Line6, "No target found");
				else
					ds->PrintfLine(DriverStationLCD::kUser_Line6, "distance to target: %lf",distance);
				delete vision;
			}

			
			// Real teleop mode: use the JoySticks to drive
				MyRobot.DriveRobot(DriveStick->GetY(),(DriveStick->GetX()), ds);
		}

    	ds->UpdateLCD();
	} // TeleopPeriodic(void)
		void Periodic(EntropyDrive& MyRobot, DriverStationLCD* ds)
		{
			unsigned int ctrs = (unsigned)(Reverse? -1 : 1) * MyRobot.GetCounters();
			ds->PrintfLine(DriverStationLCD::kUser_Line2, "c%d d%d t%d ",ctrs, Distance, t);
			if (ctrs < (Distance - t))
				MyRobot.DriveRobot(Reverse?1.0:-1.0, 0.0, ds);
			else
				MyRobot.DriveRobot(0.0, 0.0, ds);
			if (t == 0)
			{
				if (MyRobot.AtTopSpeed()) t = ctrs;
				if (ctrs > Distance/2) t = ctrs;
				// if t is just way bigger than we meant to travel, set it to the distance (which will stop us)
				if (t >= Distance) t = Distance;
			}
		}
	void RobotInit(void) {
		// Actions which would be performed once (and only once) upon initialization of the
		// robot would be put here.
		
		
		// Initialize SHS Subsystems here
		MyRobot.Initialize();
		Arm.Initialize();
		
		printf("RobotInit() completed.\n");
	}
예제 #5
0
	void TeleopPeriodic(void) {
		// increment the number of teleop periodic loops completed
		m_telePeriodicLoops++;
		
		//Feed joystick inputs to each subsystem here
		
		
		//Using triggers to turn;
		ds->Printf(DriverStationLCD::kUser_Line1,0, "GetY: %f",DriveStick->GetY());
		ds->Printf(DriverStationLCD::kUser_Line2,0, "GetZ: %f",DriveStick->GetZ());
		MyRobot.DriveRobot(DriveStick->GetY(),(-DriveStick->GetZ()));
		//original:MyRobot.DriveRobot(DriveStick->GetY(),DriveStick->GetX());
		//MyRobot.DriveRobotTrig(DriveStick->GetY(),DriveStick->GetX());
		
	} // TeleopPeriodic(void)
	void TeleopPeriodic(void) {
		// increment the number of teleop periodic loops completed
		m_telePeriodicLoops++;
		
		//Feed joystick inputs to each subsystem here		
		MyRobot.DriveRobot(DriveStick->GetY(),DriveStick->GetX());
		Arm.Update();
		Arm.UpperVerticalPos(GameStick);
		Arm.LowerVerticalPos(GameStick);
		Arm.Extend(GameStick);
		Arm.BeltEnable(GameStick);
			
		//MyRobot.DriveRobotTrig(DriveStick->GetY(),DriveStick->GetX());

//		DriverStationLCD::GetInstance()->Printf(DriverStationLCD::kUser_Line1, 1, "Sensor output: %f", InfraredSensor.GetValue());
//		DriverStationLCD::GetInstance()->UpdateLCD();

		
	} // TeleopPeriodic(void)
예제 #7
0
	void DisabledPeriodic(void)  {
		static INT32 printSec = (INT32)GetClock() + 1;
		static const INT32 startSec = (INT32)GetClock();


		// increment the number of disabled periodic loops completed
		m_disabledPeriodicLoops++;
		
		//Disable Drive
		MyRobot.Cleanup();
		//MyShooter.Cleanup();
		
		// while disabled, printout the duration of current disabled mode in seconds
		if (GetClock() > printSec) {
			// Move the cursor back to the previous line and clear it.
			printf("\x1b[1A\x1b[2K");
			printf("Disabled seconds: %d\r\n", printSec - startSec);			
			printSec++;
		}
	}