int move(float angle, float radius_or_distance, float max_speed, float end_speed)
{
pid_loop.start_state = FALSE;
encoderResetDistance(&left_encoder);
encoderResetDistance(&right_encoder);
GyroResetAngle();
float distance;
float slip_compensation;
float distance_per_wheel;
speed_params.sign = SIGN(radius_or_distance);
radius_or_distance = fabsf(radius_or_distance);
position_params.sign = SIGN(angle);
angle = fabsf(angle);
/* Apply the correction factor, delete function with the future gyro compensation */
slip_compensation = 1.0;
speed_params.end_speed = end_speed;
speed_params.max_speed = max_speed;
speed_params.accel = MAX_ACCEL;
speed_params.decel = MAX_ACCEL;
control_params.speed_state = TRUE;
if (angle == 0)
{
// position_control.position_type = ENCODERS;
position_control.position_type = GYRO;
speedProfileCompute(radius_or_distance);
positionProfileCompute(0,0);
}
else
{
position_control.position_type = GYRO;
// position_control.position_type = ENCODERS;
control_params.wall_follow_state = FALSE;
distance_per_wheel = (2.00 * PI * ROTATION_DIAMETER * (angle / 360.00)) * slip_compensation;
distance = fabsf((PI * (2.00 * radius_or_distance) * (angle / 360.00)));
positionProfileCompute(distance_per_wheel, speedProfileCompute(distance));
}
pid_loop.start_state = TRUE;
return POSITION_CONTROL_E_SUCCESS;
}
int speedControlInit(void)
{
memset(&speed_control, 0, sizeof(speed_control_struct));
memset(&speed_params, 0, sizeof(speed_params_struct));
speedProfileCompute(0, 0, 0, 0);
encoder_pid_instance.Kp = 700;//638.00;
encoder_pid_instance.Ki = 0;//0.02666 / CONTROL_TIME_FREQ;
encoder_pid_instance.Kd = 2 * CONTROL_TIME_FREQ;
speed_control.speed_pid.instance = &encoder_pid_instance;
pidControllerInit(speed_control.speed_pid.instance);
return SPEED_CONTROL_E_SUCCESS;
}
