void servo_init(void) {
timer_tim5_init();
// reset servo objects
for (int i = 0; i < PYB_SERVO_NUM; i++) {
pyb_servo_obj[i].base.type = &pyb_servo_type;
pyb_servo_obj[i].pulse_min = 64;
pyb_servo_obj[i].pulse_max = 242;
pyb_servo_obj[i].pulse_centre = 150;
pyb_servo_obj[i].pulse_angle_90 = 97;
pyb_servo_obj[i].pulse_speed_100 = 70;
pyb_servo_obj[i].pulse_cur = 150;
pyb_servo_obj[i].pulse_dest = 0;
pyb_servo_obj[i].time_left = 0;
}
// assign servo objects to specific pins (must be some permutation of PA0-PA3)
#ifdef pyb_pin_X1
pyb_servo_obj[0].pin = &pyb_pin_X1;
pyb_servo_obj[1].pin = &pyb_pin_X2;
pyb_servo_obj[2].pin = &pyb_pin_X3;
pyb_servo_obj[3].pin = &pyb_pin_X4;
#else
pyb_servo_obj[0].pin = &pin_A0;
pyb_servo_obj[1].pin = &pin_A1;
pyb_servo_obj[2].pin = &pin_A2;
pyb_servo_obj[3].pin = &pin_A3;
#endif
}
void servo_init(void) {
timer_tim5_init();
// reset servo objects
for (int i = 0; i < PYB_SERVO_NUM; i++) {
pyb_servo_obj[i].base.type = &pyb_servo_type;
pyb_servo_obj[i].servo_id = i + 1;
pyb_servo_obj[i].pulse_min = 64;
pyb_servo_obj[i].pulse_max = 242;
pyb_servo_obj[i].pulse_centre = 150;
pyb_servo_obj[i].pulse_angle_90 = 97;
pyb_servo_obj[i].pulse_speed_100 = 70;
pyb_servo_obj[i].pulse_cur = 150;
pyb_servo_obj[i].pulse_dest = 0;
pyb_servo_obj[i].time_left = 0;
}
}