void wheel_init (void)
{
pid_enable = TRUE; /* default enable PID */
encoder_init(); /* initialize tacho encoders */
motor_init(); /* initialize PWM */
motor_set_param (WHEEL_LEFT, pid_interval*10, 1, 0, 0); /* dT=100ms, Kp=1 Ki=0, Kd=0 */
pid_int_init (&pid_l); /* initialize PID controller (left) */
motor_set_param (WHEEL_RIGHT, pid_interval*10, 1, 0, 0); /* dT=100ms, Kp=1 Ki=0, Kd=0 */
pid_int_init (&pid_r); /* initialize PID controller (right) */
}
void nmea_rx_parse(void)
{
if (rx[3] == 'C' && rx[4] == 'T') /* control */
{
nmea_wd = 0; /* reset watchdog timeout */
rx_ite = 5; /* jump to first value */
vel_set_l = nmea_rx_next_val();
if (rx_ite != -1)
vel_set_r = nmea_rx_next_val();
}
else if (rx[3] == 'C' && rx[4] == 'P') /* Communication Parameters */
{
rx_ite = 5; /* jump to first value */
pfbst_interval = nmea_rx_next_val();
if (rx_ite != -1)
nmea_wd_timeout = nmea_rx_next_val();
}
else if (rx[3] == 'S' && rx[4] == 'P') /* System Parameters */
{
rx_ite = 5; /* jump to first value */
voltage_min = nmea_rx_next_val();
}
else if (rx[3] == 'W' && rx[4] == 'P') /* Wheel Parameters */
{
rx_ite = 5; /* jump to first value */
pid_enable = nmea_rx_next_val();
if (pid_enable == 1)
{
long Kp, Ki, Kd, i_max, feed_fwd;
pid_interval = nmea_rx_next_val();
pid_rate = 1000/pid_interval; /* always remember after setting pid_interval */
if (rx_ite != -1)
{
feed_fwd = nmea_rx_next_val();
if (rx_ite != -1)
{
Kp = nmea_rx_next_val();
if (rx_ite != -1)
{
Ki = nmea_rx_next_val();
if (rx_ite != -1)
{
Kd = nmea_rx_next_val();
if (rx_ite != -1)
{
i_max = nmea_rx_next_val();
wheel_param_received = true;
motor_set_param (pid_interval, Kp, Ki, Kd, i_max, feed_fwd);
}
}
}
}
}
}
}
}
