Skip to content

Instantly share code, notes, and snippets.

@jimmyli97
Last active August 29, 2015 14:15
Show Gist options
  • Save jimmyli97/e7d4a35d322c102ae67b to your computer and use it in GitHub Desktop.
Save jimmyli97/e7d4a35d322c102ae67b to your computer and use it in GitHub Desktop.
longencoder2-19
//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;
}
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment