📄 program.c
字号:
源程序：main.c
/*
**==============================================================================
** SMARTCAR LAB OF USTB
**
** FILE NAME : main.c
** Description: the main function of this project
** Author : ChenXi
** DATE : 2006-07-29
** MODIFY DATE: ****-**-**
** REMARK : **********
** version: 1.0
**==============================================================================
*/
#include <hidef.h> /* common defines and macros */
#include <mc9s12dj128.h> /* derivative information */
//#include <math.h>
#include "7279.h"
#pragma LINK_INFO DERIVATIVE "mc9s12dj128b"
/*===============================================================
程序中使用到的宏定义
===============================================================*/
#define Dir_Go PTM_PTM3 = 1 //电机正转
#define Dir_Back PTM_PTM3 = 0 //电机反转
#define LeftBrake PWMDTY23 = 130
#define RightBrake PWMDTY23 = 175
#define Start PWMDTY23 = 154
#define Middle 300 //AD二值化处理的门限值
#define PWM_TOTAL 26 //正中时的PWM与最偏是的PWM值的差
#define PWM_Middle 147 //车正中时的PWMDTY67的植
/**传感器位置定义*
//B口3个传感器
#define MiddleState PORTB & 0x02 // 0000 0*0 0000
#define MiddleLeft PORTB & 0x06 // 0000 **0 0000
#define MiddleRight PORTB & 0x03 // 0000 0** 0000
#define LittleLeft PORTB & 0x04 // 0000 *00 0000
#define LittleRight PORTB & 0x01 // 0000 00* 0000
//B口和AD
#define ADLeft01 (PORTB & 0x40) && (ucAD_Port & 0x10) // 000* *00 0000
#define ADRight01 (PORTB & 0x01) && (ucAD_Port & 0x08) // 0000 00* *000
//AD转换结果(左四个，高四位）
#define ADLeft1 ucAD_Port & 0x10 // 000* 000 0000
#define ADLeft12 ucAD_Port & 0x30 // 00** 000 0000
#define ADLeft2 ucAD_Port & 0x20 // 00*0 000 0000
#define ADLeft23 ucAD_Port & 0x60 // 0**0 000 0000
#define ADLeft3 ucAD_Port & 0x40 // 0*00 000 0000
#define ADLeft34 ucAD_Port & 0xC0 // **00 000 0000
#define ADLeft4 ucAD_Port & 0x80 // *000 000 0000
//AD转换结果(左四个，高四位）
#define ADRight1 ucAD_Port & 0x08 // 0000 000 *000
#define ADRight12 ucAD_Port & 0x0C // 0000 000 **00
#define ADRight2 ucAD_Port & 0x04 // 0000 000 0*00
#define ADRight23 ucAD_Port & 0x06 // 0000 000 0**0
#define ADRight3 ucAD_Port & 0x02 // 0000 000 00*0
#define ADRight34 ucAD_Port & 0x03 // 0000 000 00**
#define ADRight4 ucAD_Port & 0x01 // 0000 000 000*
*/
/*===============================================================
程序中使用到的变量定义
===============================================================*/
static uchar StateB; //B口最低3位输入
static uchar ucSensorState; //AD采集的传感器状态
static uint uiAD_result[8]; //AD转换结果保存数组
static uint uiPulse;
static uchar ucDepartureDegree;
static uchar ucAD_Port; //AD口返回值
static uint uiTurnAngle; //转向舵机转角
static signed char scDeparture; //有可能是小数,包括正负
static signed char scLastDeparture;//纪录上一次偏离程度,有可能是小数,包括正负
static uchar sum; //传感器在黑线上的总个数
static signed char temp; //传感器的总权重
static uint uiMaxSpeed; //速度最大值
static uint uiMinSpeed; //速度最小值
static uchar BrakeFlag;
static uchar OutFlag; //传感器在黑线上的标志,最后三位xxxxx LeftFlag MiddleFlag RightFlag
static uchar LastOutFlag; //记录上一个变量
static uchar tempPWMDTY67;
static uchar ABC;
/*==============================================================
子函数定义
==============================================================*/
//////////////////////////////////////////////////////////////////////////
///////////////////////////////PID 部分///////////////////////////////////
//////////////////////////////////////////////////////////////////////////
//定义PID参数
#define VV_KPVALUE 3 //比例
#define VV_KIVALUE 40 //积分
#define VV_KDVALUE 3 //微分
#define VV_MAX 10000 //返回的最大值,是pwm的周期值
#define VV_MIN 0
#define VV_DEADLINE 0X08 //速度PID，设置死区范围
typedef struct PID //定义数法核心数据
{
signed int vi_Ref; //速度PID，速度设定值
signed int vi_FeedBack; //速度PID，速度反馈值
signed long vi_PreError; //速度PID，前一次，速度误差,,vi_Ref - vi_FeedBack
signed long vi_PreDerror; //速度PID，前一次，速度误差之差，d_error-PreDerror;
unsigned int v_Kp; //速度PID，Ka = Kp
unsigned int v_Ki; //速度PID，Kb = Kp * ( T / Ti )
unsigned int v_Kd; //速度PID，
signed long vl_PreU; //电机控制输出值
}PID;
PID sPID; // PID Control Structure
void PIDInit(void)
{
sPID.vi_Ref = 0 ; //速度设定值
sPID.vi_FeedBack = 0 ; //速度反馈值
sPID.vi_PreError = 0 ; //前一次，速度误差,,vi_Ref - vi_FeedBack
sPID.vi_PreDerror = 0 ; //前一次，速度误差之差，d_error-PreDerror;
sPID.v_Kp = VV_KPVALUE;
sPID.v_Ki = VV_KIVALUE;
sPID.v_Kd = VV_KDVALUE;
sPID.vl_PreU = 0 ; //电机控制输出值
}
unsigned int v_PIDCalc( PID *pp )
{
signed long error,d_error,dd_error;
error = (signed long)(pp->vi_Ref - pp->vi_FeedBack); // 偏差计算
d_error = error - pp->vi_PreError;
dd_error = d_error - pp->vi_PreDerror;
pp->vi_PreError = error; //存储当前偏差
pp->vi_PreDerror = d_error;
if( ( error < VV_DEADLINE ) && ( error > -VV_DEADLINE ) ); //设置调节死区
else //速度PID计算
pp->vl_PreU += (signed long)( pp -> v_Kp * d_error + pp -> v_Ki * error + pp->v_Kd*dd_error);
if( pp->vl_PreU >= VV_MAX ) //速度PID，防止调节最高溢出
pp->vl_PreU = VV_MAX;
else if( pp->vl_PreU <= VV_MIN ) //速度PID，防止调节最低溢出
pp->vl_PreU = VV_MIN;
return ( pp->vl_PreU ); // 返回预调节占空比
}
//////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////
/*-------------------------------------------------------------
AD初始化函数
--------------------------------------------------------------*/
void AD_Init(void) //AD初始化
{
//AD结果数组初始化
uiAD_result[0] = 0;
uiAD_result[1] = 0;
uiAD_result[2] = 0;
uiAD_result[3] = 0;
uiAD_result[4] = 0;
uiAD_result[5] = 0;
uiAD_result[6] = 0;
uiAD_result[7] = 0;
//控制寄存器2：上电，标志位快速清零,查询方式不使用中断
ATD0CTL2 = (ATD0CTL2_ADPU_MASK|ATD0CTL2_AFFC_MASK);
//控制寄存器3：转换序列长度为3，FIFO模式
ATD0CTL3 = (ATD0CTL3_S8C_MASK/*ATD0CTL3_S2C_MASK|ATD0CTL3_S1C_MASK|ATD0CTL3_FIFO_MASK*/);
//控制寄存器4：10位AD，PRS=[0b00110]=6， AD clock frequency 24/6/2=2MHz
ATD0CTL4 = (/*ATD0CTL4_SRES8_MASK|*/ATD0CTL4_PRS2_MASK|ATD0CTL4_PRS1_MASK);
//控制寄存器5：右对齐方式,连续转换模式,多通道采样模式
ATD0CTL5 = (ATD0CTL5_DJM_MASK|ATD0CTL5_SCAN_MASK|ATD0CTL5_MULT_MASK);
ATD0DIEN=0x00; // 禁止数字输入缓冲
}
/*--------------------------------------------------------------
AD转换函数，查询方式
--------------------------------------------------------------*/
void AD_Convert(void)
{
if(ATD0CTL2_ASCIE == 0) //采用查询方式
{
while(!ATD0STAT0_SCF); //等待各通道转换结束
uiAD_result[0] = ATD0DR0; //读通道0的转换结果
uiAD_result[1] = ATD0DR1; //读通道1的转换结果
uiAD_result[2] = ATD0DR2; //读通道2的转换结果
uiAD_result[3] = ATD0DR3; //读通道3的转换结果
uiAD_result[4] = ATD0DR4; //读通道4的转换结果
uiAD_result[5] = ATD0DR5; //读通道5的转换结果
uiAD_result[6] = ATD0DR6; //读通道6的转换结果
uiAD_result[7] = ATD0DR7; //读通道7的转换结果
}
}
uchar Sensor_State (void)
{
uchar i;
AD_Convert();
for(i=0;i<8;i++)
{
if((*(uiAD_result+i))>=Middle)
{
ucSensorState|=(1<<i);
}
}
return ucSensorState;
}
/*-------------------------------------------------------------
I/O口初始化
--------------------------------------------------------------*/
void Port_Init(void)
{
DDRA=0x00; //传感器输入
DDRB=0x00; //传感器输入,PB7控制继电器换向
DDRP=0xFF; //pwm输出口方向初始化
DDRT=0x00; //脉冲捕捉端口初始化
DDRM=0xFF; //PORTM口初始化,使用PM3即spi0_ss控制电机正反转或尽兴33886使能
//PTM_PTM3 = 0; //使用33886初始化
//PTM_PTM5 = 1;
Dir_Go; //电机正转
}
/*--------------------------------------------------------------
计数器初始化
---------------------------------------------------------------*/
void Pulse_Count_Init()
{
MCFLG_MCZF = 1;
MCCTL_MODMC = 1;
MCCTL_MCZI = 1;
MCCTL_MCEN = 1;
MCCNT = 30000; //20ms中断一次
MCCTL_MCPR = 3; //16分频
MCCTL_FLMC = 1;
TIOS_IOS0 = 0; //PTT0作为码盘脉冲输入
TCTL4 = 0x01; //只在上升沿捕获脉冲
PBCTL_PBEN = 1;
}
/*------------------------------------------------------------*
16 bit pwm ,to control 转向舵机
周期20ms
--------------------------------------------------------------*/
void PWM67_Init(void)
{
PWMCTL_CON67 = 1; //use 16 bit pwm channel 01
PWME_PWME7 = 0; //PWME1 control pwm01,disable pwm
PWMPRCLK_PCKB = 2; //4 devide ,48MHz
PWMSCLB = 30; //48MHz/4/2/30 ~= 200KHz
PWMCLK_PCLK7 = 1;
PWMPOL_PPOL7 = 1;
PWMCNT67 = 0;
PWMDTY67 = 147; //1.5ms
PWMPER67 = 1000; //200KHz/1000/2 ~= 100Hz period is 10ms
PWME_PWME7 = 1;
}
/***********************************************
16 bit pwm ,to control DC motor
频率600Hz
************************************************/
void PWM45_Init(void)
{
PWMCTL_CON45 = 1; //use 16 bit pwm channel 45
PWME_PWME5 = 0;
PWMPRCLK_PCKA = 2; //4 devide //48MHz/4/= 12MHz
// PWMSCLA = 1;
PWMCLK_PCLK5 = 0;
PWMPOL_PPOL5 = 0; //由于MOSFET驱动电路,未加反向器，所以初始应为低电平
PWMCNT45 = 0;
PWMDTY45 = 0;
PWMPER45 = 10000; //12MHz/10000/2 ~= 600Hz
PWME_PWME5 = 1;
}
void PWM23_Init(void)
{
PWMCTL_CON23 = 1; //use 16 bit pwm channel 01
PWME_PWME3 = 0; //PWME1 control pwm01,disable pwm
PWMPRCLK_PCKB = 2; //4 devide ,48MHz
PWMSCLB = 30; //48MHz/4/2/30 ~= 200KHz
PWMCLK_PCLK3 = 1;
PWMPOL_PPOL3 = 1;
PWMCNT23 = 0;
PWMDTY23 =153; //1.5ms
PWMPER23 = 1000; //200KHz/1000/2 ~= 100Hz period is 10ms
PWME_PWME3 = 1;
}
uint GetValue(void)
{
uchar i;
ucAD_Port = Sensor_State();
StateB = PORTB&0x07;
for(i=3;i>0;i--)
{
{
if(StateB&(1<<(i-1)))
{
sum++;
temp += (4-(i<<1));
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -