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;
}
