task usercontrol() {
int stick[4],
drivePwrs;
signed char liftTargCt = 0,
liftTargCtLast;
const signed char stickThresh = 7;
while (true) {
for(int i = 0; i < 4; i++)
stick[i] =
(fabs(vexRT[i]) >= stickThresh)
? vexRT[i]
: 0;
upToggle(&liftUpToggle, LIFT_UP_BTN);
upToggle(&liftDwnToggle, LIFT_DWN_BTN);
liftTargCtLast = liftTargCt;
liftTargCt += (pressToggle(&liftUpToggle, LIFT_UP_BTN) ^ pressToggle(&liftDwnToggle, LIFT_DWN_BTN))
? (pressToggle(&liftUpToggle, LIFT_UP_BTN))
? 1
: -1
: 0;
if(liftTargCt > 2)
liftTargCt = 0;
else if(liftTargCt < 0)
liftTargCt = 2;
setDrive(arcade(&drivePwrs, stick[LY], stick[RX]));
if(liftTargCt != liftTargCtLast)
setPid(&liftPid, liftSetPts[liftTargCt]);
setLift(upPid(&liftPid));
setClaw((CLAW_OPEN_BTN ^ CLAW_CLOSE_BTN)
? (CLAW_OPEN_BTN)
? 127
: -127
: 0);
wait1Msec(20);
}
}
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);
}
void Robot::update() {
posCompute(); // Update Position Variables
float straightOut = 0.0f;
float turnOut = 0.0f;
/*Serial.print("dir: ");
Serial.print(getDirection());
Serial.print("; Angle: ");
Serial.print(getAngle()*180.0/PI);
Serial.print("; X: ");
Serial.print(getX());
Serial.print("; Y: ");
Serial.println(getY());*/
float maxAccS = 1.8;
float maxDecS = 4.0;
float maxAccT = 5.0;
float maxDecT = 9.0;
float currentSpeed = 0.0;
float currentTurn = 0.0;
// State Machine
switch(sCurrentState) {
// constants for kMoving
float straightInput;
float turnInput;
float turnSetPoint;
case kMoving:
//straightInput = euclidean(fX, fY, targetX, targetY);
straightInput = abs((targetX == lastTX) ? targetY - fY : targetX - fX);
straightOut = fMaxSpeed * (0.00 + pPosition->compute(-1.0f * straightInput));
//straightOut = fMaxSpeed * pow(pPosition->compute(-1.0f * straightInput), 2);
turnInput = angleClamp(fAngle, targetAngle);
turnSetPoint = angleClamp(atan2(targetY-fY + 3.0*sin(targetAngle), targetX-fX + 3.0*cos(targetAngle)), targetAngle);
//turnSetPoint = angleClamp(atan2(targetY-fY, targetX-fX), targetAngle);
turnOut = fMaxSpeed * pDriveAngle->compute(turnInput, turnSetPoint);
//if (straightInput < (1.5f*POS_DEADBAND))
// turnOut = 0.33*fMaxSpeed*(pAngle->compute(fAngle, targetAngle) - 0.50);
if (straightInput < POS_DEADBAND) {
sCurrentState = kWaiting;
fAngle = angleClamp(fAngle, 0.0f);
pLeftSpeed->reset();
pRightSpeed->reset();
} else
//maxAcc = 1.0;
currentSpeed = getSpeed();
if (straightOut - currentSpeed > maxAccS) straightOut = currentSpeed + maxAccS;
else
if (straightOut - currentSpeed < -maxDecS) straightOut = currentSpeed - maxDecS;
arcade(straightOut, turnOut);
break;
case kTurning:
turnInput = angleClamp(fAngle - targetAngle, 0);
if(abs(turnInput) < ANGLE_DEADBAND) {
sCurrentState = kWaiting;
mLeft->brake();
mRight->brake();
fAngle = angleClamp(fAngle, 0.0f);
}
else {
//turnOut = fMaxSpeed*pAngle->compute(angleClamp(fAngle, targetAngle), targetAngle);
turnOut = fMaxSpeed*pAngle->compute(turnInput);
if (turnOut - currentTurn > maxAccT) turnOut = currentTurn + maxAccT;
else
if (turnOut - currentTurn < -maxDecT) turnOut = currentTurn - maxDecT;
arcade(0.0, turnOut);
}
break;
case kWaiting:
// prevents build up
pLeftSpeed->reset();
pRightSpeed->reset();
// makes behavior smoother and gets rid of whining noise
mLeft->brake();
mRight->brake();
//arcade(0.0, 0.0);
break;
}
/*
// csv compatible data
// actual speed
Serial.print(getSpeed()/70.0);
Serial.print(", ");
// target speed
Serial.print(straightOut/70.0);
Serial.print(", ");
// actual angular speed
Serial.print(getAngularSpeed()/70.0);
Serial.print(", ");
// target angular speed
Serial.print(turnOut/70.0);
Serial.print(", ");
// angle error
Serial.print((angleClamp(targetAngle - getAngle(), 0) * 180.0/PI)/180.0);
Serial.print("\n");
*/
}
