C++ CANJaguar类代码示例

	void RobotInit(void) {
		// Actions which would be performed once (and only once) upon initialization of the
		// robot would be put here.
		
		// Test to see if Dashboard is connected---------------------------------------------------------
//		printf("HUE MIN: %d\n", SmartDashboard::GetNumber("HUE MIN"));
		
		// Initialize exponent value from SmartDashboard
		exp = 3.0;
		
		buttonWasDown = false;
		// Initialize settings for encoder drive PIDControllers
//		Drive_PID_Controller->SetOutputRange(-0.2, 0.2);
//		Drive_PID_Controller->SetTolerance(0.1);
		
		// Initialize settings for RobotTurn
//		Goal_Align_PID->SetOutputRange(-0.2, 0.2);
//		Goal_Align_PID->SetTolerance(0.1);
        
		// Set encoders
        Front_R->ConfigEncoderCodesPerRev(360);
        Front_L->ConfigEncoderCodesPerRev(360);
		
        // Set each drive motor to have an encoder to be its friend
        Front_R->SetPositionReference(CANJaguar::kPosRef_QuadEncoder);
        Front_L->SetPositionReference(CANJaguar::kPosRef_QuadEncoder);
        
		
		printf("RobotInit() completed.\n");
	}

	void Drive (float speed, float turn, float strafe) {
		Dlf->Set(range(speed + turn + strafe)*250, 2);
		Dlb->Set(range(speed + turn - strafe)*250, 2);
		Drf->Set(range(-speed + turn + strafe)*250, 2);
		Drb->Set(range(-speed + turn - strafe)*250, 2);
		//CANJaguar::UpdateSyncGroup(2);
					
	}

	void setMotors(float left,float right)
	{
		left=-left;
		right=-right;
		left*=300;
		right*=300;
		frontLeft->Set(-left);
		frontRight->Set(right);
		backRight->Set(right);
		backLeft->Set(-left);
		//CANJaguar::UpdateSyncGroup(2);
	}

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

void MecanumDrive::InitMotor( CANJaguar& motor )
{
	motor.ChangeControlMode( m_currControlMode );
	if ( m_currControlMode == CANJaguar::kSpeed )
	{
		motor.ConfigEncoderCodesPerRev(360);
		motor.ConfigMaxOutputVoltage(12.0);
		motor.ConfigNeutralMode(CANJaguar::kNeutralMode_Brake);
		motor.SetPID(.7,.004,0);
		motor.SetSpeedReference(CANJaguar::kSpeedRef_QuadEncoder);
	}
	motor.EnableControl();
}

void MecanumDrive::CheckForRestartedMotor( CANJaguar& motor, const char * strDescription )
{
        if ( m_currControlMode != CANJaguar::kSpeed )   // kSpeed is the default
        {
                if ( motor.GetPowerCycled() )
                {
                        Wait(0.10); // Wait 100 ms 
                        InitMotor( motor );
                        char error[256];
                        sprintf(error, "\n\n>>>>%s %s", strDescription, "Jaguar Power Cycled - re-initializing");
                        printf(error);
                        setErrorData(error, strlen(error), 100);                
                }
        }
}

	void Disabled (void) {
		debug("disable");			
		Dlf->StopMotor();
		Dlb->StopMotor();
		Drf->StopMotor();
		Drb->StopMotor();
	}

	void DoctaEight::leftyrighty(double left, double right)//set drive motors on either side
	{
		REDRUM;
		righty.Set(negate*right*.9);
		rightyB.Set(negate*right*.9);
		lefty.Set(negate*left*.9);
		leftyB.Set(negate*left*.9);
	}

	void UpdateDashboardStatus()
	{
		Dashboard &dashHigh = m_ds->GetHighPriorityDashboardPacker();
		dashHigh.AddCluster(); // PID (not used for now)
		dashHigh.AddDouble(0.0); // P
		dashHigh.AddDouble(0.0); // I
		dashHigh.AddDouble(0.0); // D
		dashHigh.FinalizeCluster();
		dashHigh.AddDouble(speedJag.GetSpeed()); // Current position
		dashHigh.AddDouble(speedJag.Get()); // Setpoint
		dashHigh.AddDouble(speedJag.GetOutputVoltage()); // Output Voltage
		dashHigh.Finalize();
	}

	void OperatorControl(void) {
		while(!IsDisabled()) {
			GetWatchdog().Feed();
			float speed = stick.GetRawAxis(2);
			float strafe = -1*stick.GetRawAxis(1);
			float turn = -1*stick.GetRawAxis(3);
			Dlf->Set(speed + turn + strafe);
			Dlb->Set(speed + turn - strafe);
			Drf->Set(-speed + turn + strafe);
			Drb->Set(-speed + turn - strafe);
			Wait(.05);	
		}
	}

	void TeleopInit(void) {
		m_telePeriodicLoops = 0;				// Reset the loop counter for teleop mode
		m_dsPacketsReceivedInCurrentSecond = 0;	// Reset the number of dsPackets in current second
		
		// Default autoPilot to off
		autoPilot = false;
		
		Front_R->EnableControl();
		Front_L->EnableControl();
		
		// Enable Goal Align PID
//		Goal_Align_PID->Disable(); // Stop previous enables
//		Goal_Align_PID->Enable();
//		Goal_Align_PID->SetSetpoint(0.0);
	}

	void RawControl::armAngle(float angle) {
		
		//angle/=300.0f;
		
		arm->Set(angle);
		//cout<<"angle= "<<angle<<"\n";
	}

 void AutonomousInit() {
     init();
     currentDistance = 0;
     goalDistance = 6.0*12.0; // 6 feet
     autonomousShooterDelay = 0;
     shooterMotor->Set(0);
     loaderMotor->Set(Relay::kOff);
 }

	void TeleopPeriodic(void) {
		// increment the number of teleop periodic loops completed
		m_telePeriodicLoops++;
		GetWatchdog().Feed();

//		if(autoPilot == true)
//		{
			// Auto Align Disable Button
//			if(operatorGamepad->GetButton(Joystick::kTopButton) == 2)
//			{
//				Goal_Align_PID->Disable(); // Stop outputs
//				Goal_Align_PID->Enable(); // Start PIDContoller up again
//				Goal_Align_PID->SetSetpoint(0.0);
//				autoPilot = false;
//			}
//		}
//		else
//		{
			// Calculate jaguar output based on exponent we pass from SmartDashboard
			double leftOutput, rightOutput;
			leftOutput = calculateDriveOutputForTeleop(operatorGamepad->GetRawAxis(2));
			rightOutput = calculateDriveOutputForTeleop(operatorGamepad->GetRawAxis(5));
			m_robotDrive->SetLeftRightMotorOutputs(leftOutput, rightOutput);
			
			if(operatorGamepad->GetRawButton(1) && !buttonWasDown)
			{
				printf("LEFT ENCODER: %f\n", Front_L->GetPosition());
				printf("RIGHT ENCODER: %f\n", Front_R->GetPosition());
			}
			
			buttonWasDown = operatorGamepad->GetRawButton(1);
			
			// Auto Align Button
//			if(operatorGamepad->GetButton(Joystick::kTopButton) == 1)
//			{
//				// Turn Auto Align on if we see a goal and we know the azimuth
//				if(SmartDashboard::GetBoolean(FOUND_KEY) == true)
//				{
//					Goal_Align_PID->SetSetpoint(SmartDashboard::GetNumber(AZIMUTH_KEY));
//					autoPilot = true;
//				}
//			}
//		}
		
	} 

    void StartWheels()
    {
	if (!spinFastNow) {
printf(">>> StartWheels\n");
	    Log(LOG_START, 0, 0);

	    spinFastNow = true;

	    // start shooter wheels in %vbus mode, max output
#ifdef HAVE_TOP_WHEEL
#ifdef HAVE_TOP_CAN1
	    jagVbus(topWheel1,    maxOutput);
	    Log(LOG_MODE, 1, 1);
#endif
#ifdef HAVE_TOP_PWM1
	    topWheel1->Set(maxOutput);
	    Log(LOG_MODE, 1, 1);
#endif
#ifdef HAVE_TOP_CAN2
	    jagVbus(topWheel2,    maxOutput);
	    Log(LOG_MODE, 2, 1);
#endif
#endif
#ifdef HAVE_BOTTOM_WHEEL
#ifdef HAVE_BOTTOM_CAN1
	    jagVbus(bottomWheel1, maxOutput);
	    Log(LOG_MODE, 3, 1);
#endif
#ifdef HAVE_BOTTOM_PWM1
	    bottomWheel1->Set(maxOutput);
	    Log(LOG_MODE, 3, 1);
#endif
#ifdef HAVE_BOTTOM_CAN2
	    jagVbus(bottomWheel2, maxOutput);
	    Log(LOG_MODE, 4, 1);
#endif
#endif
	    topPID = bottomPID = false;

	    // reset reporting counter
	    report = 0;
printf("<<< StartWheels\n");
	}
    }