示例#1
0
void control_system_task(void *data)
{
	(void)data;

	for (;;)
	{

		position_update();

		if (smooth_traj_is_paused())
		{
			// don't move
			motors_wrapper_motor_set_duty_cycle(RIGHT_MOTOR, 0);
			motors_wrapper_motor_set_duty_cycle(LEFT_MOTOR, 0);
		}
		else
		{
			platform_led_toggle(PLATFORM_LED1);

			ausbee_cs_manage(&(am.csm_distance));
			ausbee_cs_manage(&(am.csm_angle));

			control_system_set_motors_ref(am.distance_mm_diff, am.angle_rad_diff);
		}
		//ausbee_cs_manage(&(am.csm_right_motor));
		//ausbee_cs_manage(&(am.csm_left_motor));

		vTaskDelay(CONTROL_SYSTEM_PERIOD_S * 1000 / portTICK_RATE_MS);
	}
}
示例#2
0
void control_system_task(void *data)
{
  for (;;) {
    struct control_system *am = (struct control_system *)data;

#ifdef USE_DISTANCE_ANGLE_CS
    ausbee_cs_manage(&(am->csm_distance));
    ausbee_cs_manage(&(am->csm_angle));

    control_system_set_motors_ref(am, am->distance_mm_diff, am->angle_rad_diff);
#endif

#ifdef USE_MOTOR_SPEED_CS
    ausbee_cs_manage(&(am->csm_right_motor));
    ausbee_cs_manage(&(am->csm_left_motor));
#endif

    platform_led_toggle(PLATFORM_LED1);

    vTaskDelay(CONTROL_SYSTEM_PERIOD_S * 1000 / portTICK_RATE_MS);
  }
}