C++ Encoder::GetRate方法代码示例

// During every loop intervel of the teleop period
void Robot::TeleopPeriodic() {
    // Tank drive, both left and right joystick control their respective motor along the
    // joystick's 'y' axis
    //myRobot.TankDrive(-rStick.GetRawAxis(RIGHT_STICK_Y), -lStick.GetRawAxis(LEFT_STICK_Y));
    //myRobot.TankDrive(-gamePad.GetRawAxis(GAMEPAD_LEFT_STICK_Y), -gamePad.GetRawAxis(GAMEPAD_RIGHT_STICK_Y));

    // Choose the teleop drive option
    DriverControl(driveOption);


    // Edits the gyro data to account for drift
    /*
    editedGyroRate = gyro.GetRate();
    if (gyro.GetRate() > GYRO_DRIFT_VALUE_MIN && gyro.GetRate() < GYRO_DRIFT_VALUE_MAX) {
    	editedGyroRate = 0;
    } else {
    	editedGyroRate += GYRO_DRIFT_VALUE_AVERAGE;
    }
    */

    editedGyroAngle = gyro.GetAngle();



    SmartDashboard::PutString("Operation Type:", "TeleOp");

    // Print gyro data
    if (showGyro == true) {
        SmartDashboard::PutNumber("Gyro Angle (Raw)", gyro.GetAngle()*GYRO_SCALE_FACTOR);
        SmartDashboard::PutNumber("Gyro Angle (Edited)", editedGyroAngle*GYRO_SCALE_FACTOR);
        SmartDashboard::PutNumber("Gyro Rate (Raw)", gyro.GetRate()*GYRO_SCALE_FACTOR);
        SmartDashboard::PutNumber("Gyro Rate (Edited)", editedGyroRate*GYRO_SCALE_FACTOR);
    }


    // Print out the encoder data
    // Raw encoder data
    if (showEncoderRaw == true) {
        SmartDashboard::PutNumber("Encoder L get raw", encoder1.GetRaw());
        SmartDashboard::PutNumber("Encoder R get raw", encoder2.GetRaw());

    }

    // The delta encoder change
    if (showEncoderRate == true) {
        SmartDashboard::PutNumber("Encoder L get rate", encoder1.GetRate());
        SmartDashboard::PutNumber("Encoder R (reversed) get rate", encoder2.GetRate());
    }

    // Print the encoder index
    if (showEncoderIndex == true) {
        SmartDashboard::PutNumber("Encoder L index", encoder1.GetFPGAIndex());
        SmartDashboard::PutNumber("Encoder R index", encoder2.GetFPGAIndex());
    }
}

// During every loop intervel of the teleop period
void Robot::TeleopPeriodic() {
	// Tank drive, both left and right joystick control their respective motor along the
	// joystick's 'y' axis
	//myRobot.TankDrive(-rStick.GetRawAxis(RIGHT_STICK_Y), -lStick.GetRawAxis(LEFT_STICK_Y));

	// Choose the teleop drive option
	for(int c=0;c<7;c++) {
		buttonDone[c] = false;
	}

	DriverControl(driveOption);

	if(fabs(gyro.GetRate()) > GYRO_DRIFT_VALUE) {
	editedGyroRate = gyro.GetAngle();
	}
	else {
		editedGyroRate = 0;
		gyro.Reset();
	}





	SmartDashboard::PutString("Operation Type:", "TeleOp");

	// Print gyro data
	if (showGyro == true) {
		SmartDashboard::PutNumber("Gyro Angle", gyro.GetAngle()*GYRO_SCALE_FACTOR);
		SmartDashboard::PutNumber("Gyro Rate (Raw)", gyro.GetRate()*GYRO_SCALE_FACTOR);
		SmartDashboard::PutNumber("Gyro Rate (Edited)", editedGyroRate*GYRO_SCALE_FACTOR);
	}


	// Print out the encoder data
	// Raw encoder data
	if (showEncoderRaw == true) {
		SmartDashboard::PutNumber("Encoder L get raw", encoder1.GetRaw());
		SmartDashboard::PutNumber("Encoder R get raw", encoder2.GetRaw());

	}

	// The delta encoder change
	if (showEncoderRate == true) {
		SmartDashboard::PutNumber("Encoder L get rate", encoder1.GetRate());
		SmartDashboard::PutNumber("Encoder R (reversed) get rate", encoder2.GetRate());
	}

	// Print the encoder index
	if (showEncoderIndex == true) {
		SmartDashboard::PutNumber("Encoder L index", encoder1.GetFPGAIndex());
		SmartDashboard::PutNumber("Encoder R index", encoder2.GetFPGAIndex());
	}
}

	void OperatorControl(void)
	{
		NetTest();
		return;
		myRobot.SetSafetyEnabled(true);
		digEncoder.Start();
		const double ppsTOrpm = 60.0/250.0;   //Convert from Pos per Second to Rotations per Minute by multiplication
                                              // (See the second number on the back of the encoder to replace 250 for different encoders)
        const float VoltsToIn = 41.0;         // Convert from volts to cm by multiplication (volts from ultrasonic).
                                              // This value worked for distances between 1' and 10'.
		
		while (IsOperatorControl())
		{
			if (stick.GetRawButton(4)) {
				myRobot.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(), -1);
			} 
			else if (stick.GetRawButton(5))
			{
				myRobot.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(), 1);
			}
			else 
			{
				myRobot.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(), 0);
			}
			
			myRobot.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(), 0);
			
			SmartDashboard::PutNumber("Digital Encoder RPM", abs(digEncoder.GetRate()*ppsTOrpm));
			SmartDashboard::PutNumber("Ultrasonic Distance inch", (double) ultra.GetAverageVoltage()*VoltsPerInch);
			SmartDashboard::PutNumber("Ultrasonic Voltage", (double) ultra.GetAverageVoltage());

			Wait(0.1);
		}
		digEncoder.Stop();
	}

/**
 * Get the current rate of the encoder.
 * Units are distance per second as scaled by the value from SetEncoderDistancePerPulse().
 * 
 * @return The current rate of the encoder.
 *
 * @param aSlot The digital module slot for the A Channel on the encoder
 * @param aChannel The channel on the digital module for the A Channel of the encoder
 * @param bSlot The digital module slot for the B Channel on the encoder
 * @param bChannel The channel on the digital module for the B Channel of the encoder
  */
double GetEncoderRate(UINT32 aSlot, UINT32 aChannel, UINT32 bSlot, UINT32 bChannel)
{
	Encoder *encoder = AllocateEncoder(aSlot, aChannel, bSlot, bChannel);
	if (encoder != NULL)
		return encoder->GetRate();
	else
		return 0.0;
}

	void OperatorControl(void)
	{
		digEncoder.Start();
		const double ppsTOrpm = 60.0/250.0;	//This converts from Pos per Second to Rotations per Minute (See second number on back of encoder to replace 250 if you need it)
		
		while (IsOperatorControl())
		{
			SmartDashboard::PutNumber("Digital Encoder RPM", digEncoder.GetRate()*ppsTOrpm);
			Wait(0.1);
		}
		digEncoder.Stop();
	}

	/**
	 * Runs the motors with arcade steering.
	 */
	void OperatorControl(void)
	{
       encoder.Start();
		while (IsEnabled())
		{
			float controlv = control.GetVoltage();
			motor.Set(controlv/5); //get's voltage from control and divides it by 5
			double rate = encoder.GetRate();
			double distance = encoder.GetDistance();
			printf ("%f %f \n", rate, distance);

		}
	}

// Called repeatedly when this Command is scheduled to run
void DriveStraightTuning::Execute() {
    DriveSubsystem *driveSub = Robot::driveSubsystem;
    Encoder *leftEnc = Robot::driveSubsystem->leftEncoder;
    Encoder *rightEnc = Robot::driveSubsystem->rightEncoder;
    float leftRate = fabs(leftEnc->GetRate());
        float rightRate = fabs(rightEnc->GetRate());
         
        
        SmartDashboard::PutNumber("leftRate:", leftRate);
        SmartDashboard::PutNumber("rightRate:", rightRate);
    driveSub->theDriveTrain->Drive(0.5,0.0);
    
//    SmartDashboard::PutNumber("Gryo Angle", driveSub->driveGyro->GetAngle());
//    _drive_p = SmartDashboard::GetNumber("Drive P");
//    _drive_i = SmartDashboard::GetNumber("Drive i");
//    _drive_d = SmartDashboard::GetNumber("Drive d");
//    _straight_p = SmartDashboard::GetNumber("Straight P");
//    _straight_i = SmartDashboard::GetNumber("Straight i");
//    _straight_d = SmartDashboard::GetNumber("Straight d");
//    _distance = SmartDashboard::GetNumber("Distance");
//     _enabled = SmartDashboard::GetBoolean("Enable");
//     static bool prevEnabled = false; 
//     if ((_enabled == true) && (prevEnabled == false))
//     {
//         PIDController *drive = driveSub->driveDistancePID;
//         PIDController *straight = driveSub->driveStraightPID;
//         drive->SetPID(_drive_p,_drive_i,_drive_d);
//         straight->SetPID(_straight_p,_straight_i,_straight_d);
//         driveSub->DriveStraight(_distance);
//     }
//     else if (_enabled == false)
//     {
//         driveSub->Cancel();
//         prevEnabled = false;
//     }
}

//.........这里部分代码省略.........

		while (IsOperatorControl())
		{
			GetWatchdog().Feed();
			count++;
#if !(SKELETON)
			sendVisionData();
#endif
			/*
			if(LIFTLOW1BUTTON && !(counts%10)) printf("LIFTLOW1BUTTON\n");
			if(LIFTLOW2BUTTON && !(counts%10)) printf("LIFTLOW2BUTTON\n");
			if(LIFTMID1BUTTON && !(counts%10)) printf("LIFTMID1BUTTON\n");
			if(LIFTMID2BUTTON && !(counts%10)) printf("LIFTMID2BUTTON\n");
			if(LIFTHIGH1BUTTON && !(counts%10)) printf("LIFTHIGH1BUTTON\n");
			if(LIFTHIGH2BUTTON && !(counts%10)) printf("LIFTHIGH2BUTTON\n");
			*/
			/*
			if(lsLeft->Get()) printf("LSLEFT\n");
			if(lsMiddle->Get()) printf("LSMIDDLE\n");
			if(lsRight->Get()) printf("LSRIGHT\n");
			*/
			stickY[0] = stickL.GetY();
			stickY[1] = stickR.GetY();
			stickYAbs[0] = fabs(stickY[0]);
			stickYAbs[1] = fabs(stickY[1]);
			if(bDrivePID)
			{
	#if 0
				frontLeftMotor->Set(stickY[0]);
				rearLeftMotor->Set(stickY[0]);
				frontRightMotor->Set(stickY[1]);
				rearRightMotor->Set(stickY[1]);
				
				if(!(count%5)) printf("Speeds: %4.2f %4.2f Outputs: %f %f \n", leftEncoder->GetRate(),
						rightEncoder->GetRate(), frontLeftMotor->Get(), frontRightMotor->Get());
	#endif		
				if(stickYAbs[0] <= 0.05 )
				{
					leftSP = 0;
					if(!(count%3) && !BACKWARDBUTTON)
					{
						PIDDriveLeft->Reset();
						PIDDriveLeft->Enable();
					}
				}
				else leftSP = stickY[0] * stickY[0] * (stickY[0]/stickYAbs[0]); //set points for pid control
				if(stickYAbs[1] <= 0.05)
				{
					rightSP = 0;
					if(!(count%3) && !BACKWARDBUTTON)
					{
						PIDDriveRight->Reset();
						PIDDriveRight->Enable();
					}
				}
				else rightSP = stickY[1] * stickY[1] * (stickY[1]/stickYAbs[1]);
				
				if (BACKWARDBUTTON)
				{
					tempRightSP = rightSP;
					tempLeftSP = leftSP;
					rightSP = -tempLeftSP;
					leftSP = -tempRightSP; //This line and above line sets opposite values for the controller. ...Theoretically.
				}
				
				PIDDriveLeft->SetSetpoint(leftSP);

	void write()
	{
		myfile<<GetTime()<<","<<theEncoder->GetDistance()<<","<<theEncoder->GetRate()<<",\n";
	}

	void OperatorControl(void)
	{	
		//Open the file
		

		//setup watchdog	
		GetWatchdog().SetEnabled(true);
		GetWatchdog().SetExpiration(0.3);
		theEncoder->Start();
		//theLift->Enable();
#if RATE

#endif
		float maxHeight = 0;
		float maxRate = 0;

		while (IsOperatorControl())
		{	
			printf("Distance %f rate %f maxHeight %f maxRate %f\n", 
					theEncoder->GetDistance(),
					theEncoder->GetRate(),
					maxHeight,
					maxRate);
			if(writing)
			{
				write();
			}else if (!closed){
				printf("CLOSED!");
				myfile.close();
				closed = true;
			}
			
			Wait(.01);
			if(theEncoder->GetDistance() > maxHeight) maxHeight = theEncoder->GetDistance();
			if(theEncoder->GetRate() > maxRate) maxRate = theEncoder->GetRate();
			//check whether we should switch modes
			/*if (decelToStop(theEncoder->GetRate(),SETPOINT_METERS - theEncoder->GetDistance()) <= MAXDECEL && !decel)
			{
				decelRate = decelToStop(theEncoder->GetRate(),SETPOINT_METERS - theEncoder->GetDistance());
				cout<<"DecelRate: "<<decelRate<<endl;
				decel = true;
				printf("BEGINNING DECELERATION\n");
				initTime = GetTime();
			}*/
			
			/*if(theEncoder->GetDistance() > 1.0)
			{
				theJag->Set(0.0);
				writing = false;
				//theLift->SetPID(.3,0,0);
				//theLift->SetSetpoint(0.0);
				done = true;
			}else if(!done){
				theJag->Set(1.0);
				//theLift->SetSetpoint(1.5);
			}
			*/
			if(theEncoder->GetDistance() >= SETPOINT_METERS)
			{
				done = true;
				theJag->Set(0.0);
				writing = false;
			}else if (!done){
				theJag->Set(1.0);
			}
			rateSetpoint = vFinal + (decelRate * (GetTime() / pow(10,9)));
#if 0
			if(theEncoder->GetDistance() >= SETPOINT_METERS)
			{
				theJag->Set(0.0);
				writing = false;
			}else if(decel){
#if RATE
				//theLift->SetSetpoint(rateSetpoint);
				theJag->Set(0.0);
#else
				theLift->SetSetpoint(SETPOINT_METERS);
#endif
			}else{
				theJag->Set(1.0);
			}
#if RATE
			
#else
			theLift->SetSetpoint(SETPOINT_METERS);
#endif
#endif
			GetWatchdog().Feed();
		}
	}