Ejemplo n.º 1
0
void loop()
{
	static unsigned int wrout = 1;
	if (wrout != 1 << 15) {
		wrout = wrout << 1;
	} else {
		wrout = 1;
	}
	indicator_set(wrout);
	delay(500);
}
void controller_simulation_step(void)
{
    if (last_target != auto_get_target_position()) {
        last_diff = get_difference();
        last_target = auto_get_target_position();
    }

    double current_diff = get_difference();
    indicator_set(difference_indicator_id, current_diff);
    double diff_diff = current_diff - last_diff;

    roller_set_motor_torque(current_diff*3 + diff_diff*1000);

    last_diff = current_diff;

    static int count = 0;
    if (count > 100) {
        printf("%lf %lf %lf\n", auto_get_target_position(), current_diff, roller_get_encoder_value());
        count = 0;
    } else {
        count ++;
    }
}