jimmyli97

<mws:expr url="math.3.0">
	<math xmlns="http://www.w3.org/1998/Math/MathML" id="p1.1.m1" class="ltx_Math" alttext="{\displaystyle a}" display="inline" xml:id="p1.1.m1.1" xref="p1.1.m1.1.cmml">
  <semantics xml:id="p1.1.m1.1a" xref="p1.1.m1.1.cmml">
    <mi xml:id="p1.1.m1.1.1" xref="p1.1.m1.1.1.cmml">a</mi>
    <annotation-xml encoding="MathML-Content" xml:id="p1.1.m1.1.cmml" xref="p1.1.m1.1">
      <ci xml:id="p1.1.m1.1.1.cmml" xref="p1.1.m1.1.1">a</ci>
    </annotation-xml>
    <annotation encoding="application/x-tex" xml:id="p1.1.m1.1b" xref="p1.1.m1.1.cmml">{\displaystyle a}</annotation>
  </semantics>
</math>

jimmyli97

#define MAX_HARVESTMOVE_TIME_POS 800

typedef struct MotorState {
  ... //encoder data
  long timePosMs; //stores position in "time" (50 means net movement forward for 50 ms)
  long lastUpdateTimeMs; //last time this motorstate as updated
} MotorState;

void motorUpdateState() {
  for (int i=0; i<NUM_MOTORS; i++) {

jimmyli97

//Struct definition for holding states of motors
typedef struct MotorState { 
  long encoder; 
  long lastRealEncoderPos;  
} MotorState; 

//Array for storing MotorState for each motor
static MotorState motorStates[MAX_NUM_MOTORS];

//Called once per main loop to update MotorState

jimmyli97

#define LIFT_UP_POW -100
#define ENC_SIGN sgn(LIFT_UP_POW)

#define LIFT_MAX ENC_SIGN * 9800 //high goal
#define NINETY_GOAL ENC_SIGN * 7400
#define SIXTY_GOAL ENC_SIGN * 4600
#define THIRTY_GOAL ENC_SIGN * 2000
#define LIFT_MIN 0

void joyLift(DesiredMotorVals *desiredMotorVals, DesiredEncVals *desiredEncVals, TJoystick *joyState){

jimmyli97

#define ENC_ERROR_THRESHOLD 3000
int checkEnc = nMotorEncoder[curMotor];
int knownGoodEnc = motorStates[curMotor].lastRealEncoderPos;
int curEnc;

if (abs(checkEnc - knownGoodEnc) > ENC_ERROR_THRESHOLD) {
	//do nothing because enc val is bad
} else {
	curEnc = checkEnc; //enc val is good
	motorStates[curMotor].lastRealEncoderPos = curEnc;

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;
    }

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

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

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;