摄像头云台设计报告(3)

附录1：电路原理图

8

附录2：源程序

int main(void) {

u8 t=0; u16 lastpos[5][2]; float pitch,roll,yaw; short aacx,aacy,aacz; short gyrox,gyroy,gyroz; short mx,my,mz; u8 error;

u16 pwm_yaw,pwm_roll;

u8 lightmode=1,saturation=2,brightness=2,contrast=2; u8 effect=2; u16 i,j;

u32 color=0; float t_yaw,t_roll;

delay_init();

NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); uart_init(115200); LCD_Init();

TIM4_PWM_Init(4999,143); SCCB_Init() ;

POINT_COLOR=RED;

LCD_ShowString(60,50,200,16,16,\ LCD_ShowString(60,70,200,16,16,\ LCD_ShowString(60,90,200,16,16,\ LCD_ShowString(60,110,200,16,16,\ draw_button();

printf(\ OV7670_Light_Mode(lightmode);

OV7670_Color_Saturation(saturation); OV7670_Brightness(brightness); OV7670_Contrast(contrast);

OV7670_Special_Effects(effect); TIM6_Int_Init(10000,7199); //tp_dev.init(); EXTI8_Init();

OV7670_Window_Set(12,176,240,320); OV7670_CS=0;

printf(\ t_yaw = 0; t_roll = 0;

9

while(mpu_dmp_init()) {

delay_ms(200); //LED0=!LED0; }

while(1) { camera_refresh();

if(mpu_mpl_get_data(&pitch,&roll,&yaw)==0) { error=MPU_Get_Accelerometer(&aacx,&aacy,&aacz); error=MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz); error=MPU_Get_Magnetometer(&mx,&my,&mz);

} printf(\ roll:%f yaw:%f \\n\

tp_dev.scan(0); if((tp_dev.sta)&(1)) { if(tp_dev.x[0]

if(tp_dev.y[0]>400){ if(tp_dev.y[0]<533){ if(tp_dev.x[0]<240)t_yaw= t_yaw+10; if(tp_dev.x[0]>240)t_roll= t_roll+10; } else if(tp_dev.y[0]<666){ if(tp_dev.x[0]<240)t_yaw= t_yaw-10; if(tp_dev.x[0]>240)t_roll= t_roll-10; } } } } if(t_yaw>180)t_yaw = 180; if(t_yaw <-180)t_yaw =-180;

10

if(t_roll>180)t_roll = 180; if(t_roll<-180)t_roll = -180; LCD_Showangle(yaw, roll, pitch,t_yaw,t_roll); LCD_Showpwm(pwm_yaw,pwm_roll);

pwm_set(yaw,t_yaw,roll,t_roll,&pwm_yaw,&pwm_roll); } }

u16 compute_pwm_roll(float angle,float target_angle,u16 pwm){ static float angle_integral_roll=0; int pwm_differ; float kp,ki; float Bias; kp = 4; ki = 1;

Bias = angle -target_angle ;

angle_integral_roll = angle_integral_roll +Bias; if(angle_integral_roll>50)angle_integral_roll=50; if(angle_integral_roll<-50)angle_integral_roll = -50; pwm_differ = kp*Bias + ki * angle_integral_roll; if(pwm_differ > 500)pwm_differ = 500; if(pwm_differ < -500)pwm_differ = -500; pwm = pwm_differ +pwm;

if(pwm>1249)pwm = 1249; if(pwm<249)pwm = 249;

return pwm; }

void pwm_set(float yaw,float t_yaw,float roll,float t_roll,u16 *pwm_yaw,u16 *pwm_roll){

static u16 pwm_last_yaw,pwm_last_roll; pwm_last_yaw = *pwm_yaw; pwm_last_roll = *pwm_roll;

*pwm_yaw = compute_pwm_yaw(yaw,t_yaw,pwm_last_yaw); *pwm_roll = compute_pwm_roll(roll,t_roll,pwm_last_roll); TIM4->CCR2 = *pwm_roll; TIM4->CCR1 = *pwm_yaw; }

11