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 Drive::calculateDrive()
{
currGyro = getGyroAngle();
currTheta = prevTheta + (currGyro - prevGyro);
theta = (currTheta + prevTheta) / 2;
currLeft = getLeftDistance();
currRight = getRightDistance();
magnitude = (currLeft + currRight - prevLeft - prevRight) / 2;
deltaX = -magnitude * sin(theta / 180 * M_PI);
deltaY = magnitude * cos(theta / 180 * M_PI);
currX = prevX + deltaX;
currY = prevY + deltaY;
}
void Drive::update(bool isAuto)
{
float currAngle = getGyroAngle();
//float kLinVelFF = 0.155;
float kLinVelFF = 0.0;
float kLinAccelFF = 0.0;
if (doWeProfile)
{
kLinVelFF = 0.08;
kLinAccelFF = 0.03;
}
float kAngVelFF = 0;
float kAngAccelFF = 0;
//float kDccellFF = 0;
if (isAuto)
{
if (!deadPID)
{
float loopTime = loopTimer->Get();
std::vector<float> linearStep = linearGenerator->getProfile(loopTime);
std::vector<float> angularStep = angularGenerator->getProfile(loopTime);
/*
SmartDashboard::PutNumber("Velocity Error: ", linearStep[2] - getVelocity());
SmartDashboard::PutNumber("Position Error: ", linearStep[1] - getWheelDistance());
SmartDashboard::PutNumber("Angular Error: ", angularStep[1] - getGyroAngle());
*/
float linearInput, angularInput;
/*
if (linearStep[3] < 0)
{
kLinAccelFF = kDccelFF;
}
*/
linearInput = -(kLinVelFF*linearStep[2]) + -(kLinAccelFF*linearStep[3]);
angularInput = -(kAngVelFF*angularStep[2]) + -(kAngAccelFF*angularStep[3]);
/*
if (fabs(linearOutput) < .1 && linearOutput != 0)
{
linearOutput = .1 * (fabs(linearOutput)/linearOutput);
}
SmartDashboard::PutNumber("Linear Output: ", linearOutput);
SmartDashboard::PutNumber("Velocity: ", linearStep[2]);
SmartDashboard::PutNumber("Acceleration: ", linearStep[3]);
*/
float linearOutput, angularOutput;
linearOutput = drivePID->update(linearStep[1]-getWheelDistance(), loopTimer) + linearInput;
angularOutput = -rotatePID->update(angularStep[1] - getGyroAngle(), loopTimer);
std::string s = "---" + log->asString(getLeftDistance()) + ", " + log->asString(getLeftVelocity()) + ", " + log->asString(getRightDistance()) + ", " + log->asString(getRightVelocity()) + ", " + log->asString(getWheelDistance()) + ", " + log->asString(getVelocity()) + ", " + log->asString(getGyroAngle()) + ", " + log->asString(loopTime) + ", " + log->asString(ds->GetBatteryVoltage()) + ", " + log->asString(leftPower) + ", " + log->asString(rightPower) + ", " + log->asString(linearOutput) + ", " + log->asString(drivePID->getP()) + ", " + log->asString(drivePID->getI()) + ", " + log->asString(drivePID->getD()) + ", " + log->asString(-(kLinVelFF*linearStep[2])) + ", " + log->asString(-(kLinAccelFF*linearStep[3])) + ", " + log->asString(linearStep[1] - getWheelDistance()) + ", " + log->asString(linearStep[2] - getVelocity()) + ", " + log->asString(linearStep[1]) + ", " + log->asString(linearStep[2]) + ", " + log->asString(linearStep[3]) + "____";
printf("%s\n", s.c_str());
printTimer->Reset();
//SmartDashboard::PutNumber("I Contribution: ", rotatePID->update(angularStep[1]-getGyroAngle(), loopTimer) - 0.01*(angularStep[1]-getGyroAngle()));
//SmartDashboard::PutNumber("Linear Output: ", linearOutput);
arcade(linearOutput, angularOutput);
}
}
prevAngle = currAngle;
leftDrive->Set(leftPower);
rightDrive->Set(rightPower);
// was taken off for chezy champs and is no longer needed
//setKickUp(kick);
}
