/* * 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(); }