平衡车程序

//
// while(1)
// {
// uart_putchar(UART1,0xff); //UART1，TX接PTC4，RX接PTC3
// uart_putchar(UART1,0xff);//以上两个字节数据作为数据帧头
//
// angle1=(uint16)(g_fGyroscopeAngleIntegral+200); //陀螺仪采样
// uart_putchar(UART1,angle3 & 0xFF);
// }
}
//初始化
void init_all()
{
//电机 脉冲输出 初始化
tpm_pwm_init(TPM0,TPM_CH0,20000,0); //E24 //左
// }
int i;
while(1)
{
for(i=0; i<128; i++)
{
if( LINE[i]==0xFF)
{
LINE[i] = 0xFE; //遇到FF用FE替换即可
adc_init (ADC0_DP0); //PTE20 线性CCD
gpio_init(PTC8,GPO,0); //初始化SI //C8
gpio_init(PTC7,GPO,0); //初始化CLK //C7
gpio_turn(PTE0); //点亮LED
DELAY_MS(1000);
EnableInterrupts;
DELAY_MS(3000);
SpeedControlFlag=1;
// while(1)
// {
//;
int g_n1MsEventCount=0,g_fGravityAngle=0;
float g_fCarAngle=0,g_fCarSpeed=0;
float g_fGyroscopeAngleSpeed=0,g_fGyroscopeAngleIntegral=,g_fAngleControlOut=0,g_fSpeedControlIntegral=0;
#define GRAVITY_ANGLE_RATIO 180/1960 //加速度计角度转换系数 1945
#define CAR_GYRO_RATIO_INT 1.5 //可调在0.3左右，陀螺仪AD值与角速度的比例系数
#define GRAVITY_ADJUST_TIME_CONSTANT 1 //修正系数
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
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采集
#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
#define ANGLE_CONTROL_P 12//20 //可调，P值
#define CAR_ANGLE_SET 0 //平衡 //小车角度设置
#define ANGLE_CONTROL_D 0.3//0.2 //可调，D值 0.1
// uart_putchar(UART1, (angle1>> 8 ) & 0xFF);
// uart_putchar(UART1,angle1 & 0xFF);
//
// angle2=(uint16)(g_fGravityAngle+200); //加速度计采样数据
void init_all();
float speed_count=0;
void Center_Calculate();
/***************角度控制参数值****************/
#define CV_ACCE_OFFSET 1750//1775//1788//1845//1870// 1830 //1850 //加速度计零偏值
// uart_putchar(UART1, (angle2>> 8 ) & 0xFF);
// uart_putchar(UART1,angle2 & 0xFF);
//
// angle3=(uint16)(g_fCarAngle+200); //融合数据
// uart_putchar(UART1, (angle3>> 8 ) & 0xFF);
float g_fGzroscopeAngleSpeed; //转向陀螺仪差值
uint8 LINE[128]={0}; //ccd采集一次得到的数组
float fDeltaValue;
//主函数
void main(void)
{
DisableInterrupts;
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 //右
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
//
adc_init(ADC0_SE9); //PTB1 陀螺仪 前后角速度
adc_init(ADC0_SE14); //PTB2 加速度计
adc_init(ADC0_SE13); //PTB3 陀螺仪 左右角速度
#include "common.h"
#include "include.h"
void AngleCalculate();
void AngleControl();
void SpeedControl();
void SpeedControlOutput();
void DirectionControl();
tpm_pulse_init(TPM1,TPM_CLKIN1,TPM_PS_1); //计数 //左 //PTE30
uart_init (UART1, 19200); //PTC3（发送） C4（接收）
#define SPEED_CONTROL_P 150 //20 //50
#define SPEED_CONTROL_I 1 //0.1 //0.01
#define SPEED_INTEGRAL_MAX 100
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黑白线阈值差
#define GYROSCOPE_ANGLE_SIGMA_FREQUENCY 0.005 //5ms
/***************速度控制参数值****************/
#define CAR_SPEED_SET 0.5//0.5 //1.5 速度设置
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;
void DirectionControlOutput();
void MotorOutputr();
void MotorOutputl();
void pit_hander();
void delay();
void StartCCD();
void ccd_get();
uint8 Normalized_U8(uint16);