/*
* Intializes hbridges for the motors
*/
static void init_hbridge_data()
{
memset(&Motors[LEFT_MOTOR], 0, sizeof(struct hbridge_info));
memset(&Motors[RIGHT_MOTOR], 0, sizeof(struct hbridge_info));
// Enable reading value of sensor A
PORTSetPinsDigitalIn(HBRIDGE_LEFT_SA_PORT, HBRIDGE_LEFT_SA_BIT);
PORTSetPinsDigitalIn(HBRIDGE_LEFT_SB_PORT, HBRIDGE_LEFT_SB_BIT);
PORTSetPinsDigitalIn(HBRIDGE_RIGHT_SA_PORT, HBRIDGE_RIGHT_SA_BIT);
PORTSetPinsDigitalIn(HBRIDGE_RIGHT_SB_PORT, HBRIDGE_RIGHT_SB_BIT);
// Enable writing direction pin
PORTSetPinsDigitalOut(HBRIDGE_LEFT_DIR_PORT, HBRIDGE_LEFT_DIR_BIT);
PORTSetPinsDigitalOut(HBRIDGE_RIGHT_DIR_PORT, HBRIDGE_RIGHT_DIR_BIT);
// Setup left motor
Motors[LEFT_MOTOR].state.hbridge_id = LEFT_MOTOR;
Motors[LEFT_MOTOR].sensor_bit_pos[PMOD_HB_SENSOR_A] = HBRIDGE_LEFT_SA_BIT;
Motors[LEFT_MOTOR].sensor_port[PMOD_HB_SENSOR_A] = HBRIDGE_LEFT_SA_PORT;
Motors[LEFT_MOTOR].sensor_bit_pos[PMOD_HB_SENSOR_B] = HBRIDGE_LEFT_SB_BIT;
Motors[LEFT_MOTOR].sensor_port[PMOD_HB_SENSOR_B] = HBRIDGE_LEFT_SB_PORT;
Motors[LEFT_MOTOR].dir_port = HBRIDGE_LEFT_DIR_PORT;
Motors[LEFT_MOTOR].dir_bit = HBRIDGE_LEFT_DIR_BIT;
Motors[LEFT_MOTOR].ocn = HBRIDGE_LEFT_OC;
Motors[LEFT_MOTOR].tmrn = HBRIDGE_LEFT_TMR;
// Setup right motor
Motors[RIGHT_MOTOR].state.hbridge_id = RIGHT_MOTOR;
Motors[RIGHT_MOTOR].sensor_bit_pos[PMOD_HB_SENSOR_A] = HBRIDGE_RIGHT_SA_BIT;
Motors[RIGHT_MOTOR].sensor_port[PMOD_HB_SENSOR_A] = HBRIDGE_RIGHT_SA_PORT;
Motors[RIGHT_MOTOR].sensor_bit_pos[PMOD_HB_SENSOR_B] = HBRIDGE_RIGHT_SB_BIT;
Motors[RIGHT_MOTOR].sensor_port[PMOD_HB_SENSOR_B] = HBRIDGE_RIGHT_SB_PORT;
Motors[RIGHT_MOTOR].dir_port = HBRIDGE_RIGHT_DIR_PORT;
Motors[RIGHT_MOTOR].dir_bit = HBRIDGE_RIGHT_DIR_BIT;
Motors[RIGHT_MOTOR].ocn = HBRIDGE_RIGHT_OC;
Motors[RIGHT_MOTOR].tmrn = HBRIDGE_RIGHT_TMR;
// Setup function pointers
Motors[LEFT_MOTOR].state.read_sensor_state = &read_sensor_state;
Motors[RIGHT_MOTOR].state.read_sensor_state = &read_sensor_state;
Motors[LEFT_MOTOR].state.set_direction = &set_direction;
Motors[RIGHT_MOTOR].state.set_direction = &set_direction;
Motors[LEFT_MOTOR].state.clear_direction = &clear_direction;
Motors[RIGHT_MOTOR].state.clear_direction = &clear_direction;
Motors[LEFT_MOTOR].state.set_speed = &set_speed;
Motors[RIGHT_MOTOR].state.set_speed = &set_speed;
Motors[LEFT_MOTOR].state.get_speed = &get_speed;
Motors[RIGHT_MOTOR].state.get_speed = &get_speed;
// Intialize direction
set_hbridge_direction(&(Motors[LEFT_MOTOR].state), PMOD_HB_CCW);
set_hbridge_direction(&(Motors[RIGHT_MOTOR].state), PMOD_HB_CW);
// Intialize speed
set_hbridge_speed(&(Motors[LEFT_MOTOR].state), 0);
set_hbridge_speed(&(Motors[LEFT_MOTOR].state), 0);
set_target_speed(LEFT_MOTOR, 0);
set_target_speed(RIGHT_MOTOR, 0);
}
void pid_init () // call this anytime before calling calculate_pid
{
// set up PID values
set_pid_constants_pitch(PITCH_CONST_P, PITCH_CONST_I, PITCH_CONST_D, PITCH_ALPHA);
set_pid_constants_roll(ROLL_CONST_P, ROLL_CONST_I, ROLL_CONST_D, ROLL_ALPHA);
set_pid_constants_yaw(YAW_CONST_P, YAW_CONST_I, YAW_CONST_D, YAW_ALPHA);
set_pid_constants_depth(DEPTH_CONST_P, DEPTH_CONST_I, DEPTH_CONST_D, DEPTH_ALPHA);
// Initialize target orientation
set_target_depth(0);
set_target_speed(0);
set_target_heading(0);
// Initialize motor linearization lookup table
init_lookup();
}