longencoder2-19

longencoder_2-19.c

//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
void motorUpdateState() {
  for (int i=0; i<NUM_MOTORS; i++) {
    tMotor curMotor = motorList[i];
    int curEnc = nMotorEncoder[curMotor];
    motorStates[curMotor].encoder += (curEnc - motorStates[curMotor].lastRealEncoderPos);
    motorStates[curMotor].lastRealEncoderPos = curEnc;

    if (abs(curEnc) > 30000) { //reset real encoder on overflow
      motorStates[curMotor].lastRealEncoderPos = 0;
      nMotorEncoder[curMotor] = 0;
    }
  }
}