平衡车程序

#include "common.h"
#include "include.h"

void AngleCalculate();
void AngleControl();
void SpeedControl();
void SpeedControlOutput();
void DirectionControl();
void DirectionControlOutput();
void MotorOutputr();
void MotorOutputl();
void pit_hander();
void delay();
void StartCCD();
void ccd_get();
uint8 Normalized_U8(uint16);
void init_all();
float speed_count=0;
void Center_Calculate();
/***************角度控制参数值****************/

#define CV_ACCE_OFFSET 1750//1775//1788//1845//1870// 1830 //1850 //加速度计零偏值
#define GRAVITY_ANGLE_RATIO 180/1960 //加速度计角度转换系数 1945
#define CAR_GYRO_RATIO_INT 1.5 //可调在0.3左右，陀螺仪AD值与角速度的比例系数
#define GRAVITY_ADJUST_TIME_CONSTANT 1 //修正系数
#define ANGLE_CONTROL_P 12//20 //可调，P值
#define CAR_ANGLE_SET 0 //平衡 //小车角度设置
#define ANGLE_CONTROL_D 0.3//0.2 //可调，D值 0.1
#define CAR_ANGLE_SPEED_SET 0 //平衡 //小车角速度设置
#define ANGLE_CONTROL_OUT_MAX 90 //电机最大占空比
#define ANGLE_CONTROL_OUT_MIN 0 //电机最小占空比，死区电压
#define GYROSCOPE_ANGLE_SIGMA_FREQUENCY 0.005 //5ms

/***************速度控制参数值****************/
#define CAR_SPEED_SET 0.5//0.5 //1.5 速度设置
#define SPEED_CONTROL_P 150 //20 //50
#define SPEED_CONTROL_I 1 //0.1 //0.01
#define SPEED_INTEGRAL_MAX 100
#define SPEED_INTEGRAL_MIN 1

/****************方向控制参数值********************/
#define DIRECTION_CONTROL_P 2//27 //25//0.5 小了 3 大
#define DIRECTION_CONTROL_D 0.01//0.009//0.006//0.5 //0.2//0.01//0.05//0.05



uint16 cy_gyro_zero,cx_gyro_zero; //陀螺仪零偏值
int k,l=0,m=0;
int accvalue, gyrovalue;
uint16 Gyro_ad_y,Acc_ad,Gyro_ad_x; //陀螺仪加速度计 //AD采集
int g_n1MsEventCount=0,g_fGravityAngle=0;
float g_fCarAngle=0,g_fCarSpeed=0;
float g_fGyroscopeAngleSpeed=0,g_fGyroscopeAngleIntegral=0,g_fAngleControlOut=0,g_fSpeedControlIntegral=0;
float g_fSpeedControlOutOld=0,g_fSpeedControlOutNew=0;
float g_direction_outold=0,g_direction_outnew=0;
float g_fSpeedControlOut=0,g_fSpeedControl,g_fGyroscopeAngleSpeed;
int g_nSpeedControlPeriod=0;
int g_nSpeedControlCount=0,g_nDirectionControlCount=0;
int g_nDirectionControlOut=0,g_nDirectionControlPeriod=0;
int g_ccddelta=45; //40 //CCD黑白线阈值差
int g_icenter; //CCD的中心
int center=61; //目标中心（赛道中心）
int g_width = 30; //128 //赛道宽
int SpeedControlFlag=0; //速度控制标志位
float g_fGzroscopeAngleSpeed; //转向陀螺仪差值
uint8 LINE[128]={0}; //ccd采集一次得到的数组
float fDeltaValue;

//主函数

voi

d main(void)
{
DisableInterrupts;
init_all(); //初始化

uint16 gyro_ad_zero_y=0, gyro_ad_zero_x=0;
DELAY_MS(1000);
//获得陀螺仪X轴和Y轴的零偏值
for(k=0;k<=9;k++)
{
gyro_ad_zero_y += adc_once(ADC0_SE9, ADC_12bit); // PTB1
gyro_ad_zero_x += adc_once(ADC0_SE13, ADC_12bit); // PTB3
}
cy_gyro_zero = gyro_ad_zero_y/10;
cx_gyro_zero = gyro_ad_zero_x/10;

gpio_turn(PTE0); //点亮LED

DELAY_MS(1000);
EnableInterrupts;
DELAY_MS(3000);
SpeedControlFlag=1;
// while(1)
// {
//;
// }
int i;
while(1)
{

for(i=0; i<128; i++)
{

if( LINE[i]==0xFF)
{
LINE[i] = 0xFE; //遇到FF用FE替换即可
}
uart_putchar(UART1,LINE[i]);
}
uart_putchar(UART1,0xFF);
DELAY_MS(10);
}
////麦哥上位机调试
// uint16 angle1,angle2,angle3;
//
// while(1)
// {
// uart_putchar(UART1,0xff); //UART1，TX接PTC4，RX接PTC3
// uart_putchar(UART1,0xff);//以上两个字节数据作为数据帧头
//
// angle1=(uint16)(g_fGyroscopeAngleIntegral+200); //陀螺仪采样
// uart_putchar(UART1, (angle1>> 8 ) & 0xFF);
// uart_putchar(UART1,angle1 & 0xFF);
//
// angle2=(uint16)(g_fGravityAngle+200); //加速度计采样数据
// uart_putchar(UART1, (angle2>> 8 ) & 0xFF);
// uart_putchar(UART1,angle2 & 0xFF);
//
// angle3=(uint16)(g_fCarAngle+200); //融合数据
// uart_putchar(UART1, (angle3>> 8 ) & 0xFF);
// uart_putchar(UART1,angle3 & 0xFF);
// }

}

//初始化

void init_all()
{

//电机 脉冲输出 初始化
tpm_pwm_init(TPM0,TPM_CH0,20000,0); //E24 //左
tpm_pwm_init(TPM0,TPM_CH1,20000,0); //E25 //左
tpm_pwm_init(TPM0,TPM_CH2,20000,0); //E29 //右
tpm_pwm_init(TPM0,TPM_CH4,20000,0); //E31 //右

//
adc_init(ADC0_SE9); //PTB1 陀螺仪 前后角速度
adc_init(ADC0_SE14); //PTB2 加速度计
adc_init(ADC0_SE13); //PTB3 陀螺仪 左右角速度
adc_init (ADC0_DP0); //PTE20 线性CCD


gpio_init(PTC8,GPO,0); //初始化SI //C8
gpio_init(PTC7,GPO,0); //初始化CLK //C7

gpio_init(PTB19,GPI,0); //编码器测向 右
gpio_init(PTC1,GPI,0); //编码器测向 左

gpio_init(PTE0,GPO,1); //初始化LED

tpm_pulse_init(TPM2,TPM_CLKIN0,TPM_PS_1); //计数 //右 //PTB16
tpm_pulse_init(TPM1,TPM_CLKIN1,TPM_PS_1); //计数 //左 //PTE30

uart_init (UART1, 19200); //PTC3（发送） C4（接收）

StartCCD();

pit_init_ms(PIT1, 5); //定时 5ms 后中断

set_vector_handler(PIT_VECTORn,pit_hander); //设置中断复位函数到中断向量

表里
enable_irq(PIT_IRQn); //使能LPTMR中断
}

//PIT中断
uint16 gyro_ad_y, acc_ad;
//int i,ii,iii;
void pit_hander(void)
{
PIT_Flag_Clear(PIT1); //清中断标志位

Gyro_ad_y = adc_once(ADC0_SE9, ADC_12bit); // PTB1
Acc_ad = adc_once(ADC0_SE14, ADC_12bit); // PTC0

// for(i=0;i<=19;i++)
// { ii+=adc_once(ADC0_SE14, ADC_12bit);}
// iii=ii/20;
// ii=0;
//
//方向控制
AngleCalculate();
AngleControl();

//速度控制 100ms一次
g_nSpeedControlCount++;
g_nSpeedControlPeriod++;
if(g_nSpeedControlCount==20)
{
g_nSpeedControlCount=0;
g_nSpeedControlPeriod=0;
if(SpeedControlFlag==1)
SpeedControl();
}
SpeedControlOutput();


//方向控制
g_nDirectionControlPeriod++;
g_nDirectionControlCount++;
if(g_nDirectionControlCount==4)
{
ccd_get();
DirectionControl();
g_nDirectionControlCount=0;
g_nDirectionControlPeriod=0;
}
DirectionControlOutput();

//电机输出
MotorOutputl();
MotorOutputr();


//
// //重新采集 陀螺仪X轴和Y轴零偏值
// m++;
// if(m==2000) //每隔0.005s*2000=10s采集一次
// {
// m=0;
// int n,gyro_ad_zero_y,gyro_ad_zero_x;
// for(n=0;n<20;n++) //取20次的平均值
// {
// gyro_ad_zero_y += adc_once(ADC0_SE9, ADC_12bit); // PTB1
// gyro_ad_zero_x += adc_once(ADC0_SE13, ADC_12bit); // PTB3
// }
// cy_gyro_zero = gyro_ad_zero_y/20;
// cx_gyro_zero = gyro_ad_zero_x/20;
// gyro_ad_zero_y=0;
// gyro_ad_zero_x=0;
// }

}

//角度
void AngleCalculate()
{
accvalue =(int)(CV_ACCE_OFFSET-Acc_ad);
g_fGravityAngle = (accvalue * GRAVITY_ANGLE_RATIO);
gyrovalue = (int)(-cy_gyro_zero+Gyro_ad_y);
g_fGyroscopeAngleSpeed = (float)(gyrovalue* CAR_GYRO_RATIO_INT);
g_fCarAngle = g_fGyroscopeAngleIntegral;
fDeltaValue = (g_fGravityAngle-g_fCarAngle)/GRAVITY_ADJUST_TIME_CONSTANT;
g_fGyroscopeAngleIntegral+=(float)((g_fGyroscopeAngleSpeed +fDeltaValue)*GYROSCOPE_ANGLE_SIGMA_FREQUENCY);
}
void AngleControl()
{
g_fAngleControlOut = ANGLE_CONTROL_P *(CAR_ANGLE_SET-g_fCarAngle) + ANGLE_CONTROL_D*(CAR_ANGLE_SPEED_SET - g_fGyroscopeAngleSpeed);
}

float count_left,count_right;
float speed_left, speed_right,CarSpeed;
uint8 direction_right,direction_left;

//速度
float fDelta,fP,fI;
void SpeedControl()
{


//脉冲计数
count_left=tpm_pulse_get(TPM1); //左
count_right=tpm_pulse_get(TPM2); //右
direction_right=gpio_get(PTB19); //右 可能要换引脚
direc

tion_left=gpio_get(PTC1); //左

//速度计算
speed_left = count_left/1160*3.1415*0.048*10; //左
speed_right = count_right/1160*3.1415*0.048*10;//右
if(direction_left==1) speed_left=-speed_left;
if(direction_right==0) speed_right=-speed_right;
g_fCarSpeed=(speed_left+speed_right)/2; //小车速度

//计数清零
tpm_pulse_clean(TPM1);
tpm_pulse_clean(TPM2);

//车轮转动方向


fDelta = -(CAR_SPEED_SET- g_fCarSpeed);
fP = fDelta * SPEED_CONTROL_P;
fI = fDelta * SPEED_CONTROL_I;

g_fSpeedControlIntegral += fI;//I的作用消除稳定误差

//
// if(g_fSpeedControlIntegral > SPEED_INTEGRAL_MAX)
// g_fSpeedControlIntegral = SPEED_INTEGRAL_MAX;
// if(g_fSpeedControlIntegral < SPEED_INTEGRAL_MIN)
// g_fSpeedControlIntegral = SPEED_INTEGRAL_MIN;

g_fSpeedControlOutOld = g_fSpeedControlOutNew;
g_fSpeedControlOutNew = fP + g_fSpeedControlIntegral;

speed_count++;
if(speed_count>=10) speed_count=10;
g_fSpeedControlOutNew = g_fSpeedControlOutNew*speed_count/10;
}
void SpeedControlOutput()
{
float fValue1;
fValue1 = g_fSpeedControlOutNew - g_fSpeedControlOutOld;
g_fSpeedControl = fValue1 * (g_nSpeedControlPeriod + 1)*0.05 + g_fSpeedControlOutOld;
g_fSpeedControlOut= (int)(g_fSpeedControl);
}

//方向
int delta;
void DirectionControl(void)
{
int delta; //离赛道中心的偏差

//ccd数据处理
Center_Calculate();
// int i,leftbreak,rightbreak,leftbreak_num,rightbreak_num;
// for(i=1;i<63;i++)
// {
// if(ABS(LINE[i-1]-LINE[i]) >=30)
// {
// leftbreak = i;
// break;
// }
// // leftbreak_num++;
// }
//
// for(i=126;i>64;i--)
// {
// if(ABS(LINE[i+1]-LINE[i]) >=30) {
// rightbreak = i;
// break;
// }
// // rightbreak_num++;
// }
// g_icenter=(leftbreak+rightbreak)/2;


delta=(int)(g_icenter-center); //求出黑线中心点与赛道中心点差值
Gyro_ad_x = adc_once(ADC0_SE13,ADC_12bit); // ADC0_SE11, PTC2
g_fGzroscopeAngleSpeed=Gyro_ad_x - cx_gyro_zero;
g_direction_outnew=delta*DIRECTION_CONTROL_P-g_fGzroscopeAngleSpeed*DIRECTION_CONTROL_D;
g_direction_outold=g_direction_outnew;
}
void DirectionControlOutput(void)
{
int nValue;
nValue = (int)(g_direction_outnew - g_direction_outold);
g_nDirectionControlOut =(int) (nValue * (g_nDirectionControlPeriod + 1)/4+ g_direction_outold); //？？？？

}

//电机输出
float f_right;
void MotorOutputr()
{
// float f_right;
f_right= g_fAngleControlOut+ g_fSpeedControlOut + g_nDi

rectionControlOut; //
// f_right= g_fAngleControlOut;
// f_right= g_fSpeedControlOut ;
// f_right= g_fAngleControlOut+ g_fSpeedControlOut;
// f_right= g_nDirectionControlOut;
// f_right= g_fAngleControlOut+ g_nDirectionControlOut;
if(f_right <0) //正转
{
f_right = -f_right;

if(f_right>ANGLE_CONTROL_OUT_MAX) f_right=ANGLE_CONTROL_OUT_MAX;
if(f_right
tpm_pwm_duty(TPM0, TPM_CH0,(int)f_right);
tpm_pwm_duty(TPM0, TPM_CH1,0);
}
else //反转
{
if(f_right>ANGLE_CONTROL_OUT_MAX) f_right=ANGLE_CONTROL_OUT_MAX;
if(f_right
tpm_pwm_duty(TPM0, TPM_CH1,(int)f_right);
tpm_pwm_duty(TPM0, TPM_CH0,0);
}
}
float f_left;
void MotorOutputl()
{
//float f_left;
f_left=g_fAngleControlOut+ g_fSpeedControlOut- g_nDirectionControlOut;
// f_left=g_fAngleControlOut;
// f_left= g_fSpeedControlOut;
// f_left=g_fAngleControlOut+ g_fSpeedControlOut;
// f_left= g_nDirectionControlOut;
// f_left=g_fAngleControlOut -g_nDirectionControlOut;
if(f_left <0) //正转
{
f_left = -f_left;

if(f_left>ANGLE_CONTROL_OUT_MAX) f_left=ANGLE_CONTROL_OUT_MAX;
if(f_left
tpm_pwm_duty(TPM0, TPM_CH2,(int)f_left);
tpm_pwm_duty(TPM0, TPM_CH4, 0);
}
else //反转
{

if(f_left>ANGLE_CONTROL_OUT_MAX) f_left=ANGLE_CONTROL_OUT_MAX;
if(f_left
tpm_pwm_duty(TPM0, TPM_CH4,(int)f_left);
tpm_pwm_duty(TPM0, TPM_CH2, 0);
}
}

//ccd时序 采集
void ccd_get()
{
int i=0;
delay();
gpio_set(PTC8,1);
delay();
gpio_set(PTC7,1);
delay();
gpio_turn(PTC8);//发送第1个CLK
delay();
LINE[0] = Normalized_U8(adc_once(ADC0_DP0, ADC_12bit));
gpio_turn(PTC7);

//发送第2~129个CLK
for(i=1; i<128; i++)
{
delay();
gpio_turn(PTC7);
delay();
LINE[i] = Normalized_U8(adc_once(ADC0_DP0, ADC_12bit));
gpio_turn(PTC7);
}

//发送第129个clk
delay();
gpio_turn(PTC7);
delay();
gpio_turn(PTC7);
}

uint8 Normalized_U8(uint16 data)
{
return (uint8)((uint32)data*255/4096);
}

//延时
void delay()
{
int i=0;
for(i=0;i<10;i++)
{
asm("nop");
}
}

vo

id StartCCD()
{
int i=0;

delay();
gpio_set(PTC8,1);
delay();
gpio_set(PTC7,1);
delay();
gpio_turn(PTC8);//发送第1个CLK
delay();
gpio_turn(PTC7); //发送第2~129个CLK
for(i=1; i<129; i++)
{
delay();
gpio_turn(PTC7);
delay();
gpio_turn(PTC7);
}
}


void Center_Calculate(void)
{
int i=0,left=0,right=0,left_1=0,right_1=0;
int High,Low,threshold;
// int left_1_flag=1,right_1_flag=1;

int left_flag=1;
int right_flag=1;
High=LINE[0];
Low=LINE[0];

for(i=0;i<120;i++)
{
if(High>=LINE[i])High=LINE[i];
if(Low<=LINE[i]) Low=LINE[i];
}

threshold=(High+Low)/2;
for(i=3;i<118;i++)
{
if(LINE[i]>=(threshold)) //左边黑线检测
{
left=i;
// DEBUG1 =left;
left_flag=0;
break;
}

}


for(i=118;i>=3;i--)
{
if(LINE[i]>=(threshold)) //右侧黑线检测
{
right=i;
//DEBUG2=right;
right_flag=0;
break;
}

}

if(left_flag==0&&right_flag==0) //左右黑线均检测到
{
for(i=60;i>=0;i--)
{
if(LINE[i]<=threshold) //左侧黑线检测
{
left_1=i;
//DEBUG2=right;

break;
}
}



for(i=61;i<120;i++)
{
if(LINE[i]<=threshold) //右侧黑线检测
{
right_1=i;
//DEBUG2=right;

break;
}
}

g_icenter=(int)((left+right)/2);
g_width=right_1-left_1;
}


if(right_flag==1&&left_flag==0){
g_icenter=(int)(left+g_width/2); //右边界线丢失
}if(left_flag==1&&right_flag==0){
g_icenter=(int)(right-g_width/2); //左边界线丢失
}



if(right_flag==1&&left_flag==1){
;
}


// if(left_flag==0)
// {
// g_icenter=(int)(left_1-g_width/2);
// }
//
//
// if(right_flag==0)
// {
// g_icenter=(int)(right_1+g_width/2);
// }


}







///*****************CCD信号控制**********************/
//#define SI_SetVal() PTC8_OUT = 1
//#define SI_ClrVal() PTC8_OUT = 0
//#define CLK_ClrVal() PTC7_OUT = 0
//#define CLK_SetVal() PTC7_OUT = 1



////麦哥上位机调试
// uint16 angle1,angle2,angle3;
//
// while(1)
// {
// uart_putchar(UART1,0xff); //UART1，TX接PTC4，RX接PTC3
// uart_putchar(UART1,0xff);//以上两个字节数据作为数据帧头
//
// angle1=(uint16)(g_fGyroscopeAngleIntegral+200); //陀螺仪采样
// uart_putchar(UART1, (angle1>> 8 ) & 0xFF);
// uart_putchar(UART1,angle1 & 0xFF);
//
// angle2=(uint16)(g_fGravityAngle+200); //加速度计采样数据

// uart_putchar(UART1, (angle2>> 8 ) & 0xFF);
// uart_putchar(UART1,angle2 & 0xFF);
//
// angle3=(uint16)(g_fCarAngle+200); //融合数据
// uart_putchar(UART1, (angle3>> 8 ) & 0xFF);
// uart_putchar(UART1,angle3 & 0xFF);
// }



// void SpeedControl(void)
//{
//float fDelta=0;
//float fP=0;
//float fI=0;
////int SPEED_COUNT=0;
//
//
// PUSE_ad();
// g_fCarSpeed = (float)(pulse_left + pulse_right) / 2.0;
//
// g_fCarSpeed *= CAR_SPEED_CONSTANT;
//
//fDelta =(float)(CAR_SPEED_SET - g_fCarSpeed);
//
// if(CODE_READ1==0x00)
// {
// fDelta =-fDelta;
// }
//
//
// fP = fDelta * SPEED_CONTROL_P;
// fI = fDelta * SPEED_CONTROL_I;
//
// g_fSpeedControlIntegral += fI;
//
//
// g_fSpeedControlOutOld = g_fSpeedControlOutNew;
// g_fSpeedControlOutNew = fP + g_fSpeedControlIntegral;
//
//
//}
// void SpeedControlOutput(void)
//{
//
//
//float fValue=0;
//
//SpeedControl();
//fValue = g_fSpeedControlOutNew - g_fSpeedControlOutOld;
//g_nSpeedControlOut = (int)(fValue * (g_nSpeedControlPeriod + 1) / SPEED_CONTROL_PERIOD + g_fSpeedControlOutOld);//
//
//
//}


//编码器 不用

// FTM2_QUAD_Init();
// FTM1_QUAD_Init();
//void FTM2_QUAD_Init(void)
//{
// PORTB_PCR18= PORT_PCR_MUX(6); // 设置引脚A10引脚为FTM2_PHA功能
// PORTB_PCR19= PORT_PCR_MUX(6); // 设置引脚A11引脚为FTM2_PHB功能
// SIM_SCGC3|=SIM_SCGC3_FTM2_MASK;//使能FTM2时钟
// FTM2_MODE |= FTM_MODE_WPDIS_MASK;//写保护禁止
// FTM2_QDCTRL|=FTM_QDCTRL_QUADMODE_MASK;//AB相同时确定方向和计数值
// FTM2_CNTIN=0;//FTM0计数器初始值为0
// FTM2_MOD=65535;//结束值
// FTM2_QDCTRL|=FTM_QDCTRL_QUADEN_MASK;//启用FTM2正交解码模式
// FTM2_MODE |= FTM_MODE_FTMEN_MASK;//FTM2EN=1
// FTM2_CNT=0;
//}
//void FTM1_QUAD_Init(void)
//{
// PORTA_PCR12= PORT_PCR_MUX(7); // 设置引脚A12引脚为FTM1_PHA功能
// PORTA_PCR13= PORT_PCR_MUX(7); // 设置引脚A13引脚为FTM1_PHB功能 !!!!!!!!!!!!!!!!!!!管脚重复
// SIM_SCGC6|=SIM_SCGC6_FTM1_MASK;//使能FTM1时钟
// FTM1_MODE |= FTM_MODE_WPDIS_MASK;//写保护禁止
// FTM1_QDCTRL|=FTM_QDCTRL_QUADMODE_MASK;//AB相同时确定方向和计数值
// FTM1_CNTIN=0;//FTM0计数器初始值为0
// FTM1_MOD=65535;//结束值
// FTM1_QDCTRL|=FTM_QDCTRL_QUADEN_MASK;//启用FTM1正交解码模式
// FTM1_MODE |= FTM_MODE_FTMEN_MASK;//FTM1EN=1
// FTM1_CNT=0;
//}