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