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 ++;
}
}