C++ Task::Start方法代码示例

	RobotSystem(void):
		robotInted(false)
		,stick(1)		// as they are declared above.
		,stick2(2)
		,line1(10)
		,line2(11)
		,line3(12)
		//,camera(AxisCamera::GetInstance())
		,updateCAN("CANUpdate",(FUNCPTR)UpdateCAN)
		,cameraTask("CAMERA", (FUNCPTR)CameraTask)
		,compressor(14,1)
		,EncArm(2,3)
		,EncClaw(5,6)
		,PIDArm(.04,0,0) // .002, .033
		,PIDClaw(.014,.0000014,0)
		,LowArm(.1)
		/*
		,MiniBot1(4)
		,MiniBot2(2)
		,ClawGrip(3)
		*/
		,MiniBot1a(8,1)
		,MiniBot1b(8,2)
		,MiniBot2a(8,3)
		,MiniBot2b(8,4)
		,ClawOpen(8, 8)
		,ClawClose(8,7)
		,LimitClaw(7)
		,LimitArm(13)
	{
	//	myRobot.SetExpiration(0.1);
		GetWatchdog().SetEnabled(false);
		GetWatchdog().SetExpiration(1);
		compressor.Start();
		debug("Waiting to init CAN");
		Wait(2);
		
		Dlf = new CANJaguar(6,CANJaguar::kSpeed);
		Dlb = new CANJaguar(3,CANJaguar::kSpeed);
		Drf = new CANJaguar(7,CANJaguar::kSpeed);
		Drb = new CANJaguar(2,CANJaguar::kSpeed);
		arm1 = new CANJaguar(5);
		arm1_sec = new CANJaguar(8);
		arm2 = new CANJaguar(4);
		
		
		EncArm.SetDistancePerPulse(.00025);
		EncClaw.SetDistancePerPulse(.00025);
		EncClaw.SetReverseDirection(false);
		EncArm.SetReverseDirection(true);
		EncArm.Reset();
		EncClaw.Reset();
		
		
		updateCAN.Start((int)this);
		//cameraTask.Start((int)this);
		EncArm.Start();
		EncClaw.Start();
		debug("done initing");
	}

	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);
	}

	CANRobotDemo()
		: speedJag(2, CANJaguar::kSpeed)
		, stick(1)
		, axis(Joystick::kXAxis)
		, commandTask("DashboardCommandServer", (FUNCPTR)DashboardCommandServerStub)
	{
		GetWatchdog().SetExpiration(100);
		speedJag.ConfigEncoderCodesPerRev(360);
		speedJag.ConfigMaxOutputVoltage(6.0);
		speedJag.ConfigNeutralMode(CANJaguar::kNeutralMode_Brake);
		wpi_stackTraceEnable(true);
		commandTask.Start((INT32)this);
	}

/**
 * 
 * Start the robot code.
 * This function starts the robot code running by spawning a task. Currently tasks seemed to be
 * started by LVRT without setting the VX_FP_TASK flag so floating point context is not saved on
 * interrupts. Therefore the program experiences hard to debug and unpredictable results. So the
 * LVRT code starts this function, and it, in turn, starts the actual user program.
 */
void RobotBase::startRobotTask(FUNCPTR factory)
{
#ifdef SVN_REV
	if (strlen(SVN_REV))
	{
		printf("WPILib was compiled from SVN revision %s\n", SVN_REV);
	}
	else
	{
		printf("WPILib was compiled from a location that is not source controlled.\n");
	}
#else
	printf("WPILib was compiled without -D'SVN_REV=nnnn'\n");
#endif

	// Check for startup code already running
	int32_t oldId = taskNameToId(const_cast<char*>("FRC_RobotTask"));
	if (oldId != ERROR)
	{
		// Find the startup code module.
		char moduleName[256];
		moduleNameFindBySymbolName("FRC_UserProgram_StartupLibraryInit", moduleName);
		MODULE_ID startupModId = moduleFindByName(moduleName);
		if (startupModId != NULL)
		{
			// Remove the startup code.
			unldByModuleId(startupModId, 0);
			printf("!!!   Error: Default code was still running... It was unloaded for you... Please try again.\n");
			return;
		}
		// This case should no longer get hit.
		printf("!!!   Error: Other robot code is still running... Unload it and then try again.\n");
		return;
	}

	// Let the framework know that we are starting a new user program so the Driver Station can disable.
	FRC_NetworkCommunication_observeUserProgramStarting();

	// Let the Usage Reporting framework know that there is a C++ program running
	nUsageReporting::report(nUsageReporting::kResourceType_Language, nUsageReporting::kLanguage_CPlusPlus);
	
	RobotBase::WriteVersionString();

	// Start robot task
	// This is done to ensure that the C++ robot task is spawned with the floating point
	// context save parameter.
	Task *task = new Task("RobotTask", (FUNCPTR)RobotBase::robotTask, Task::kDefaultPriority, 64000);
	task->Start((int32_t)factory, (int32_t)task);
}

	/**
	 * 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;
		
//.........这里部分代码省略.........

	void Autonomous(void)
	{
		Timer t;
		int step = 0;
		myRobot.SetSafetyEnabled(false);
		
		//Value that indicates error in Image Tracking
		setTurnOffset(0);
		setHeight(0);
		locateBackboard.Start();
		susanpid->SetSetpoint(0.0);
		//if (!susanpid->IsEnabled()) susanpid->Enable();
		float launchSpeed;
		t.Start();
		//myRobot.TankDrive(1.0, 1.0);
		while( IsAutonomous() && !IsDisabled() )
		{
			//Sets the error of imageProcessing
			//Creates a critical Section to Protect turnOffset
			double error = -getTurnOffset();
			//vex->Set(0.5);
			//myRobot.TankDrive(0.5, 0.5);
			//double ratio = getWidth() / 320;
			//dist = 12.0 / tan(3.14159265358979323846264338 * 23.5 / 180.0) / ratio;
			//dist = (12 / tan(3.14159265 * 23.5 / 180)) / ((float)getHeight() / 320);
			dist = 580.0 / (float)getHeight();
			dist = ((dist >= 12.0 && dist <= 20.0 ) ? dist : 19.0);
			
			//Moves the turret
			//moveSusan(false, susanSpeed);
			
			/*if (step == 0 && t.Get() >= 0.0) {
				//launchSpeed = scalePower( distToNormalizedPower( dist ) 2.70, DriverStation::GetInstance()->GetBatteryVoltage());
				//launchSpeed = max(0.16, min(launchSpeed, 0.25));
				//launchSpeed = 0.22;
				susanpid->Disable();
				step++;
				susanSpeed = 0.0;

				float backSpin = (true) ? BS : 0;
				//launchSpeed = scalePower( distToNormalizedPower( dist ), DriverStation::GetInstance()->GetBatteryVoltage());
				//launchSpeed = max(0.2, min(launchSpeed, 0.45));
				//launchSpeed = 0.217;
				//Actuates the launchers based on launchSpeed
				launch1.Set(-launchSpeed - backSpin);
				launch2.Set(-launchSpeed + backSpin);
				wheelf.Set(Relay::kOn);
				t.Reset();
			} else if (step == 1 && t.Get() >= 0.3) {
				step++;
				//launchSpeed = scalePower( 2.70, DriverStation::GetInstance()->GetBatteryVoltage());
				//launchSpeed = max(0.16, min(launchSpeed, 0.25));
				launchSpeed = 0.205;
				wheelf.Set(Relay::kOff);
				float backSpin = BS;
				launch1.Set(-launchSpeed * (1 - backSpin) );
				launch2.Set(-launchSpeed * (1 + backSpin) );
				t.Reset();
			} else if (step == 2 && t.Get() >= 5.0) {
				step++;
				conveyorf.Set(Relay::kOn);
				t.Reset();
			} else if (step == 3 && t.Get() >= 1.0) {
				step++;
				conveyorf.Set(Relay::kOff);
				t.Reset();
			} else if (step == 4 && t.Get() >= 6.5) {
				step++;
				conveyorf.Set(Relay::kOn);
				t.Reset();
			} else if (step == 5 && t.Get() >= 15.0) {
				step++;
				conveyorf.Set(Relay::kOff);
				launch1.Set(0.0);
				launch2.Set(0.0);
				//wheelf.Set(Relay::kOn);
				t.Reset();
			} */
			
			//2 point shot
			/*if (step == 0 && t.Get() >= 0.0) {
				step++;
				//launchSpeed = scalePower( 2.70, DriverStation::GetInstance()->GetBatteryVoltage());
				//launchSpeed = max(0.16, min(launchSpeed, 0.25));
				launchSpeed = 0.1625;
				float backSpin = BS;
				launch1.Set(-launchSpeed * (1 - backSpin) );
				launch2.Set(-launchSpeed * (1 + backSpin) );
				t.Reset();
				
				myRobot.ArcadeDrive(0.5, 0.0);
				
			} else if (step == 1 && t.Get() >= 5.0) {
				step++;
				myRobot.ArcadeDrive(0.0,0.0);
				wheelf.Set(Relay::kOn);
				t.Reset();
			} else if (step == 2 && t.Get() >= 0.3) {
				step++;
				wheelf.Set(Relay::kOff);
//.........这里部分代码省略.........

	/**
	 * Image processing code to identify 2013 Vision targets
	 */
	void Process(void)
	{
		processTask.Start((UINT32) &hotGoal);
	}