示例#1
0
void updateDrivetrain()
{
	updateGyro();
	if ( drivetrainTurning )
	{
		int output = calcPID(drivetrainTurnPID, getGyroAngle());
		output = hlLimit(output, 45, -45);
		setDrivetrainSpeeds(output, -output);
	}
	else
	{
		int output = hlLimit(calcPID(drivetrainPID, getDrivetrainDistance()), 100, -100);
		int left = hlLimit(output, drivetrainMaxSpeed, -drivetrainMaxSpeed);
		int right = hlLimit(output, drivetrainMaxSpeed, -drivetrainMaxSpeed);
		if ( drivetrainGyroDrivestraight )
		{
			drivetrainTurnPID.error = drivetrainTurnPID.target-getGyroAngle();
			int turnAmt = drivetrainTurnPID.error*DRIVETRAIN_ANGLECORRECT_CONSTANT;
			nxtDisplayString(2, "%i %i", turnAmt, DRIVETRAIN_ANGLECORRECT_CONSTANT);
			left += turnAmt;
			right -= turnAmt;
		}
		setDrivetrainSpeeds(left, right);
	}
}
示例#2
0
void PIDArmControl()
{

    if((vexRT[Btn6D] - vexRT[Btn6U])!= 0)
    {  
      setArmSpeed((vexRT[Btn5D] - vexRT[Btn5U]) * maxArm);
      disable(PID);  
    }  
    else
    {  
      setArmSpeed(calcPID(PID));  
    }    
    if(vexRT[Btn7U] == 1)
    {  
      setSetpoint(PID, highScore);
      enable(PID);  
    }  
    else if (vexRT[Btn7D] == 1)
    {  
      setSetpoint(PID, lowScore);
      enable(PID);  
    }  
    else if (vexRT[Btn7L] == 1)
    {  
      setSetpoint(PID, midScore);
      enable(PID);
    }  
    else if (vexRT[Btn7R] == 1)
    {  
      disable(PID);  
    }
}
示例#3
0
void updateSwerveModule(SwerveModule &swerveModule)
{
  int reading = HTSPBreadADC(proto, swerveModule.number, 8);



	int turnSpeed = calcPID(swerveModule.turnPID, reading);

	if ( swerveModule.inverted )
		turnSpeed = -turnSpeed;

	servo[swerveModule.turnMotor] = -(turnSpeed+(swerveModule.turnZeroOffset+127));
	nxtDisplayString(5, "%i", -(turnSpeed+(swerveModule.turnZeroOffset+127)));
/*
	if ( abs(swerveModule.driveSpeed) > 10 )
		motor[swerveModule.driveMotor] = (swerveModule.driveMotorInverted?-swerveModule.driveSpeed:swerveModule.driveSpeed);
	else if ( turnSpeed > 10 )
	  motor[swerveModule.driveMotor] = -13;
	else if ( turnSpeed < -10 )
	  motor[swerveModule.driveMotor] = 13;
	else
	  motor[swerveModule.driveMotor] = 0;
*/


}
示例#4
0
void armBaseUpdate()
{
    int potInput = HTSPBreadADC(S3, 0, 10)+BASE_OFFSET;
    static int loopsStable = 0;

    if ( abs(base.error) <= 60 ) // was 75
    {
      if ( loopsStable < 1000 ) loopsStable++;
    }
    else
      loopsStable = 0;

    if ( potInput < 500 && potInput > 270 )
    {
      // Arm is in top range - very little force needed to maintain position
      base.Kp = 0.7;
      base.Ki = 0.01;
      base.errorSum = 0;
    }
    else
    {
      base.Kp = 1.5;
      base.Ki = 0.07;
    }

    if ( abs(base.error) > 70 && base.target == BASE_PICKUP ) // Arm is headed forward and we are far away
      base.Kd = 2.5; // increase D to prevent slaming against the floor
    //else if ( potInput > 450 && base.target > 450 ) // TODO: check if this D modification is still needed
    //  base.Kd = 2.5;
    else
      base.Kd = 0.4;

    if ( loopsStable <= 9 ) // was 20
      motor[armBase] = dbc(limitVar(calcPID(base, potInput), 80), 23);
    else
    {
      calcPID(base, potInput); // keep PID updated for error calculation
                               // even though we are not using it's output

      base.errorSum = 0;       // prevent I from building up
      motor[armBase] = 0;
    }
}
示例#5
0
task autonomous()
{
  pre_auton();
  while(true)
  {
    if(vexRT[AButton] == 1)
    {
      setArmSpeed(calcPID(PID));
      encoderDriveStraight(44.0/12.0);
      setCollectorSpeed(MAX_TREAD);
      wait10Msec(400);
      setCollectorSpeed(0);
      setSetPoint(PID, armGround);
      enable(PID);
      encoderDriveStraight(-44.0/12.0);
    }
  }
}