/*******获取角度的模拟值*****/
void Get_AD_angle()
{
u16 Gyro_Angle,J_Angle;
Gyro_Angle=ad_ave(ADC1,SE4a,ADC_12bit,70); //50均值滤波;之前用的是10次滤波。现在改为了100次的滤波（摄像头的问题好像是出现在这里)
J_Angle=ad_ave(ADC1,SE5a,ADC_12bit,70); //均值滤波50;
Gyro_Angle_Num =Gyro_Angle0 - Gyro_Angle;//陀螺仪角速度
J_Angle_Num=J_Angle0-J_Angle;//角度
Gyro_numcha=Gyro_Angle_Num;
}
void PIT0_IRQHandler(void)
{
DisableInterrupts;
TimeCount ++ ;
I1_Vol_present_value=ad_ave(ADC0, AD9,ADC_16bit,10);
I1_Vol_present_value=I1_Vol_present_value/65535*3.3;
U1_present_value=ad_ave(ADC0, AD10,ADC_16bit,10);
U1_present_value=U1_present_value/65535*3.3*16;
U2_present_value=ad_ave(ADC0, AD12,ADC_16bit,10);
U2_present_value=U2_present_value/65535*3.3*16;
sum+=I1_Vol_present_value;
PID_control();
if(TimeCount==3000)
{
TimeCount = 0 ;
sum/=3000;
I1_Vol_present_value=sum;
Refresh_Present();
sum=0;
if(U1_present_value>=24)
{
gpio_init(PORTC,3,GPO,0);
gpio_init(PORTC,4,GPO,0);
start_flag=0;
ftm_flag=0;
}
}
EnableInterrupts;
PIT_Flag_Clear(PIT0); //清中断标志位
}
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()
{
uart_init(UART3, 115200); // For our flashed bluetooth
//uart_init(UART3, 9600); // For our non-flashed bluetooth
gpio_init(PORTE,6,GPI,0); // SW2 goes into gyro calibration mode
printf("\nWelcome to the SmartCar 2013 Sensor team developement system\n");
while(1){
printf("==============================================================\n");
printf("Please select mode:\n---------------------------\n");
printf("1:Accelerometer&gyro\n");
printf("2:LinearCCD\n");
printf("3:Flash Memory\n");
printf("4:Encoder testing\n");
printf("6:Motor control test\n");
printf("7:SystemLoop Test\n");
delayms(300);
if(gpio_get(PORTE,6)==0){
adc_init(ADC1,AD5b);
adc_init(ADC1,AD7b);
printf("We are now in gyro calibration mode, Please keep car stationery and wait till light dies");
gpio_init(PORTE,24,GPO,0);
gpio_init(PORTE,25,GPO,0);
gpio_init(PORTE,26,GPO,0);
gpio_init(PORTE,27,GPO,0);
delayms(3000);
turn_gyro_offset=ad_ave(ADC1,AD5b,ADC_12bit,1000);
balance_gyro_offset=ad_ave(ADC1,AD7b,ADC_12bit,1000);
store_u32_to_flashmem1(turn_gyro_offset);
store_u32_to_flashmem2(balance_gyro_offset);
gpio_turn(PORTE,24);
gpio_turn(PORTE,25);
gpio_turn(PORTE,26);
gpio_turn(PORTE,27);
while(1){}
}
g_char_mode = '7'; // Hard code mode = system loop
//g_char_mode = uart_getchar(UART3);
switch (g_char_mode){
case '0':
//VR analog input
adc_init(ADC0,AD14);
while(1){
delayms(500);
printf("\n%d",ad_once(ADC0,AD14,ADC_16bit));//vr value
}
break;
case '1':
uart_sendStr(UART3,"The mode now is 1: Accelerometer and Gyroscope Test");
//accl_init();
adc_init(ADC1,AD6b);
adc_init(ADC1,AD7b);
adc_init(ADC0,AD14);
adc_init(ADC1,AD4b);
balance_centerpoint_set=ad_ave(ADC0,AD14,ADC_12bit,10);
printf("\nEverything Initialized alright\n");
while(1)
{
//printf("\n\f====================================");
//control_tilt=(ad_ave(ADC1,AD6b,ADC_12bit,8)-3300)+(balance_centerpoint_set/10);
//printf("\nMain gyro%d",control_tilt);//theta
//printf("\n%d",ad_once(ADC1,AD7b,ADC_12bit)-1940);//omega
printf("\n%d",ad_ave(ADC1,AD6b,ADC_12bit,8)-940);
delayms(50);
}
break;
case '2':
uart_sendStr(UART3,"The mode now is 2: Linear CCD");
ccd_interrupts_init();
printf("\nEverything Initialized alright\n");
while(1)
{
//ccd_sampling(1); // Longer SI CCD Sampling
}
break;
case '3':
uart_sendStr(UART3,"The mode now is 3:Flash Memory\n");
Flash_init();
printf("Flash Memory init ok\n");
printf("writing the u32 : 3125 to memory 1\n");
store_u32_to_flashmem1(3125);
printf("writing the u32: 1019 to memory 2\n");
store_u32_to_flashmem2(1019);
printf("Now reading from memory\n");
printf("Memory 1:%d\n",get_u32_from_flashmem1());
printf("Memory 2:%d\n",get_u32_from_flashmem2());
while(1)
{
//stops loop to see results
}
break;
case '4':
//temporary case for debuggine encoder libraries, move to encoder.h later
uart_sendStr(UART3,"The mode now is 4: encoder test");
DisableInterrupts;
exti_init(PORTA,6,rising_up); //inits left encoder interrupt capture
exti_init(PORTA,7,rising_up); //inits right encoder interrupt capture
pit_init_ms(PIT1,500); //periodic interrupt every 500ms
EnableInterrupts;
printf("\nEverything Initialized alright\n");
while(1){
}
break;
case '6':
uart_sendStr(UART3,"The mode now is 6: Motor Control test");
//inits
motor_init();
printf("\nEverything Initialized alright\n");
delayms(1000);
while(1)
{
//printf("\n\fInput 0-9 Motor Speed, Currently:%d",motor_test);
//motor_test=100*(uart_getchar(UART3)-48);
FTM_PWM_Duty(FTM1,CH0,2000);
FTM_PWM_Duty(FTM1,CH1,2000);//left
printf("\n\f Input direction : 0 or 1");
motor_test = uart_getchar(UART3)-48;
if (motor_test){
gpio_set(PORTD,9,1);
gpio_set(PORTD,7,1);//this is DIR
}else{
gpio_set(PORTD,9,0);
gpio_set(PORTD,1,0);//this is DIR
}
}
break;
case '7':
printf("\n The Mode is now 7: SystemLoop Test");
adc_init(ADC1,AD6b);
adc_init(ADC1,AD7b);
adc_init(ADC0,AD14);
adc_init(ADC1,AD5b);
balance_centerpoint_set=ad_ave(ADC0,AD14,ADC_12bit,10);
motor_init();
gpio_set(PORTD,9,0); //dir
gpio_set(PORTD,7,0); //dir
FTM_PWM_Duty(FTM1, CH0, 3000); //initital speed
FTM_PWM_Duty(FTM1, CH1, 3000); //initital speed
ccd_interrupts_init();
pit_init_ms(PIT3,1);
printf("\nEverything inited alright");
while(1){
//system loop runs
}
break;
default :
printf("\n\fYou entered:%c, Please enter a number from 1-7 to select a mode\n\f",g_char_mode);
}
}
}
void main()
{
OLED_Init(); //初始化oled
qd=1;
ftm_pwm_init(FTM0,FTM_CH3,10000,0);
ftm_pwm_init(FTM0,FTM_CH4,10000,0);
ftm_pwm_init(FTM2,FTM_CH0,10000,0);
ftm_pwm_init(FTM2,FTM_CH1,10000,0);
adc_init (ADC1_SE10);
adc_init (ADC1_SE11);
adc_init (ADC1_SE12);
adc_init (ADC1_SE13); //按键初始化
gpio_init (PTA13, GPI,HIGH);//拨码开关初始化
gpio_init (PTA19, GPI,HIGH);
gpio_init (PTA24, GPI,HIGH);
gpio_init (PTA25, GPI,HIGH);
gpio_init (PTA26, GPI,HIGH);
gpio_init (PTA27, GPI,HIGH);
gpio_init (PTA28, GPI,HIGH);
gpio_init (PTA29, GPI,HIGH);
led_init (LED0);
mpu6050_init();
lptmr_delay_ms(1000);
gyro_zero=ad_ave(100);
gyro_zero1=ad_ave1(100);
mpu6050_read();
accel_accel=(accel_x-accel_zero)/16384.0;
if(accel_accel>1) accel_accel=1;
if(accel_accel<-1) accel_accel=-1;
angle_fuse=180/pi*(asin(accel_accel));
accel_accel1=(accel_y-accel_zero1)/16384.0;
if(accel_accel1>1) accel_accel1=1;
if(accel_accel1<-1) accel_accel1=-1;
angle_fuse1=180/3.1415926*(asin(accel_accel1));
pit_init_ms(PIT0, 5); //初始化PIT0，定时时间为： 5ms
set_vector_handler(PIT0_VECTORn ,PIT0_IRQHandler); //设置PIT0的中断服务函数为 PIT0_IRQHandler
enable_irq (PIT0_IRQn); //使能PIT0中断
uart_init(UART3, 115200);
while(aa<200); //初始化 1秒
DIP_switch();
while(1)
{
//display[0]=angle_fuse;
//display[1]=angle_fuse1;
display[0]=angle_fuse3;
oledplay();
if(flag==1)
mode1();
else if (flag==2)
mode2();
else if (flag==3)
mode3();
else if (flag==4)
mode4();
vcan_sendware((unsigned char *)display, 20);
}
}