Download PCA9685
Transcript
// fin código específico funcion inicializa cambio_magnitud
}
/*********************************************************
* void funcionNormal()
**********************************************************/
void funcion_normal_cambio_magnitud(void *instancia, int t_ciclo) {
struct cambio_magnitud *este = (struct cambio_magnitud *)instancia;
#ifdef LOG
// sprintf(msgLog, "Normal function '%s'...", este->nombre);
// logPrint(msgLog, 6);
#endif
struct timespec ts;
crononsec(1, &ts);
// principio código específico funcion normal cambio_magnitud
if ((*este->out_pitch > 0.005) || (*este->out_pitch < -0.005)){
este->pwm_motor_gris1 = floor(este->potencia + (*este->out_pitch * 0.95 * 1) +
0.5);
este->pwm_motor_gris2 = floor(este->potencia - (*este->out_pitch * 1.0 * 1) + 0.5);
}
else{
este->pwm_motor_gris1 = este->potencia;
este->pwm_motor_gris2 = este->potencia;
}
if ((*este->out_roll > 0.005) || (*este->out_roll < -0.005)){
este->pwm_motor_negro1 = floor(este->potencia + (*este->out_roll * 0.98 * 1) +
0.5);
este->pwm_motor_negro2 = floor(este->potencia - (*este->out_roll * 1.02 * 1) +
0.5);
}
else{
este->pwm_motor_negro1 = este->potencia;
este->pwm_motor_negro2 = este->potencia;
}
if (este->pwm_motor_gris1 > 2800){
este->pwm_motor_gris1 = 2800;
}
else if (este->pwm_motor_gris2 > 2800){
este->pwm_motor_gris2 = 2800;
}
else if (este->pwm_motor_gris1 < 1800){
este->pwm_motor_gris1 = 1800;