//ODOMETRY NEEDS TO BE IMPROVED A LOT> IT IS BUGGY >>> :-(
void Odometry()
{
double robot_x,robot_y,robot_theta,beta;
double temp;
//robot_theta= (((getLeftDistance()-getRightDistance()))/(uint32_t)(ROTATION_FACTOR));
if (getLeftDistance() > getRightDistance())
robot_theta = (double)((uint32_t)(100)*(getLeftDistance()-getRightDistance())/(uint32_t)(2*ROTATION_FACTOR));
else
robot_theta = -(double)((uint32_t)(100)*(getRightDistance()-getLeftDistance())/(uint32_t)(2*ROTATION_FACTOR));
robot_theta = (abs(robot_theta) <=359 && abs(robot_theta) > 0) ? robot_theta : 0.0;
/*temp = cos(robot_theta);
if (mleft_dir != mright_dir)
robot_x = (M_PI*25*temp*(getRightDistance()+getLeftDistance()))/625;
else{
if (mleft_dir != 0)
robot_x = -(M_PI*25*temp*(getRightDistance()+getLeftDistance()))/625;
else
robot_x = (M_PI*25*temp*(getRightDistance()+getLeftDistance()))/625;
}
robot_x = (M_PI*25*temp*(getRightDistance()+getLeftDistance()))/625;
temp = sin(beta);
robot_y = (M_PI*25*temp*(getRightDistance()+getLeftDistance()))/625;
temp = (robot_x-robot_old_x);
robot_old_x = robot_x;
source[0] = source[0]+temp;
source[1] = robot_y/10;*/
double temp_theta,temp_x,temp_y;
temp_theta= robot_theta - robot_old_theta;
source[2] = source[2]+temp_theta;
if (abs(source[2]) >180)
source[2] = (source[2] < 0)? (360+source[2]):(source[2]-360);
robot_x = (M_PI*25*cos((robot_theta*M_PI)/180)*(getRightDistance()+getLeftDistance()))/625;
robot_y = (M_PI*25*sin((robot_theta*M_PI)/180)*(getRightDistance()+getLeftDistance()))/625;
temp_x = robot_x - robot_old_x;
temp_y = robot_y - robot_old_y;
source[0] = source[0]+temp_x;
source[1] = source[1]+temp_y;
robot_old_x = robot_x;
robot_old_y = robot_y;
robot_old_theta = robot_theta;
}
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);
}
