Exemplo n.º 1
0
// ============================================================================================
// 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));

}