C++ PIDController::GetError方法代码示例

	void UpdateDashboard() {
		float r = 0.00001 * i;
		SmartDashboard::PutNumber("State", currentState + r);
		SmartDashboard::PutNumber("PID Turn Error",
				turnController->GetError() + r);
		SmartDashboard::PutNumber("PID Target",
				turnController->GetSetpoint() + r);
//		SmartDashboard::PutBoolean("Straight", straight);
		SmartDashboard::PutData("test", turnController);
		SmartDashboard::PutNumber("Yaw:", ahrs->GetYaw() + r);
		SmartDashboard::PutNumber("Roll:", ahrs->GetRoll() + r);
		SmartDashboard::PutNumber("Pitch", ahrs->GetPitch() + r);
		SmartDashboard::PutNumber("Scissor 1", pdp->GetCurrent(1) + r);
		SmartDashboard::PutNumber("Scissor 2", pdp->GetCurrent(2) + r);
		SmartDashboard::PutNumber("Left Drive 1", pdp->GetCurrent(12) + r);
		SmartDashboard::PutNumber("Left Drive 2", pdp->GetCurrent(13) + r);
		SmartDashboard::PutNumber("Right Drive 1", pdp->GetCurrent(14) + r);
		SmartDashboard::PutNumber("Right Drive 2", pdp->GetCurrent(15) + r);
		SmartDashboard::PutNumber("Constant Lift", constantLift);
		SmartDashboard::PutNumber("Rotate Rate", rotateRate + r);
		i = (i + 1) % 2;
		printf("2.1");
//		.PutLong("test1.2", 1337);
		printf("2.2");
//		mqServer.PutDouble("test",DriverStation::GetInstance().GetMatchTime());
		printf("2.3");
//		mqServer.PutString("test1.1","YOLO_SWAGINS");
		printf("2.4");
//		SmartDashboard::PutString("test1.2", mqServer.GetString("test1.1"));
//		SmartDashboard::PutNumber("test1", mqServer.GetDouble("test"));
//		SmartDashboard::PutNumber("test1.3", mqServer.GetLong("test1.2"));
//		SmartDashboard::PutNumber("test2", DriverStation::GetInstance().GetMatchTime());
	}

//.........这里部分代码省略.........
						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);
				PIDDriveRight->SetSetpoint(rightSP);
					
				
				leftSpeed = leftEncoder->GetRate();
				rightSpeed = rightEncoder->GetRate();
				if(!(count++ % 5))
				{
				printf("rate L: %2.2f R: %2.2f SP %2.4f %2.4f ERR %2.2f %2.2f Pow: %1.2f %1.2f\n", 
						leftPIDSource->PIDGet(), rightPIDSource->PIDGet(), leftSP, rightSP,
						PIDDriveLeft->GetError(), PIDDriveRight->GetError(), frontLeftMotor->Get(),
						frontRightMotor->Get());
						
				
				//printf("Throttle value: %f", stickR.GetThrottle());
				if(PIDDriveRight->OnTarget()) printf("Right on \n");
				if(PIDDriveLeft->OnTarget()) printf("Left on \n");
				}
					
				if(PIDRESETBUTTON)
				{
					//PIDDriveRight->SetPID(stickR.GetThrottle()+1,DRIVEINTGAIN, DRIVEDERIVGAIN); 
					//PIDDriveLeft->SetPID(stickR.GetThrottle()+1,DRIVEINTGAIN, DRIVEDERIVGAIN);
					PIDDriveLeft->Reset();
					PIDDriveRight->Reset();
					PIDDriveLeft->Enable();
					PIDDriveRight->Enable();
				}
			}
			else
			{
				if(PIDDriveLeft->IsEnabled()) PIDDriveLeft->Reset();
				if(PIDDriveRight->IsEnabled()) PIDDriveRight->Reset();
				if(DEMOSWITCH)
				{
					stickY[0] = stickY[0]*(1 - lift->getPosition()); //reduces power based on lift height
					stickY[1] = stickY[0]*(1 - lift->getPosition());
				}
				if(stickYAbs[0] > 0.05)
				{
					frontLeftMotor->Set(stickY[0]);
					rearLeftMotor->Set(stickY[0]);
				}

//.........这里部分代码省略.........
							liftSP = 0;
							popstage++;
						}
						else if(popstage == 5 && popTime - popStart > 4.85)
						{
							frontLeftMotor->Set(0);
							rearLeftMotor->Set(0);
							frontRightMotor->Set(0);
							rearRightMotor->Set(0);
						}
						*/
					}
					
					//Start tele-op lift code
					if(!lift->isBraking() && !disengageBrake)
					{
						PIDLift->SetSetpoint(liftSP);
						if(liftSP == 0 && liftPIDSource->PIDGet() < 0.1)
						{
							//PIDLift->SetPID(LIFTPROPGAIN, LIFTINTGAIN, 3*LIFTDERIVGAIN);
							PIDLift->SetOutputRange(-liftPIDSource->PIDGet() - 0.1, 1);
						}
						else PIDLift->SetOutputRange(-0.75, 1);
					}
					if(lift->isBraking() && lastLiftSP != liftSP)
					{
						PIDLift->SetSetpoint(lift->getPosition() + 0.04);
						PIDLift->SetPID(11.5 + 2*lift->getPosition(), LIFTINTGAIN + 0.4 + 3*lift->getPosition()/10, 0);
						lift->brakeOff();
						disengageBrake = true;
						if(!liftCount) liftCount = counts;
					}
					//set brake (because near)
					if(fabs(PIDLift->GetError()) < 0.015 && !lift->isBraking() && !disengageBrake)
					{
						if(liftCount == 0) liftCount = counts;
						if(counts - liftCount > 1000)
						{
							PIDLift->Reset();
							liftMotor1->Set(LIFTNEUTRALPOWER);
							liftMotor2->Set(LIFTNEUTRALPOWER);
							Wait(0.02);
							lift->brakeOn();
							Wait(0.02);
							liftMotor1->Set(0.0);
							liftMotor2->Set(0.0);
							PIDLift->Enable();
							//PIDLift->SetSetpoint(lift->getPosition());
							liftCount = 0;
						}
						if(counts%3000) printf("Braking/Not PID \n");
					}
					if(lift->isBraking() && !(counts%100000)) PIDLift->SetSetpoint(lift->getPosition());
					if(fabs(PIDLift->GetError()) < 0.01 && disengageBrake && counts - liftCount > 1000)
					{
						disengageBrake = false;
						PIDLift->SetPID(LIFTPROPGAIN, LIFTINTGAIN, LIFTDERIVGAIN);
						liftCount = 0;
					}
					//End tele-op lift code
#endif
					//myRobot.SetLeftRightMotorOutputs(0,0);
					break;
				case NOLINEMODE: //Fork turn left. Scores on far right column of left grid.
					lineFollowDone = true;
					if(!lineFollowDone){}