// ============================================================================================
// SERVO MOTOR
// ============================================================================================
void servo_motor_control()
{
float servo_pos_flt, speed = 8.0;
servo_pos_flt = 90*cos(mchip.hd * speed);
if(print_servo_info)
fprintf_P(&usart_stream, PSTR("%i.%i, deg: %i.%i\n\r"), prt_flt3(mchip.hd), prt_flt3(servo_pos_flt));
set_servo_position(servo_pos_flt);
}
void servo_motor_control()
{
float servo_pos_flt;
// Angle of axis Aa is calculated by
// Aa = 2.0 * cos(mchip.hd)
// This is just an example. There are many ways to translate
// virtual agent's movement to the servo's angle.
// mchip.hd (range: 0 - 2*PI)
//servo_pos_flt = (mchip.hd-PI)*RADtoDEG;
servo_pos_flt = 180*cos(mchip.hd); // x = r*cos(theta)
fprintf_P(&usart_stream, PSTR("hd: %i.%i, deg: %i.%i\r\n"), prt_flt3(mchip.hd), prt_flt3(servo_pos_flt));
}