void amsEncoderSetup(void) {
// Need delay for encoders to be ready
delay_ms(100);
// LSB = R/W* .
// 1. send slave <a2:a1>, a0=W (write reg address)
// 2. send slave register address <a7:a0>,
// 3. send slave <a2:a1>, a0=R (read reg data)
// 4. read slave data <a7:a0>
encAddr[0] = 0b10000001; //Encoder 0 rd;wr A1, A2 = low
encAddr[1] = 0b10000000; // write
encAddr[2] = 0b10000011; //Encoder 1 rd;wr A2 = low, A1 = high
encAddr[3] = 0b10000010;
encAddr[4] = 0b10000101; //Encoder 2 rd;wr A2 = high, A1 = low
encAddr[5] = 0b10000100;
encAddr[6] = 0b10000111; //Encoder 3 rd;wr A2, A1 = high
encAddr[7] = 0b10000110;
encoder_number = 0;
encoder_new_pos = 0;
state = AMS_ENC_IDLE;
//setup I2C port I2C1
encoderSetupPeripheral();
amsEncoderResetPos();
}
// zero position setpoint for both motors (avoids big offset errors)
void pidZeroPos(int pid_num){
// disable interrupts to reset state variables
DisableIntT1; // turn off pid interrupts
amsEncoderResetPos(); // reinitialize rev count and relative zero encoder position for both motors
pidObjs[pid_num].p_state = 0;
// reset position setpoint as well
pidObjs[pid_num].p_input = 0;
pidObjs[pid_num].v_input = 0;
pidObjs[pid_num].leg_stride = 0; // strides also reset
EnableIntT1; // turn on pid interrupts
}
