/*************************************************************************
* 野火嵌入式開發工作室
*
* 函數名稱：ad_mid
* 功能說明：采集三次一路模擬量的AD值，返回 中值
* 參數說明：ADCx 模塊號（ ADC0、 ADC1）
* ADC_Channel 通道號
* ADC_nbit 精度（ ADC_8bit,ADC_12bit, ADC_10bit, ADC_16bit ）
* 函數返回：無符號結果值
* 修改時間：2012-2-10
* 備 注：修改蘇州大學的例程
*************************************************************************/
u16 ad_mid(ADCn adcn,ADC_Ch ch,ADC_nbit bit)
{
u16 i,j,k,tmp;
ASSERT( ((adcn == ADC0) && (ch>=AD8 && ch<=AD18)) || ((adcn == ADC1)&& (ch>=AD4a && ch<=AD17)) ) ; //使用斷言檢測ADCn_CHn是否正常
//3次ADC轉換
i = ad_once(adcn,ch,bit);
j = ad_once(adcn,ch,bit);
k = ad_once(adcn,ch,bit);
//取中值
tmp = i>j ? i:j; //tmp取兩者最大值
return k > tmp ? tmp : ( k>i ? k : i );
// k>tmp>i tmp>k>i tmp>i>k
}
void parameter_init()
{
//Acc_z_init[] = ad_once(ADC0, SE13, ADC_12bit); //z轴最小值107最大值141中间124 3点测
int i=0;
for(i=0;i<20;i++)
{
Gyro_init[i]= ad_once(ADC0, SE12, ADC_12bit); //陀螺仪零偏值105
}
}
/*************************************************************************
* 野火嵌入式開發工作室
*
* 函數名稱：ad_ave
* 功能說明：多次采樣，取平均值
* 參數說明：ADCx 模塊號（ ADC0、 ADC1）
* ADC_Channel 通道號
* ADC_nbit 精度（ ADC_8bit,ADC_12bit, ADC_10bit, ADC_16bit ）
* N 均值濾波次數(範圍:0~255)
* 函數返回：16位無符號結果值
* 修改時間：2012-2-10
* 備 注：修改蘇州大學的例程
*************************************************************************/
u16 ad_ave(ADCn adcn,ADC_Ch ch,ADC_nbit bit,u8 N) //均值濾波
{
u32 tmp = 0;
u8 i;
ASSERT( ((adcn == ADC0) && (ch>=AD8 && ch<=AD18)) || ((adcn == ADC1)&& (ch>=AD4a && ch<=AD17)) ) ; //使用斷言檢測ADCn_CHn是否正常
for(i = 0; i < N; i++)
tmp += ad_once(adcn,ch,bit);
tmp = tmp / N;
return (u16)tmp;
}
//归一化计算并得到真实角度值//
void AD_Calculate(void)
{
// ENC03 ad角速度
// MMA7361 ad角度
// Gyro_Now归一角速度
// angle_offset_vertical归一角度
MMA7361=ad_once(ADC0, SE13, ADC_12bit);
ENC03=ad_once(ADC0, SE12, ADC_12bit);
ENC03_DIR=ad_once(ADC0, SE17, ADC_12bit);
Gyro_Now = (gyro_offset - ENC03 ) * 0.2 ;//0.2,0.3
Gyro_DIRR = -(gyro_DIR - ENC03_DIR ) * 0.1 ;
angle_offset_vertical = (1257- MMA7361) *0.13;//减小向前 1257
//if(Gyro_DIR>=0) Gyro_DIR=Gyro_DIRR*P_DIRR;
// else Gyro_DIR=Gyro_DIRR*P_DIRL;
//if(angle_offset_vertical > 90)angle_offset_vertical = 90;
// if(angle_offset_vertical < -90)angle_offset_vertical = -90;
//Kalman_Filter(angle_offset_vertical,Gyro_Now) ;
QingHua_AngleCalaulate(angle_offset_vertical,Gyro_Now) ;
}
u16 ad_flt(ADCn adcn, ADC_Ch ch, ADC_nbit bit)
{
static u16 buf[(1<<(SAMP_COUNT))] = {0}; //保存前 2^SAMP_COUNT 次 的采样数据
static u8 n = (u8)(0x100 - (s8)(1 << (SAMP_COUNT)));
static u32 sum = 0;
ASSERT( ((adcn == ADC0) && (ch >= AD8 && ch <= AD18)) || ((adcn == ADC1) && (ch >= AD4a && ch <= AD17)) ) ; //使用断言检测ADCn_CHn是否正常
if(n >= (u8)(0x100 - (s8)(1 << (SAMP_COUNT))))
{
buf[(u8)((1<<(SAMP_COUNT))+n)] = ad_once(adcn, ch, bit);
sum += buf[(u8)((1<<(SAMP_COUNT))+n)];
n++;
return ((u16)(sum >> SAMP_COUNT));
}
void directcontrol(int ccdcenter)
{
gyro=ad_once (ADC0,AD17,ADC_12bit);
D_Last_fDValue = fDValue;
g_fDirectionControlOutOld=g_fDirectionControlOutNew;
if(Rzhangai==0&& Lzhangai==0)
fDValue=ccdcenter-64;
else
{
if(Rzhangai==1)
fDValue=ccdcenter-74;
else
fDValue=ccdcenter-54;
}
out_kp = D_Kp * fDValue/10.0;
out_kd = D_Kd * (fDValue - D_Last_fDValue)/10.0;
out_kdd = D_Kdd * ( gyro_DIR- gyro)/10.0;
if(out_kd>=zhangaiDmax)
out_kd=zhangaiDmax;
if(out_kd<=-zhangaiDmax)
out_kd=-zhangaiDmax;
direction_pwm=out_kp + out_kdd+out_kd;
if(direction_pwm>=direction_pwmmax)/////////////////////////¿ØÖÆÊ®×ÖÄÚÇÐ
direction_pwm=direction_pwmmax;
if(direction_pwm<=-direction_pwmmax)
direction_pwm=-direction_pwmmax;
g_fDirectionControlOutNew=direction_pwm;
}
void kalman_update(void)
{
float Q =0.5,R = 20.0; //R越小跟的越紧Q陀螺仪 R加速度计
static float RealData = 0,RealData_P = 0;
static int i ;
float NowData = 0,NowData_P = 0;
float Kg = 0,gyroscope_rate = 0,accelerometer_angle=0;
float Acc_z = 0, Gyro = 0;
// static float his_acc = 0.0,his_accx = 0.0,his_accz = 0.0;
// while(!ATD0STAT0_SCF); //等待转换完成
acc_zero=acc_o;
acc_speed_zero=acc_o;
Acc_z = ad_once(ADC0, SE13, ADC_12bit); //z轴最小值107最大值141中间124 3点测
Gyro = ad_once(ADC0, SE12, ADC_12bit); //陀螺仪零偏值105
if(i<100)
{
i++;
}
else
{
power = ad_once(ADC0, SE8, ADC_12bit); //采集电池电压值
i = 0;
}
//uart_putchar (UART2, 0x03);
//uart_putchar (UART2, 0xfc);
//uart_putchar (UART2, Acc_z);
//uart_putchar (UART2, Gyro);
//uart_putchar (UART2, 0xfc);
//uart_putchar (UART2, 0x03);
//OutData[0] = Gyro;
//OutData[3]=Acc_z; //输出绝对的倾角
OutData[3]=power;
acc_z_juedui=Acc_z;
Acc_z = Acc_z - acc_speed_zero; //1988中间平衡
Gyro = Gyro - gyro_zero;
/*if(Gyro>80.0)
{
Gyro = 80.0;
}
else if(Gyro< -80.0)
{
Gyro = -80.0;
}*/
Gyro_Data = Gyro;
OutData[0] = Gyro_Data;
// Gyro_intergral=Gyro_intergral+ Gyro_Data*0.01*(-15.34); //陀螺仪积分，15.34 放大倍数 0.005 5毫秒
//OutData[2]=Gyro_intergral;
gyro_rate=-Gyro_Data; //加速度计的微分
accelerometer_angle =Acc_z; //
OutData[1] = accelerometer_angle;
gyroscope_rate = Gyro*1; //0.414 //参考电压3.3v 12位ADC 放大9.1倍 enc-03 0.67mv/deg./sec.
//(3300/4096)/(0.67*9.1)*(3.14/180) = 0.0023
NowData = RealData + gyroscope_rate*0.01*(-15.34); //1.预估计 X(k|k-1) = A(k,k-1)*X(k-1|k-1) + B(k)*u(k)
NowData_P = sqrt(Q*Q+RealData_P*RealData_P); //2.计算预估计协方差矩阵 P(k|k-1) = A(k,k-1)*P(k-1|k-1)*A(k,k-1)'+Q(k)
Kg = sqrt(NowData_P*NowData_P/(NowData_P*NowData_P+R*R)); //3.计算卡尔曼增益矩阵 K(k) = P(k|k-1)*H(k)' / (H(k)*P(k|k-1)*H(k)' + R(k))
RealData = NowData + Kg*(accelerometer_angle - NowData); //4.更新估计 X(k|k) = X(k|k-1)+K(k)*(Z(k)-H(k)*X(k|k-1))
RealData_P = sqrt((1-Kg)*NowData_P*NowData_P); //5.计算更新后估计协防差矩阵 P(k|k) =（I-K(k)*H(k)）*P(k|k-1)
//Q是陀螺仪偏差R是加速度计偏差
QingJiao = RealData;
OutData[2] = QingJiao;
if(xianshi==1)
{
OutPut_Data();
}
}
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()
{ 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();
}
}
static u16 buf[(1<<(SAMP_COUNT))] = {0}; //保存前 2^SAMP_COUNT 次 的采样数据
static u8 n = (u8)(0x100 - (s8)(1 << (SAMP_COUNT)));
static u32 sum = 0;
ASSERT( ((adcn == ADC0) && (ch >= AD8 && ch <= AD18)) || ((adcn == ADC1) && (ch >= AD4a && ch <= AD17)) ) ; //使用断言检测ADCn_CHn是否正常
if(n >= (u8)(0x100 - (s8)(1 << (SAMP_COUNT))))
{
buf[(u8)((1<<(SAMP_COUNT))+n)] = ad_once(adcn, ch, bit);
sum += buf[(u8)((1<<(SAMP_COUNT))+n)];
n++;
return ((u16)(sum >> SAMP_COUNT));
}
sum -= buf[n];
buf[n] = ad_once(adcn, ch, bit);
sum += buf[n];
if (++n >= (1 << (SAMP_COUNT)))
{
n = 0;
}
return ((u16)(sum >> SAMP_COUNT)); /* ADC采样值由若干次采样值平均 */
}
#undef SAMP_COUNT
/*************************************************************************
