jimmyli97

writeDebugStream("FL Motor: %d\n", motor[MecMotor_FL]);
writeDebugStream("BL Motor: %d\n", motor[MecMotor_BL]);

jimmyli97

//Recall that Robotc stores the list of motors as the following:
typedef enum tMotor {
  leftMotor = 0,
  rightMotor = 7,
  centerMotor = 1,
} tMotor;

//We can keep track of the number of motors using a constant that is updated by hand
#define NUM_MOTORS 3

jimmyli97

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

jimmyli97

//RobotC variable that keeps track of # of total motors
int kNumbOfTotalMotors = 5;

int maxMotorPowers[kNumbOfTotalMotors];

maxMotorPowers[(int) leftMotor] = 100; //(int) leftMotor translates to 1, a valid array index
maxMotorPowers[(int) rightMotor] = 50;
...

jimmyli97

//An example of how RobotC stores motors
typedef enum tMotor {
  leftMotor = 0,
  rightMotor = 7, 
  centerMotor = 1,
} tMotor;

//RobotC variable that keeps track of # of maximum motors
int kNumbOfTotalMotors = 10;