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;

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

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

//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

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

jimmyli97

bool centerWingIsMoving = false;
bool centerWingDown = false;
int centerWingPulseTimeMs = 200;

void joyWing(DesiredMotorVals *desiredMotorVals, TJoystick *joyState) {
  if (centerWingIsMoving) {
    if (time1[T1] > centerWingPulseTimeMs) {
      desiredMotorVals->power[Wing_Middle] = 0;
      centerWingIsMoving = false;
      centerWingDown = !centerWingDown;

jimmyli97

typedef enum {
	NO_BALLS,
	BIG_BALLS,
	ALL_BALLS,
} HarvestState;

typedef enum {
	NO_BALLS,
	BIG_BALLS,
	ALL_BALLS,

jimmyli97

/* The winch has three settings:
1. The starting setting, this doesn't allow any balls to be selected.
2. The middle position which only picks up big balls.
3. The last and closest to the floor position that collects big and small balls.
In all these positions the stopper will be in a closed position in case that any
other robot hits our robot, the robot won't break our stopper.
 
The enum HarvestState represents the current setting the winch is on.
*/
 

jimmyli97

typedef enum {
	NO_BALLS, BIG_BALLS, ALL_BALLS,
} HarvestState;
 
typedef enum {
	NO_BALLS, BIG_BALLS, ALL_BALLS,
} HarvestPreState;
 
typedef enum {
	STOPPER_OPEN, STOPPER_CLOSE, WINCH_BIG_ALL, WINCH_BIG_START,

jimmyli97

//Check for sporadic encoder values as documented by Cougar Robotics #623
int checkEnc = nMotorEncoder[curMotor];
int curEnc = nMotorEncoder[curMotor];

if (pow > 0) { //we expect encoder value to be changing
  while (true) {
    //This can potentially take a long time, so set time limit
    if ((nPgmTime - encFnStartTimeMs) > 5) { 
      break;
    }