int Fan_Duty_Dn(double delt) { //left channel if(pwm_duty[L_CH-1]-delt<20.0) { pwm_duty[L_CH-1] = 20.0; } else { pwm_duty[L_CH-1] -= delt; } //right channel if(pwm_duty[R_CH-1]-delt<20.0) { pwm_duty[R_CH-1] = 20.0; } else { pwm_duty[R_CH-1] -= delt; } W_PWM_FUNC(pwm_duty[0], pwm_duty[1], pwm_duty[2], pwm_duty[3]); return 1; }
/**@attention :步进增加涵道的占空比 */ int Fan_Duty_Up(double delt) { //left channel if(pwm_duty[L_CH-1]+delt>43.0) { pwm_duty[L_CH-1] = 43.0; } else { pwm_duty[L_CH-1] += delt; } //right channel if(pwm_duty[R_CH-1]+delt>43.0) { pwm_duty[R_CH-1] = 43.0; } else { pwm_duty[R_CH-1] += delt; } W_PWM_FUNC(pwm_duty[0], pwm_duty[1], pwm_duty[2], pwm_duty[3]); return 1; }
/**@attention :给涵道开到最大速度 * */ void Fan_Set_Max(void) { pwm_duty[F_CH - 1] = 43.0; pwm_duty[R_CH - 1] = 43.0; pwm_duty[L_CH - 1] = 43.0; pwm_duty[B_CH - 1] = 43.0; W_PWM_FUNC(pwm_duty[0], pwm_duty[1], pwm_duty[2], pwm_duty[3]); }
/**@attention :pwm_chx range 1~4 */ void Fan_Duty(u8 pwm_chx, double duty) { assert_param(duty<=100.0); /* 限幅 */ if(duty>100.0) { pwm_duty[pwm_chx-1] = 43.0; W_PWM_FUNC(pwm_duty[0], pwm_duty[1], pwm_duty[2], pwm_duty[3]); return; } else if(duty<0.0) { pwm_duty[pwm_chx-1] = 20.0; W_PWM_FUNC(pwm_duty[0], pwm_duty[1], pwm_duty[2], pwm_duty[3]); return; } pwm_duty[pwm_chx-1] = 20.0 + duty*(43.0-20.0)/100.0; W_PWM_FUNC(pwm_duty[0], pwm_duty[1], pwm_duty[2], pwm_duty[3]); }
/**@attention :停止涵道 */ void Fan_Set_Min(void) { pwm_duty[L_CH-1] = 20.0; pwm_duty[R_CH-1] = 20.0; W_PWM_FUNC(pwm_duty[0], pwm_duty[1], pwm_duty[2], pwm_duty[3]); }
/* * 函数名: FanAccelerate * 描 述: 使用涵道风扇为机器人加速 * 输 入: x y 方向 风扇速度 * 输 出: 无 * 调 用: 外部调用 */ void FanAccelerate(double speed_x, double speed_y) { double theta=0; double fan_y = 0, fan_x = 0; double error_angle; double Vout_A; double v=sqrt(speed_x*speed_x+speed_y*speed_y); unsigned char i,j; static double lastpwm[4]={20,20,20,20}; theta = atan2(speed_y, speed_x); error_angle = theta - GPS.radian; Vout_A = 1000 * AnglePID(error_angle, Pid_List[29]); switch ((int) (theta / (pi / 2.0))) { case 0: fan_y = -Vout_A; break; case 1: fan_x = Vout_A; break; case 2: fan_y = Vout_A; break; case 3: fan_x = -Vout_A; break; default: break; } theta -= pi / 4.0; fan_y += sin(theta)*v; fan_x += cos(theta)*v; fan_y=speed_y; fan_x=speed_x; if(fan_y>100.0||fan_y<-100.0) { fan_x*=100.0/Abs(fan_y); fan_y*=100.0/Abs(fan_y); } if(fan_x>100.0||fan_x<-100.0) { fan_y*=100.0/Abs(fan_x); fan_x*=100.0/Abs(fan_x); } pwm_duty[B_CH-1]= fan_y > 0 ? fan_y : 0; pwm_duty[F_CH-1]=fan_y > 0 ? 0 : -fan_y; pwm_duty[L_CH-1]=fan_x > 0 ? fan_x : 0; pwm_duty[R_CH-1]=fan_x > 0 ? 0 : -fan_x; // for(i=0;i<4;i++){ // if(Abs(pwm_duty[i]-lastpwm[i])>0.5){ // for(j=0;j<4;j++) pwm_duty[j]=Abs(lastpwm[i]+0.5)/pwm_duty[j]; // } // } for(i=0;i<4;i++) pwm_duty[i] = 20.0 + pwm_duty[i] * (43.0 - 20.0) / 100.0; W_PWM_FUNC(pwm_duty[0], pwm_duty[1], pwm_duty[2], pwm_duty[3]); // else j++; }