void motor_init(void){ //motor_deadzone_left=100; //motor_deadzone_right=100; /*connection config: Hardware DIR PWM Physical location ---------------+---------------+---------------+----------------- Motor right PTD9 FTM1_CH1 top?? Motor left PTD7 FTM1_CH0 top?? */ FTM_PWM_init(FTM1,CH0,10000,0);//motor takes 0-10000 pwm values for duty FTM_PWM_init(FTM1,CH1,10000,0);//motor takes 0-10000 pwm values for duty gpio_init(PORTD,9,GPO,0); // Right motor dir gpio_init(PORTD,7,GPO,0); // Left motor dir gpio_init(PORTC,4,GPI_UP,1);//encoder rt dir gpio_init(PORTC,5,GPI_UP,1);//encoder lt dir DisableInterrupts; exti_init(PORTA,6,rising_up); //inits left encoder interrupt capture exti_init(PORTA,7,rising_up); //inits right encoder interrupt capture EnableInterrupts; }
void Device_init() { gpio_init(PTC11,GPI,1); //拨码开关5 gpio_init(PTC12,GPI,1); //拨码开关4 gpio_init(PTC13,GPI,1); //拨码开关3 gpio_init(PTC15,GPI,1); //拨码开关2 gpio_init(PTD2,GPI,1); //拨码开关1 gpio_init(PTD1,GPO,1); //三个指示灯依次亮起 gpio_init(PTD3,GPO,1); gpio_init(PTD4,GPO,1); //每进入一次初始化PTD4灯灭,初始化完成亮起 gpio_init(PTC5,GPO,1); gpio_init(PTC4,GPI,0); //干簧管 FTM_PWM_init(FTM1,FTM_CH0,50,angle_to_period(0)); //舵机初始化,从百分之十占空比到百分之七十占空比测试 FTM_PWM_init(FTM0,FTM_CH1,10000,0); //电机pwm初始化 FTM_PWM_init(FTM0,FTM_CH2,10000,0); //FTM_PWM_init(FTM2,FTM_CH0,3000000,93000); //编码器初始化 //FTM_PWM_init(FTM2,FTM_CH1,3000000,93000); FTM_QUAD_Init(FTM2); //初始化为正交解码 LCD_Init(); PIT_init(); gpio_set(PTD4,1); }
/*********************************************************** 函数名称:MOTORPWM_init 函数功能: 入口参数: 出口参数:无 备 注: ***********************************************************/ void MOTORPWM_init(void) { lptmr_counter_init(LPT0_ALT2, LIN_COUT, 2, LPT_Rising) ; //初始化脉冲计数器 ,PTC5输入捕获脚,LIN_cout =100,预处理为2,上升沿捕获 FTM_PWM_init(FTM0 , CH0, 80000,0); //电机占空比设置初始化 占空比 = duty /(MOD +1 ) ;FTM_CnV_REG(FTMx[ftmn], ch) = cv; FTM_PWM_init(FTM0 , CH1, 80000,0); //电机占空比设置初始化 占空比 = duty /(MOD +1 ) ;FTM_CnV_REG(FTMx[ftmn], ch) = cv; // FTM_PWM_init(FTM0 , CH2, 80000,0); //电机占空比设置初始化 占空比 = duty /(MOD +1 ) ;FTM_CnV_REG(FTMx[ftmn], ch) = cv; // FTM_PWM_init(FTM0 , CH3, 80000,0); //电机占空比设置初始化 占空比 = duty /(MOD +1 ) ;FTM_CnV_REG(FTMx[ftmn], ch) = cv; }
void motor_init() { gpio_init (PORTC,5, GPO,1); //REN 右轮 gpio_init (PORTC,1, GPO,0); //PWM_L1 C3 FTM_PWM_init(FTM0, CH1, 20000, 0); //PWM_L2 C4 gpio_init (PORTC,6, GPO,1); //LEN 左轮 gpio_init (PORTC,4, GPO,0); //PWM_R1 c1 FTM_PWM_init(FTM0, CH2, 20000, 0); //PWM_R2 C2 }
void FTM_init() { FTM_QUAD_Init(FTM1); FTM_QUAD_Init(FTM2); FTM_PWM_init(FTM0,FTM_CH0,20*1000,0);//leftMotorDeadDutyA:340 FTM_PWM_init(FTM0,FTM_CH1,20*1000,0);//leftMotorDeadDutyB:590 FTM_PWM_init(FTM0,FTM_CH2,20*1000,0);//rightMotorDeadDutyA:530 FTM_PWM_init(FTM0,FTM_CH3,20*1000,0);//rightMotorDeadDutyB:380 }
/************************************************************************* * 野火嵌入式开发工作室 * PWM实验示波器简单测试 * * 实验说明:野火PWM实验,用示波器测输出频率。 * * 实验操作:这里用 FTM1_CH0 产生 PWM 脉冲波 ,即 PTA8 管脚。 * 把 PTA8 接入示波器 * * 实验效果:可以测出频率为 35kHz * * 实验目的:测试 PWM 频率是否正确 * * 修改时间:2012-2-29 已测试 * * 备 注:FTM.h 里有各个FTM通道所对应管脚的表格,方便查看 * 占空比传递进入的参数,要根据 FTM_PRECISON 的定义来选择 *************************************************************************/ void main() { FTM_PWM_init(FTM1, CH0, 3000, 50); //初始化FTM1_CH0输出频率为35kHZ,占空比为50%的PWM :FTM1_CH0对应PTA8口 while(1) { } }
/*********************************************************** 函数名称:TURNPWM_init 函数功能: 入口参数: 出口参数:无 备 注: ***********************************************************/ void TURNPWM_init(void) { // FTM_PWM_init(FTM1, CH0 , 180,30); //舵机占空比设置初始化 MOD =17361 ;舵机1 FTM_PWM_init(FTM1, CH1 , 180,30); //舵机占空比设置初始化 MOD =17361 ;舵机2 FTM_CnV_REG(FTMx[FTM1], CH1) = MIDSTRING ; }
/************************************************************************* * 野火嵌入式开发工作室 * PWM实验LED测试 * * 实验说明:野火PWM实验,用LED来测试占空比的变化。 * * 实验操作:这里用 FTM1_CH0 产生 PWM 脉冲波 * 在 FTM.h 里,可以查到 FTM1_CH0 对应管脚为 PTA8 * 把 PTA8 接入 LED0,即 PTA8 和 PTD15 短接 * * 实验效果:可以看到 LED0 由暗变亮,再突然暗,如此下去…… * * 实验目的:测试 PWM 频率是否正确 * * 修改时间:2012-2-29 已测试 * * 备 注:野火Kinetis开发板的 LED0~3 ,分别接PTD15~PTD12 ,低电平点亮 * FTM.h 里有各个FTM通道所对应管脚的表格,方便查看 * 占空比传递进入的参数,要根据 FTM_PRECISON 的定义来选择 *************************************************************************/ void main() { u32 i; FTM_PWM_init(FTM1, CH0, 35000, 100); //FTM1_CH0初始化PWM :PA8 while(1) { for(i = 10; i > 1; i--) { FTM_PWM_Duty(FTM1, CH0, i * 10); //改变占空比,逐渐变小 ,LED 逐渐变亮 (低电平点亮) time_delay_ms(100); //延时100ms } } }
void pit3_system_loop(void) { switch (system_mode) { case 0: if(gpio_get(PORTE, 6) == 0) { system_mode=99; } if(gpio_get(PORTE, 8) == 0) {system_mode=98;} if(gpio_get(PORTE, 9) == 0) {system_mode=97;} if(e<5000){e++;system_mode=0; //wait 5 sec } break; case 99: ideal_speed=300; trankp=14; leftkp=12; rightkp=13; leftkd=8; rightkd=6; system_mode=100; break; case 98: ideal_speed=300; trankp=13; leftkp=13; rightkp=11; leftkd=9; rightkd=7; system_mode=100; break; case 97: ideal_speed=180; trankp=18; leftkp=18; rightkp=14; leftkd=11; rightkd=9; system_mode=100; break; case 100: if(t<3000){t++; //wait 3 sec system_mode=100; } else{ {system_mode=1; FTM_PWM_Duty(FTM1,CH1,3800); }} break; case 1: case 2: case 3: case 4: case 5: case 6: case 7: case 8: case 9: case 10: case 11: case 12: case 13: case 14: case 15: case 16: case 17: case 18: case 19: case 20: //get sensor advalue for(i = 0; i < 6; i++){ ADvalue_left += ad_max(ADC0,SE14,ADC_16bit,20); ADvalue_right += ad_max(ADC0,SE15,ADC_16bit,20);} ADvalue_left=ADvalue_left/6; ADvalue_right=ADvalue_right/6; //decide turn left or turn right ADvalue_left=ADvalue_left-41500; ADvalue_right=ADvalue_right-42000; if(ADvalue_left<0){ADvalue_left=1;} //avoid 0/0 if(ADvalue_right<0){ADvalue_right=1;} k=ADvalue_right-ADvalue_left; // easy to go straight if(k<1400 && k>-1400 && ADvalue_left>6000 && ADvalue_right>6000) {k=750; FTM_PWM_Duty(FTM0,CH1,k); tempk=k; system_mode=system_mode+1; ADvalue_left=0; // set ad value to zero ADvalue_right=0; break; } q=ADvalue_right+ADvalue_left; k=k*1000; k=k/q; if(k>0){ k=k*k;} if(k<0){ k=-k*k;} k=k/1000; k=k*trankp/100; if(k>0){k=k*rightkp/10;} if(k<0){k=k*leftkp/10;} k=750+k; if(k>910){k=909;} if(k<570){k=571;} errork=k-tempk; if(k>0){errork=errork*rightkd/10;} if(k<0){errork=errork*leftkd/10;} k=k+errork; if(k>910){k=909;} if(k<570){k=571;} absoluterrork=k-tempk; if(absoluterrork>50 || absoluterrork<-57){k=tempk;} FTM_PWM_Duty(FTM0,CH1,k); ADvalue_left=0; ADvalue_right=0; tempk=k; system_mode=system_mode+1;//go to next state on next cycle if(s<5000){s++; rsw=0;} else{ if(rsw>1){system_mode=22;}} break; case 21: // speed control encoder20ms = FTM2_CNT; error = ideal_speed-encoder20ms ; if(error==ideal_speed){error= error*7;} if(error>ideal_speed*7/10){error = error*4;} if(error>ideal_speed*9/20){error = error*2;} if(error>ideal_speed*1/5){error = error*1;} if(error>0){error = error*1/5;} if(error<-ideal_speed){error = error*7;} if(error<-ideal_speed*7/10){error = error*4;} if(error<-ideal_speed*9/20){error = error*2;} if(error<-ideal_speed*1/5){error = error*1;} if(error<0){error = error*1/5;} if(encoder20ms!=ideal_speed){final_speed = final_speed+error;} if(final_speed>8000){final_speed=7600;} if( encoder20ms>ideal_speed*2) { gpio_set(PORTD,9,0); final_speed= 2000; } else{gpio_set(PORTD,9,1);} FTM_PWM_Duty(FTM1,CH1,final_speed); FTM2_CNT=0; system_mode=1; //back to the top of pit if(s<5000){s++; rsw=0;} else{ if(rsw>1){system_mode=22;}} break; case 22: FTM_PWM_init(FTM1,CH1,10000,0); break; } PIT_Flag_Clear(PIT3); }
void main() { DisableInterrupts; volatile u8 i; u32 bianliang; flash_init(); gpio_init (PORTA, 24, GPI_UP_PF,1);///////拨码 gpio_init (PORTA, 25, GPI_UP_PF,1); gpio_init (PORTA, 6, GPI_UP_PF,1); gpio_init (PORTA, 10, GPI_UP_PF,1);///////按键 gpio_init (PORTA, 12, GPI_UP_PF,1); gpio_init (PORTA, 8, GPI_UP_PF,1); if(gpio_get(PORTA,24)==1&&gpio_get(PORTA,25)==1&&gpio_get(PORTA,6)==0) { LCD_Init(); //flash_erase_sector(255); // flash_write(255,0x004,0x03e8);/////////////////////////////需要给定初值时 while(1&&tx==0) { if(gpio_get(PORTA,10)==0) { delayms(100); if(gpio_get(PORTA,10)==0)/////////确定按下加计数 { bianliang=flash_read(255,0x004,u32); delayms(50); bianliang+=5; flash_erase_sector(255); delayms(50); flash_write(255,0x004, (u32)bianliang); delayms(50); LCD_CLS(); LCD_PrintValueI(8, 4, bianliang); } } if(gpio_get(PORTA,12)==0) { delayms(100); if(gpio_get(PORTA,12)==0) { bianliang=flash_read(255,0x004,u32); delayms(50); bianliang-=5; flash_erase_sector(255); delayms(50); flash_write(255,0x004,(u32)bianliang); delayms(50); LCD_CLS();//清屏 LCD_PrintValueI(8, 4,bianliang); } } if(gpio_get(PORTA,8)==0) { delayms(100); if(gpio_get(PORTA,8)==0) { tx=1; } else tx=0; } } } /////////////////////////////////////////////////////////////////////////////////////////单道设置档 if(gpio_get(PORTA,24)==0&&gpio_get(PORTA,25)==1&&gpio_get(PORTA,6)==0)//////////////右转正值 { LCD_Init(); while(1) { if(gpio_get(PORTA,10)==0) { delayms(100); if(gpio_get(PORTA,10)==0)/////////单道右转 { delayms(50); flash_erase_sector(254); delayms(50); flash_write(254,0x004, (u32)6); delayms(50); LCD_Char( 10,3,'R'); LCD_Char(26 ,3,'+') ; LCD_Char(32 ,3,'6') ; } } if(gpio_get(PORTA,12)==0) { delayms(100); if(gpio_get(PORTA,12)==0)/////////单道左转 { delayms(50); flash_erase_sector(254); delayms(50); flash_write(254,0x004, (u32)(-6)); delayms(50); LCD_CLS(); LCD_Char( 10,3,'L'); LCD_Char(26 ,3,'-') ; LCD_Char(32 ,3,'6') ; } } if(gpio_get(PORTA,8)==0) { delayms(100); if(gpio_get(PORTA,8)==0)/////////单道直道 { delayms(50); flash_erase_sector(254); delayms(50); flash_write(254,0x004, (u32)0); delayms(50); LCD_CLS(); LCD_Char(32 ,3,'0'); } } } } /////////////////////////////////////////////////////////////////////////////////////////////////////////////速度档位 if(gpio_get(PORTA,24)==1&&gpio_get(PORTA,25)==1&&gpio_get(PORTA,6)==1)///////////////////// { yz=10,yz1=6,yz2=3;///定义搜线阈值 buxianL=25,buxianR=27;//左右补线值 dandao=(int)flash_read(254,0x004,u32); zhijiao=8;//直角延时场数 quanheiMAX=60;//全黑最大值 quanbaiMIN=80;//全白最小值 dandaoBDD=9.455;//////////////////////////////////////////////////////////// dandaoHDD=9.455;//////////////////////////////////////////////////////////// zhijiaoBDpwmmax=500.0; zhijiaoBS_Kp=10.0; zhijiaoBD_Kp=378.25; zhijiaoHDpwmmax=300.0; zhijiaoHS_Kp=42.0;//50轻微振荡 zhijiaoHD_Kp=305.25;//299.25 zkp=2200;//清华PD:2053 卡尔曼PD:2200 zkd=47;//44/////////////50 S_Kpstart=10.0;// 1.3 S_Kistart=33.91;//11.8 S_Kpend=42.0;///////////////29.0 S_Kiend=32.01;//11.8 Sqiwang =1400;// 1.3 Sjishu =7;//11.8 D_Kp =305.25; //299.25 D_Kd =300.0;// D_Kdd=9.455;//12.05 zhangaiDmax=300.0; direction_pwmmax=300.0; } if(gpio_get(PORTA,24)==0&&gpio_get(PORTA,25)==1&&gpio_get(PORTA,6)==1)///////////////////// { yz=10,yz1=6,yz2=3;///定义搜线阈值 buxianL=25,buxianR=27;//左右补线值 dandao=(int)flash_read(254,0x004,u32); zhijiao=10;//直角延时场数 quanheiMAX=60;//全黑最大值 quanbaiMIN=80;//全白最小值 dandaoBDD=9.455;//////////////////////////////////////////////////////////// dandaoHDD=9.455;//////////////////////////////////////////////////////////// zhijiaoBDpwmmax=500.0; zhijiaoBS_Kp=10.0; zhijiaoBD_Kp=378.25; zhijiaoHDpwmmax=300.0; zhijiaoHS_Kp=42.0;//50轻微振荡 zhijiaoHD_Kp=305.25;//299.25 zkp=2200;//清华PD:2053 卡尔曼PD:2200 zkd=47;//44/////////////50 S_Kpstart=10.0;// 1.3 S_Kistart=33.91;//11.8 S_Kpend=42.0;///////////////29.0 S_Kiend=32.01;//11.8 Sqiwang =1400;// 1.3 Sjishu =7;//11.8 D_Kp =305.25; //299.25 D_Kd =300.0;// D_Kdd=9.455;//12.05 zhangaiDmax=300.0; direction_pwmmax=300.0; } if(gpio_get(PORTA,24)==1&&gpio_get(PORTA,25)==0&&gpio_get(PORTA,6)==1)///////////////////// { yz=10,yz1=6,yz2=3;///定义搜线阈值 buxianL=25,buxianR=27;//左右补线值 dandao=(int)flash_read(254,0x004,u32); zhijiao=12;//直角延时场数 quanheiMAX=60;//全黑最大值 quanbaiMIN=80;//全白最小值 dandaoBDD=9.455;//////////////////////////////////////////////////////////// dandaoHDD=9.455;//////////////////////////////////////////////////////////// zhijiaoBDpwmmax=500.0; zhijiaoBS_Kp=10.0; zhijiaoBD_Kp=378.25; zhijiaoHDpwmmax=300.0; zhijiaoHS_Kp=42.0;//50轻微振荡 zhijiaoHD_Kp=305.25;//299.25 zkp=2200;//清华PD:2053 卡尔曼PD:2200 zkd=47;//44/////////////50 S_Kpstart=10.0;// 1.3 S_Kistart=33.91;//11.8 S_Kpend=42.0;///////////////29.0 S_Kiend=32.01;//11.8 Sqiwang =1400;// 1.3 Sjishu =7;//11.8 D_Kp =305.25; //299.25 D_Kd =300.0;// D_Kdd=9.455;//12.05 zhangaiDmax=300.0; direction_pwmmax=300.0; } if(gpio_get(PORTA,24)==0&&gpio_get(PORTA,25)==0&&gpio_get(PORTA,6)==1)//////////////////////最低速 1400 { yz=10,yz1=6,yz2=3;///定义搜线阈值 buxianL=25,buxianR=27;//左右补线值 dandao=(int)flash_read(254,0x004,u32); zhijiao=14;//直角延时场数 quanheiMAX=60;//全黑最大值 quanbaiMIN=80;//全白最小值 dandaoBDD=9.455;//////////////////////////////////////////////////////////// dandaoHDD=9.455;//////////////////////////////////////////////////////////// zhijiaoBDpwmmax=500.0; zhijiaoBS_Kp=10.0; zhijiaoBD_Kp=378.25; zhijiaoHDpwmmax=300.0; zhijiaoHS_Kp=42.0;//50轻微振荡 zhijiaoHD_Kp=305.25;//299.25 zkp=2200;//清华PD:2053 卡尔曼PD:2200 zkd=47;//44/////////////50 S_Kpstart=10.0;// 1.3 S_Kistart=33.91;//11.8 S_Kpend=42.0;///////////////29.0 S_Kiend=32.01;//11.8 Sqiwang =1400;// 1.3 Sjishu =7;//11.8 D_Kp =305.25; //299.25 D_Kd =300.0;// D_Kdd=9.455;//12.05 zhangaiDmax=300.0; direction_pwmmax=300.0; } ////////////////////////////////////////////////////////////////////////////////////////////////////////////特殊速度档 if(gpio_get(PORTA,24)==0&&gpio_get(PORTA,25)==0&&gpio_get(PORTA,6)==0) { yz=10,yz1=6,yz2=3;///定义搜线阈值 buxianL=25,buxianR=27;//左右补线值 dandao=(int)flash_read(254,0x004,u32); zhijiao=14;//直角延时场数 quanheiMAX=60;//全黑最大值 quanbaiMIN=80;//全白最小值 dandaoBDD=9.455;//////////////////////////////////////////////////////////// dandaoHDD=9.455;//////////////////////////////////////////////////////////// zhijiaoBDpwmmax=500.0; zhijiaoBS_Kp=10.0; zhijiaoBD_Kp=378.25; zhijiaoHDpwmmax=300.0; zhijiaoHS_Kp=42.0;//50轻微振荡 zhijiaoHD_Kp=305.25;//299.25 zkp=2400;//清华PD:2053 卡尔曼PD:2200 zkd=47;//44/////////////50 S_Kpstart=10.0;// 1.3 S_Kistart=31.91;//11.8 S_Kpend=42.0;///////////////29.0 S_Kiend=30.01;//11.8 Sqiwang =1400;// 1.3 Sjishu =7;//11.8 D_Kp =305.25; //299.25 D_Kd =300.0;// D_Kdd=9.455;//12.05 zhangaiDmax=300.0; direction_pwmmax=300.0; } if(gpio_get(PORTA,24)==1&&gpio_get(PORTA,25)==0&&gpio_get(PORTA,6)==0)//////////////////////////有值太大 { yz=10,yz1=6,yz2=3;///定义搜线阈值 buxianL=25,buxianR=27;//左右补线值 dandao=(int)flash_read(254,0x004,u32); zhijiao=16;//直角延时场数 quanheiMAX=60;//全黑最大值 quanbaiMIN=80;//全白最小值 dandaoBDD=9.455;//////////////////////////////////////////////////////////// dandaoHDD=9.455;//////////////////////////////////////////////////////////// zhijiaoBDpwmmax=500.0; zhijiaoBS_Kp=10.0; zhijiaoBD_Kp=378.25; zhijiaoHDpwmmax=300.0; zhijiaoHS_Kp=42.0;//50轻微振荡 zhijiaoHD_Kp=305.25;//299.25 zkp=2400;//清华PD:2053 卡尔曼PD:2200 zkd=47;//44/////////////50 S_Kpstart=10.0;// 1.3 S_Kistart=31.91;//11.8 S_Kpend=42.0;///////////////29.0 S_Kiend=30.01;//11.8 Sqiwang =1400;// 1.3 Sjishu =7;//11.8 D_Kp =305.25; //299.25 D_Kd =300.0;// D_Kdd=9.455;//12.05 zhangaiDmax=300.0; direction_pwmmax=300.0; } /**/ //串口初始化 asm("nop"); asm("nop"); asm("nop"); asm("nop"); asm("nop"); asm("nop"); asm("nop"); asm("nop"); if(tx==1) uart_init(UART5,115200); else uart_init(UART5,9600); asm("nop"); asm("nop"); //CCD及其各引脚初始化 CCD1_init() ; CCD2_init() ; asm("nop"); asm("nop"); //PWM输出初始化 FTM_PWM_init(FTM0, CH0, 10000, 0); //R后退 FTM_PWM_init(FTM0, CH1, 10000, 0); //R前进 FTM_PWM_init(FTM0, CH2, 10000, 0); //L后退 FTM_PWM_init(FTM0, CH3, 10000, 0); //L前进 //拨码初始化 // gpio_init (PORTA, 7, GPI_UP_PF,1); asm("nop"); asm("nop"); gpio_init (PORTE, 1, GPI,0); gpio_init (PORTD, 15, GPI,0); gpio_init (PORTD, 14, GPI,0); gpio_init (PORTD, 12,GPI,0); asm("nop"); asm("nop"); asm("nop"); asm("nop"); //正交解码初始化 FTM1_QUAD_Iint(); FTM2_QUAD_Iint(); asm("nop"); asm("nop"); asm("nop"); asm("nop"); //各种中断初始化 pit_init_ms(PIT0, 1); asm("nop"); asm("nop"); asm("nop"); asm("nop"); // 陀螺仪及加速度计初始化 adc_init(ADC0, AD12); ////// PTB2 ENC adc_init(ADC0, AD12); ///PTB3 Z adc_init(ADC0, AD17); ///PTE24 DIR_ENC /////////////////////////////////////////////////陀螺仪开机自检 delayms(1000); gpio_init (PORTA, 17, GPO, 0); MMA7361level=flash_read(255,0x004,u32); rectifyT = ad_ave(ADC0,SE12,ADC_12bit,10); gyro_DIR=ad_ave(ADC0,SE17,ADC_12bit,10); // gyro_DIR=gyro_DIR/20; ///////////////////////////////////////////////// // gpio_init (PORTC, 0, GPI,0);不知道要用否 delayms(1500); EnableInterrupts; while(1) { if((gpio_get(PORTD,12)==0)&&(gpio_get(PORTD,14)==0))//||(gpio_get(PORTE,1)==1) {b=1;d=0;} if(fs>=1) { DisableInterrupts; fs=0;js=0; ImageCapture1(Pixel1); ImageCapture2(Pixel2); EnableInterrupts; Analyze_CCD_Data(); } if(tx==1) { sendDataToCCDViewer(0,Pixel1);////////////////////////////////////// sendDataToCCDViewer(1,Pixel2); sendDataToScope(); } else { OutData[0]=Min_Data1;//L_Max_Data_Place3 OutData[1]=Max_Data1;//R_Max_Data_Place1 OutData[2]=L_Max_Data_Place1; //L_Max_Data_Place1 OutData[3]=R_Max_Data_Place1;// OutPut_Data(); }// //sb[0]=FTM1_CNT; //sb[1]=MMA7361; // tongxun(); } }
void main() { int i=0,j=0,k=0; DisableInterrupts; //禁止总中断 V_Cnt=0; //行计数 Is_SendPhoto=0; //从串口发送图像 uart_init(UART4, 9600); adc_init(ADC0, SE12); adc_init(ADC0, SE13); adc_init(ADC0, SE17); FTM1_QUAD_Iint(); FTM2_QUAD_Iint(); FTM_PWM_init(FTM0, CH0, 8000, 0); //PTC1 1000Hz FTM_PWM_init(FTM0, CH1, 8000, 0); //PTC2 1000Hz FTM_PWM_init(FTM0, CH2, 8000, 0); //PTC3 1000Hz FTM_PWM_init(FTM0, CH3, 8000, 0); //PTC4 1000Hz gpio_init (PORTA, 7, 0, 0); gpio_init (PORTA, 8, 0, 0); gpio_init (PORTE, 28, 0, 0); pit_init_ms(PIT0, 1); delayms(100); for(k=0;k<20;k++) { gyro_offset+=ad_once(ADC0, SE12, ADC_12bit); gyro_DIR+=ad_once(ADC0, SE17, ADC_12bit); } gyro_offset=gyro_offset/20; gyro_DIR=gyro_DIR/20; delayms(1500); /*********************************************************************************/ exti_init(PORTA,9,rising_down); //行中断,PORTA29 端口外部中断初始化 ,上升沿触发中断,内部下拉 disable_irq(87); //行中断关闭 exti_init(PORTB,4,falling_down); //场中断,PORTB0 端口外部中断初始化 ,下降沿触发中断,内部下拉 /*************************************************************************************/ // pll_init(PLL50); // LCD_Init(); EnableInterrupts; //LCD_clear(); while(1) { enableisstartline++; // if(podao==1) podaocount++; if(enableisstartline<1100) IsStartLine=0; else { enableisstartline=1100; CheckStartLine(); Stop2(); } Stop(); // OutData[0]=Gyro_DIRR;//k1;//SubBasePoint; OutData[1]= boma1; OutData[2]=boma2; OutData[3]=boma3; OutPut_Data(); } }
void ImageProc()//赛道宽度150 中心值125 閥值140 { u32 i=0,j=0,m=0; for(i=39;i>=30;i--) //90-99 { for(j=130;j>5;j--) { if(Buffer1[i+row_qian][j]<140) { zuoPos = j; FLAG_ZUO=1; break; } else { zuoPos = j;//未搜索到边界 continue;//继续搜索下一次 } } for(j=130;j<COL-1;j++) { if(Buffer1[i+row_qian][j]<140) { youPos = j;//偏离中间位置的值 FLAG_YOU=1; break; } else { youPos = j; //未搜索到边界 continue;//继续下一次搜索 } } if((FLAG_ZUO==1)&&(FLAG_YOU==1)) { FLAG_ZUO=0; FLAG_YOU=0; capture[39-i]=(zuoPos+youPos)/2; if(capture[39-i]!=130) { middle++; } if(39-i==9) { zuo=zuoPos; you=youPos; }//取出被捕线位置 } else { if((FLAG_ZUO==1)&&(FLAG_YOU==0))//左边搜索到 即右转弯 偏差大于0 { if((zuoPos>50)&&(zuoPos<60)) { capture[39-i]=(zuoPos+280)/2;//280 需要进行调节 考虑这个切内 } if((zuoPos>60)&&(zuoPos<70)) { capture[39-i]=(zuoPos+275)/2;//280 需要进行调节 考虑这个切内 } if((zuoPos>70)&&(zuoPos<80)) { capture[39-i]=(zuoPos+270)/2; } if((zuoPos>80)&&(zuoPos<90)) { capture[39-i]=(zuoPos+265)/2;//上面几句不起作用 } if((zuoPos>90)&&(zuoPos<100)) { capture[39-i]=(zuoPos+270)/2; } else { capture[39-i]=(zuoPos+265)/2; } } if((FLAG_ZUO==0)&&(FLAG_YOU==1))//右边搜索到了 即左转弯 且偏差小于0 { if((youPos>130)&&(youPos<140)) { capture[39-i]=(youPos+40)/2; } if((youPos>140)&&(youPos<150)) { capture[39-i]=(youPos+38)/2; } if((youPos>150)&&(youPos<160)) { capture[39-i]=(youPos+28)/2; } if((youPos>170)&&(youPos<180)) { capture[39-i]=(youPos+27)/2; } if((youPos>180)&&(youPos<190)) { capture[39-i]=(youPos+15)/2; } if((youPos>200)&&(youPos<210)) { capture[39-i]=(youPos-10)/2;//上面几句不起作用 } if((youPos>210)&&(youPos<220)) { capture[39-i]=(youPos-30)/2; } else { capture[39-i]=(youPos-40)/2; } } if((FLAG_ZUO==0)&&(FLAG_YOU==0))//十字弯 { for(m=130;m>1;m--) { if(Buffer1[60][m]<140) { shizileft = m; zuoflag=1; break; } else { shizileft = m; //未搜索到边界 continue;//继续搜索下一次 } } for(m=130;m<COL-5;m++) { if(Buffer1[60][m]<140) { shiziright = m; youflag=1; break; } else { shiziright = m; //未搜索到边界 continue;//继续搜索下一次 } } if((zuoflag==1)&&(youflag==1)&&(shizileft==130)&&(shiziright==130)) { capture[9]=130; } if((zuoflag==1)&&(youflag==1)&&(shizileft!=130)&&(shiziright!=130)) { capture[9]=(shizileft+shiziright)/2; } else { capture[9]=115; } shiziright=0; shizileft=0; zuoflag=0; youflag=0; m=0; FLAG_ZUO=0; FLAG_YOU=0; } } FLAG_ZUO=0; FLAG_YOU=0; } /***************************************************************************/ /************************************************************/ DIR_CONTROL_P=PreDirection; /*******************************下面是越界处理************************/ if(capture[9]==130) { for(i=130;i>110;i--) { if(Buffer1[30+row_qian][i]>140)//左搜最多全黑允许超过20个点进行越界处理 { posleft = i; hinder_left=1; break; } else { posleft = i; //未搜索到边界最多搜10个点越界处理 continue;//继续搜索下一次 } } for(i=130;i<150;i++)//右搜最多搜20个点 { if(Buffer1[30+row_qian][i]>140) { posright = i; hinder_right=1; break; } else { posright = i; //未搜索到边界 continue;//继续搜索下一次 } } if((hinder_left==1)&&(hinder_right==0))//左边搜索到白边 { if(130-posleft<20) { capture[9]=40; } }else if((hinder_left==0)&&(hinder_right==1)) { if(posright-130<20) { capture[9]=220; } }else if((hinder_left==0)&&(hinder_right==0)) { for(i=130;i>115;i--) { if(Buffer1[60][i]>140)//100左搜最多全黑允许超过10个点进行越界处理 { posleft = i; hinder_left=1; break; } else { posleft = i; //未搜索到边界最多搜10个点越界处理 continue;//继续搜索下一次 } } for(i=130;i<145;i++)//右搜最多搜十个点 { if(Buffer1[60][i]>140)//100 { posright = i; hinder_right=1; break; } else { posright = i; //未搜索到边界 continue;//继续搜索下一次 } } if((hinder_left==0)&&(hinder_right==0)) { capture[9]=shizinum; DIR_CONTROL_P=PreDirection1; } } } /*************************人字***************************/ /*****************************起跑线的识别***************************/ if(middle==10&&abs(capture[9]-124)<5) { for(j=80;j<170;j+=3)//每隔3列搜索一次 { for(i=150;i<160;i++) { if(Buffer1[i][j]<140) { FLAG1=1; if(FLAG2==1) { start_run++; FLAG2=0; } } else { FLAG2=1; if(FLAG1==1) { FLAG1=0; start_run+=1; } } } } } if(start_run>=2&&count==8000&&shiziflag==1)//停车 { CAR_SPEED_SET=0; delayms(50); FTM_PWM_init(FTM0, CH4, 20000,0);//电机1正 PTD4 FTM_PWM_init(FTM0, CH5, 20000,0);//电机1反 PTD5 FTM_PWM_init(FTM0, CH6, 20000,0);//电机2正 PTD6 FTM_PWM_init(FTM0, CH7, 20000,0);//电机2反 PTD7 20khz DisableInterrupts; } /**********************************************************************/ FLAG1=0; FLAG2=0; flag_right=0; start_run=0; middle=0; flag_you=0; posleft=0; posright=0; hinder_right=0; hinder_right1=0; hinder_right2=0; hinder_left=0; hinder_left1=0; }