polardrive.c

void driveSetMecMotorPolarDegrees(DesiredMotorVals *desiredMotorVals, int angle, float powerRatio,
                                  float rotationRatio) {
  //Holds max motor powers
  float maxPowFLBR = cosDegrees(45.0 - (float)angle);
  float maxPowFRBL = cosDegrees(45.0 + (float)angle);
  float powFL = (powerRatio * maxPowFLBR) + (rotationRatio * abs(maxPowFLBR));
  float powBL = (powerRatio * maxPowFRBL) + (rotationRatio * abs(maxPowFRBL));
  float powFR = (powerRatio * maxPowFRBL) - (rotationRatio * abs(maxPowFRBL));
  float powBR = (powerRatio * maxPowFLBR) - (rotationRatio * abs(maxPowFLBR));
  //Cap motor values
  powFL = helpFindSign(powFL) * helpFindMinAbsFloat(powFL, maxPowFLBR);
  powBL = helpFindSign(powBL) * helpFindMinAbsFloat(powBL, maxPowFRBL);
  powFR = helpFindSign(powFR) * helpFindMinAbsFloat(powFR, maxPowFRBL);
  powBR = helpFindSign(powBR) * helpFindMinAbsFloat(powBR, maxPowFLBR);
  //Holds max reference power
  float maxRefPow = (float) motorGetMaxReferencePower();
  //Scale to max reference power
  float absHighestPow = helpFindMaxAbsFloat(maxPowFLBR, maxPowFRBL);
  float multiplier = maxRefPow / absHighestPow;
  float scaledPowFL = powFL * multiplier;
  float scaledPowBL = powBL * multiplier;
  float scaledPowFR = powFR * multiplier;
  float scaledPowBR = powBR * multiplier;
  desiredMotorVals->power[MecMotor_FL] = helpRoundFloat(scaledPowFL);
  desiredMotorVals->power[MecMotor_BL] = helpRoundFloat(scaledPowBL);
  desiredMotorVals->power[MecMotor_FR] = helpRoundFloat(scaledPowFR);
  desiredMotorVals->power[MecMotor_BR] = helpRoundFloat(scaledPowBR);
}