void do_stop(void) { LPLD_FTM0_PWM_ChangeDuty(7,0); LPLD_FTM0_PWM_ChangeDuty(6,0); LPLD_FTM0_PWM_ChangeDuty(5,0); LPLD_FTM0_PWM_ChangeDuty(4,0); control_call_callback(C_STOP, E_FINISHED); can_interrupt = 1; }
void right_set_v(int v) { if(v>=0) { LPLD_FTM0_PWM_ChangeDuty(7,v); LPLD_FTM0_PWM_ChangeDuty(6,0); } else { LPLD_FTM0_PWM_ChangeDuty(7,0); LPLD_FTM0_PWM_ChangeDuty(6,-v); } }
void left_set_v(int v) { if(v>=0) { LPLD_FTM0_PWM_ChangeDuty(4,v); LPLD_FTM0_PWM_ChangeDuty(5,0); } else { LPLD_FTM0_PWM_ChangeDuty(4,0); LPLD_FTM0_PWM_ChangeDuty(5,-v); } }
void PIT1_IRQHandler(void) { //PIT_Flag_Clear(PIT1); //清中断标志位 PIT_BASE_PTR->CHANNEL[PIT1].TFLG |= PIT_TFLG_TIF_MASK; static uint8 TimerCntccd = 0; static uint8 TimerCntSpeed = 0; static uint8 TimerCntservo = 0; static uint16 stopcount = 0; static uint16 display_count = 0; static uint16 Miter_count = 0; uint8 integration_piont; TimerCntccd++; TimerCntSpeed++; TimerCntservo++; /* 根据曝光时间计算20ms周期内的曝光点*/ integration_piont = CCDtime - IntegrationTime; if(integration_piont >= 2) { /* 曝光时间小于2则不进行再曝光 */ /* */ if(integration_piont == TimerCntccd) StartIntegration(); ///曝光开始 } if(TimerCntccd >= CCDtime) { TimerCntccd = 0; CCDtime_flag = 1; } if(TimerCntSpeed >=Speedtime) { TimerCntSpeed = 0; Speedtime_flag = 1; } if(TimerCntservo >= Servotime) { TimerCntservo = 0; Servotime_flag = 1; } if(Startflag) { Starttime++; if(Starttime>=1500) { Startflag = 0; Miter_count = 1; } } if(Miter_count>0 && Enable_Miter) { if(++Miter_count>Miter_time) { Miter_count = 0; Miter_flag = 1; } } if(Delayflag) { if(++Delaycount>70) { Delaycount = 0; Delayflag = 0; } } if(Stop_flag) { if(++stopcount>50) { Speed_Out = 0; Speed_PWM = 0; stopcount = 0; LPLD_FTM0_PWM_ChangeDuty(2,Speed_PWM); LPLD_FTM0_PWM_ChangeDuty(1,Speed_PWM); LCD_clear(); LCD_Display_String(0,0,"Stop!"); LCD_Display_String(0,1,"offset="); LCD_Display_Num(notch_offset); LCD_Display_String(0,2,"whiteavg="); //LCD_Display_Num(whiteavg); LCD_Display_String(0,3,"Kp="); LCD_Display_Num(Kp); LCD_Display_String(0,4,"Kd="); LCD_Display_Num(Kd); LCD_Display_String(0,5,"Speed_temp"); LCD_Display_Num(Speed_temp); } } if(Collide_flag) { Speed_Out = 0; Speed_PWM = 0; LPLD_FTM0_PWM_ChangeDuty(2,Speed_PWM); LPLD_FTM0_PWM_ChangeDuty(1,Speed_PWM); if(++display_count>40) { display_count = 0; LPLD_FTM0_PWM_ChangeDuty(2,0); LPLD_FTM0_PWM_ChangeDuty(1,0); LCD_clear(); LCD_Display_String(0,0,"Boom!"); LCD_Display_String(0,1,"offset="); LCD_Display_Num(notch_offset); LCD_Display_String(0,2,"whiteavg="); //LCD_Display_Num(whiteavg); LCD_Display_String(0,3,"Kp="); LCD_Display_Num(Kp); LCD_Display_String(0,4,"Kd="); LCD_Display_Num(Kd); LCD_Display_String(0,5,"Speed_temp"); LCD_Display_Num(Speed_temp); } } }