Question

跪求第七届全国大学生飞思卡尔智能车设计大赛，电磁组直立走以mc9s12xs128芯片编程的工程文件，以供参考

Answer 1

下面是官方的程序，我不会的也是参考他的编的。有什么问题可以一起交流啊！我也是做电磁的。

//MCU：Mega16；晶振：8MHz；系统频率：50Hz；20ms；
//赵国霖：10.05.10；
#include <iom16v.h>
#include <macros.h>
#include <math.h>

//#define checkbit(var,bit) (var&(0x01<<(bit))) /*定义查询位函数*/
//#define setbit(var,bit) (var|=(0x01<<(bit))) /*定义置位函数*/
//#define clrbit(var,bit) (var&=(~(0x01<<(bit)))) /*定义清零位函数*/

//-------------------------------------------------------
//输出端口初始化
void PORT_initial()
{
DDRA=0b00000000;
PINA=0X00;
DDRB=0b00000000;
PINB=0X00;
DDRC=0b00010111;
PINC=0X00;
DDRD=0b11110010;
PIND=0X00;
}

//-------------------------------------------------------
//定时器1初始化
void T1_initial()
{
TCCR1A|=(1<<COM1A1)|(1<<WGM10)|(1<<COM1B1); //T1：8位快速PWM模式、匹配时清0，TOP时置位
TCCR1B|=(1<<WGM12)|(1<<CS11); //PWM：8分频：8M/8/256=4KHz;
}

//-------------------------------------------------------
//定时器2初始化
void T2_initial() //T2：计数至OCR2时产生中断
{
OCR2=0X4E; //T2：计数20ms(0X9C)10ms(0X4E)时产生中断;
TIMSK|=(1<<OCIE2);
TCCR2|=(1<<WGM21)|(1<<CS22)|(1<<CS21)|(1<<CS20); //CTC模式，1024分频
}

//-------------------------------------------------------
//外部中断初始化
void INT_initial()
{
MCUCR|=(1<<ISC01)|(1<<ISC00)|(1<<ISC11)|(1<<ISC10); //INT0、INT1上升沿有效
GICR|=(1<<INT0)|(1<<INT1); //外部中断使能
}

//-------------------------------------------------------
//串口初始化；
void USART_initial( unsigned int baud )
{
UBRRH = (unsigned char)(baud>>8); //设置波特率；
UBRRL = (unsigned char)baud;
UCSRB = (1<<RXEN)|(1<<TXEN); //接收器与发送器使能；
UCSRC = (1<<URSEL)|(1<<UCSZ0)|(1<<UCSZ1); //设置帧格式: 8 个数据位, 1 个停止位；
}

//-------------------------------------------------------
//串口发送数据；
void USART_output( unsigned char data )
{
while ( !( UCSRA & (1<<UDRE))); //等待发送缓冲器为空；
UDR = data; //将数据放入缓冲器，发送数据；
}

//-------------------------------------------------------
//计算LH侧轮速：INT0中断；
//-------------------------------------------------------
int speed_real_LH;
//-------------------------------------------------------
#pragma interrupt_handler SPEEDLHINT_fun:2
void SPEEDLHINT_fun()
{
if (0==(PIND&BIT(0)))
{
speed_real_LH+=1;
}
else
{
speed_real_LH-=1;
}
}

//-------------------------------------------------------
//计算RH侧轮速，：INT1中断；
//同时将轮速信号统一成前进方向了；
//-------------------------------------------------------
int speed_real_RH;
//-------------------------------------------------------
#pragma interrupt_handler SPEEDRHINT_fun:3
void SPEEDRHINT_fun()
{
if (0==(PIND&BIT(1)))
{
speed_real_RH-=1;
}
else
{
speed_real_RH+=1;
}
}

//-------------------------------------------------------
//ADport采样：8位，采样基准电压Aref
//-------------------------------------------------------
//static int AD_data;
//-------------------------------------------------------
int ADport(unsigned char port)
{
ADMUX=port|(1<<ADLAR);
ADCSR|=(1<<ADEN)|(1<<ADSC)|(1<<ADPS1)|(1<<ADPS0); //采样频率为8分频；
while(!(ADCSR&(BIT(ADIF))));
// AD_data=ADCH;
// AD_data-=128;
return (ADCH-128);
}

/*
//-------------------------------------------------------
//Kalman滤波
//-------------------------------------------------------
extern float angle, angle_dot;

static const float Q_angle=0.001, Q_gyro=0.003, R_angle=0.5, dt=0.05; //注意：dt的取值为kalman滤波器采样时间;
static float P[2][2] = {
{ 1, 0 },
{ 0, 1 }
};

static float Pdot[4] ={0,0,0,0};

static const char C_0 = 1;

static float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;
//-------------------------------------------------------
void Kalman_Filter(float angle_m,float q_m) //q_m:gyro_measure
{
angle+=(q_m-q_bias) * dt;

Pdot[0]=Q_angle - P[0][1] - P[1][0];
Pdot[1]=- P[1][1];
Pdot[2]=- P[1][1];
Pdot[3]=Q_gyro;

P[0][0] += Pdot[0] * dt;
P[0][1] += Pdot[1] * dt;
P[1][0] += Pdot[2] * dt;
P[1][1] += Pdot[3] * dt;

angle_err = angle_m - angle;

PCt_0 = C_0 * P[0][0];
PCt_1 = C_0 * P[1][0];

E = R_angle + C_0 * PCt_0;

K_0 = PCt_0 / E;
K_1 = PCt_1 / E;

t_0 = PCt_0;
t_1 = C_0 * P[0][1];

P[0][0] -= K_0 * t_0;
P[0][1] -= K_0 * t_1;
P[1][0] -= K_1 * t_0;
P[1][1] -= K_1 * t_1;

angle += K_0 * angle_err;
q_bias += K_1 * angle_err;
angle_dot = q_m-q_bias;
}
*/

//-------------------------------------------------------
//互补滤波
//************************
extern float angle_cfed;
static float bias_cf,gyro_cf_temp;
static float dt=0.01;
//************************
void complement_filter(float angle_cf,float gyro_cf)
{
bias_cf*=0.998; //陀螺仪零飘低通滤波；500次均值；
bias_cf+=gyro_cf*0.002;
gyro_cf_temp=gyro_cf-bias_cf;
angle_cfed=(angle_cfed+gyro_cf_temp*dt)*0.95+angle_cf*0.05;
//加速度低通滤波；20次均值；按100次每秒计算，低通5Hz；
}

//-------------------------------------------------------
//AD采样；
//以角度表示；
//加速度计：1.2V=1g=90°；满量程：1.3V~3.7V；
//陀螺仪：0.5V~4.5V=-80°~+80°；满量程5V=200°=256=200°；
//-------------------------------------------------------
//extern int speed_need,turn_need;//,angle_cfed;
static float gyro,acceler; //phi;
//unsigned char temp_data;
//-------------------------------------------------------
void AD_calculate()
{
extern float angle;

//GICR&=(~(1<<INT0) & ~(1<<INT1)); //采样期间屏蔽外部中断

acceler=ADport(2);
gyro=ADport(3);

acceler*=1.465; //以角度表示：90/（128*1.2/2.5）=1.46484375；
gyro*=0.78125; //角速度系数：100/128=0.78125；

complement_filter(acceler,gyro);
}

//-------------------------------------------------------
//PWM输出
void PWM_output (int PWM_LH,int PWM_RH)
{

if (PWM_LH<-255)
{
PWM_LH=255;
PORTD|=BIT(6);
}
else if (PWM_LH<0)
{
PWM_LH=-PWM_LH;
PORTD|=BIT(6);
}
else if (PWM_LH==0)
{
PWM_LH=1;
}
else if (PWM_LH<256)
{
PORTD&=~BIT(6);
}
else if (PWM_LH>255)
{
PWM_LH=255;
PORTD&=~BIT(6);
}

if (PWM_RH<-255)
{
PWM_RH=255;
PORTD|=BIT(7);
}
else if (PWM_RH<0)
{
PWM_RH=-PWM_RH;
PORTD|=BIT(7);
}
else if (PWM_RH==0)
{
PWM_RH=1;
}
else if (PWM_RH<256)
{
PORTD&=~BIT(7);
}
else if (PWM_RH>255)
{
PWM_RH=255;
PORTD&=~BIT(7);
}

OCR1AH=0;
OCR1AL=PWM_LH; //OC1A输出；

OCR1BH=0;
OCR1BL=PWM_RH; //OC1B输出；

}

//-------------------------------------------------------
//计算PWM输出值
//车辆直径：76mm; 12*64pulse/rev; 1m=3216pulses;
//*************************************//
//extern int speed_real_LH,speed_real_RH;

//const float Kx=0.054/3216; //位移系数：1m=/3216;
//const float Kx_dot=2.95/64.32; //速度系数:(count*50Hz/(12*64pulse/rev))*0.238.75m/rev=50/3216;
//const float Kphi=9; //角度比例调整;
//const float Kphi_dot=0.6; //角速度比例调整;

//static int spall,spdiff,speed_error,AVR_output;
//static int differr_0,differr_1,differr_2,differr_all;
static int speed_output_LH,speed_output_RH;
//*************************************//
void PWM_calculate()
{

//spall=(speed_real_LH+speed_real_RH)*0.5;
//spdiff=speed_real_RH-speed_real_LH;
//speed_error+=spall-speed_need;
//AVR_output=Kx*speed_error+Kx_dot*spall+Kphi*angle+Kphi_dot*angle_dot; //计算所需电压；
//AVR_output*=36; //PWM:256/7.2V=35.55;
/*
if (AVR_output>=0)
{
AVR_output+=4;
}
else
{
AVR_output-=4;
}

differr_2=differr_1; //左右轮速差PID控制；
differr_1=differr_0;
differr_0=turn_need-spdiff;
differr_all += 1*(differr_0 - differr_1) + 0.2*differr_0 + 2*(differr_0 - 2*(differr_1) + differr_2);
*/
//speed_output_LH = AVR_output;
//speed_output_RH = AVR_output;

speed_output_LH = 2*angle_cfed;
speed_output_RH = -2*angle_cfed;

PWM_output (speed_output_LH,speed_output_RH);
}

//-------------------------------------------------------
//定时器2中断处理
//*************************************//
static unsigned char temp;
//extern int speed_real_LH,speed_real_RH;
//*************************************//
#pragma interrupt_handler T2INT_fun:4
void T2INT_fun()
{
AD_calculate();

if(temp>=4) //10ms即中断；每秒计算PWM：100/4=25次；
{
PWM_calculate();

USART_output(angle_cfed+128);
USART_output(speed_real_LH+128);
USART_output(speed_real_RH+128);

speed_real_LH=0;
speed_real_RH=0;

temp=0;
}

temp+=1;

}

//*************************************//
float angle, angle_dot; //angle_cfed;
//*************************************//
int speed_need,turn_need;
//*************************************//

//*************************************//
void main()
{
PORT_initial();
T1_initial();
T2_initial();
INT_initial();
USART_initial (18200);

SEI();

while(1)
{
;
}
}

追问

能站起来没有？

追答

没呢，还早着，一点一点学

Answer 2

同学，能不能把你电磁组相关的资料传给我一些啊？我也准备做，谢谢了，1079155616@qq.com

Answer 3

求程序，我们也是电磁组的，才刚刚上手，求资料 jimwcj@163.com