求指教啊？哪位大神帮帮忙啊？

若水如一 · 发表于 2013-5-16 17:05:09

MC56F8346 这一款单片机问什么陀螺仪采集程序下进去后，5110液晶显示屏上的数据不变啊？
帮帮忙检查一下程序，非常感谢！

/* Including needed modules to compile this module/procedure */
#include "Cpu.h"
#include "Events.h"
#include "LCD_SCLK.h"
#include "LCD_SDIN.h"
#include "LCD_DC.h"
#include "LCD_CE.h"
#include "LCD_RET.h"
#include "ADC1.h"
#include "PWMC1.h"
#include "TI1.h"
#include "COUNTER1.h"
#include "COUNTER2.h"
#include "Key1.h"
#include "Key2.h"
#include "Key3.h"
#include "Key4.h"
//#include "Key1.h"
//#include "Key2.h"
//#include "Key3.h"
//#include "Key4.h"
/* Including shared modules, which are used for whole project */
#include "PE_Types.h"
#include "PE_Error.h"
#include "PE_Const.h"
#include "IO_Map.h"

#include <intrinsics_56800e.h>
#define CarSub_GLOBALS       1
#include  "f3.H"
#include  "5110.h"
#include  "ADC.h"
float  g_fCarGyroVal=0,g_fCarAcceVal=0;
float  angle=0,angle_I=0;
unsigned int TLYDZ;
extern DIR_COUNT;
extern g_lnCarMagneticLeftCount,g_lnCarMagneticRightCount;
float zhuanwan;
unsigned int time=0;
int speed;
char flag=0;
char flag1=1;
int anglespeed;
//#define MOTOR_LEFT_SPEED_POSITIVE (g_nLeftMotorOut > 0)
//#define MOTOR_RIGHT_SPEED_POSITIVE (g_nRightMotorOut > 0)
// unsigned int g_nCarSpeedCount=0;

//=========================================================
//--------------------串口通讯------------------------------
//============================================================
/**********************UART_Init寄存器***********************/
void UART_Init (void)
{
// clrSetReg16Bits(INTC_IPR8,0X5140,0XA280);
   setReg(SCI0_SCICR,0X2C);
   setReg(SCI0_SCIBR,391);

}

void uart_putchar ( unsigned char c)
{

  while(!(getReg(SCI0_SCISR)&0x8000)) ;       //keep waiting when not empty
  setReg(SCI0_SCIDR,c);
}

/*void UART_Init (void)
{
//printf("UART_Init\n");

   SCI0CR2=0x2c; //enable Receive Full Interrupt,RX enable,Tx enable

SCI0BDH=0x01; //busclk  8MHz,19200bps,SCI0BDL=0x1a
SCI0BDL=0xa0; //SCI0BDL=busclk/(16*SCI0BDL)
               //busclk 32MHz, 9600bps,SCI0BDL=0xD0
               //115200 bps    SCI0BDL=0x11
}                // 64M 9600 SCI0BDH=0x01; SCI0BDL=0xA0;

void uart_putchar ( unsigned char c)
{

  while(!(SCI0SR1&0x80)) ;       //keep waiting when not empty
  SCI0DRL=c;
}*/

void uart_putstr(char ch[])
{
  unsigned char ptr=0;
  while(ch[ptr]){
   uart_putchar((unsigned char)ch[ptr++]);
  }
}

unsigned char uart_getchar(void)
{
   byte res=0;
while(!(getReg(SCI0_SCISR)&0x80)) ;    //keep waiting when not empty
return (getReg(SCI0_SCIDR));

}

/**********************OutData_init***********************/
float OutData[4] = { 0 };

unsigned short CRC_CHECK(unsigned char *Buf, unsigned char CRC_CNT)
{
unsigned short CRC_Temp;
unsigned char i,j;
CRC_Temp = 0xffff;

for (i=0;i<CRC_CNT; i++){
      CRC_Temp ^= Buf[i];
      for (j=0;j<8;j++) {
         if (CRC_Temp & 0x01)
            CRC_Temp = (CRC_Temp >>1 ) ^ 0xa001;
         else
            CRC_Temp = CRC_Temp >> 1;
      }
}
return(CRC_Temp);
}

void OutPut_Data(void)
{
  int temp[4] = {0};
  unsigned int temp1[4] = {0};
  unsigned char databuf[10] = {0};
  unsigned char i;
  unsigned short CRC16 = 0;
  for(i=0;i<4;i++)
{

temp[i]  = (int)OutData[i];
temp1[i] = (unsigned int)temp[i];

}

  for(i=0;i<4;i++)
  {
databuf[i*2] = (unsigned char)(temp1[i]%256);
databuf[i*2+1] = (unsigned char)(temp1[i]/256);
  }

  CRC16 = CRC_CHECK(databuf,8);
  databuf[8] = CRC16%256;
  databuf[9] = CRC16/256;

  for(i=0;i<10;i++)
uart_putchar(databuf[i]);
}
//------------------------------------------------------
//-----------------电机使能函数-------------------------
//======================================================
void PWM_OUT(void)
{
   setRegBit(PWMA_PMCTL,LDOK);
   setReg16(PWMA_PMOUT,0x8000);

}
//------------------初始化函数--------------------------
//======================================================
void Car_Init()
{
init_5110();
clear_5110();
UART_Init ();

Key1_SetDir(0);

Key2_SetDir(0);
Key3_SetDir(0);
Key4_SetDir(0);
ClearMotorSpeed();                         // Clear the motor speed counter1, 2
g_nLeftMotorSpeed = 0;                   // The value is get at 1ms interrupt.
g_nRightMotorSpeed = 0;                   // The value is get at 1ms interrupt.

g_nLeftSpeedSet = 0;
g_nRightSpeedSet = 0;
g_nMotorSpeedSet = 0;

g_nLeftMotorOutSpeed = 0;
g_nRightMotorOutSpeed = 0;
g_nLeftMotorSpeedCount = 0;
g_nRightMotorSpeedCount = 0;
g_nMotorOutSpeedOld = 0;
g_nMotorOutSpeedNew = 0;
g_nMotorOutSpeedKeep = 0;
g_nMotorLeftRightDiffKeep=0;

g_nCarAcceVal = 0;
g_nCarGyroVal = 0;
g_lnCarAngleSigma = 0;

g_nCarAngleSet = 0;                      // Car angle set

g_nCarMotionCount = 0;
DIR_COUNT=0;

ad1_init();
}
//===================================================================//
//----------------------ad值处理（平均滤波）-------------------------//
//=====================================================================//
void CarVoltageGet(void)
{

int i;
long values1=0;
long values2=0;
long values3=0;
long values4=0;

for(i=0;i<20;i++)
{
ADC_GetValue16(g_nCarVoltage);
values1+=(long)CV_GYRO_VAL;
values2+=(long)CV_ACCE_VAL;
values3+=(long)CV_MAGNETLEFT_VAL;
values4+=(long)CV_MAGNETRIGHT_VAL;
}
CV_GYRO_VAL=values1/20;
CV_ACCE_VAL=values2/20;
CV_MAGNETLEFT_VAL=values3/20;
CV_MAGNETRIGHT_VAL=values4/20;
      g_lnCarMagneticLeftCount +=CV_MAGNETLEFT_VAL;
    g_lnCarMagneticRightCount += CV_MAGNETRIGHT_VAL;

}
//------------------------------------------------------------------------
//-----------------------倾角计算函数-------------------------------------
//========================================================================
void CarAngleCalculate()
{
long lnDeltaValue;

lnDeltaValue = (int)CV_ACCE_VAL;
lnDeltaValue = lnDeltaValue - (int)CV_ACCE_OFFSET;
g_nCarAcceVal = (int)lnDeltaValue;

g_nCarAcceVal =mult_r(g_nCarAcceVal,CV_ACCE_ANGLE_RATIO);//归化到0-1000
g_nCarGyroVal = (int)CV_GYRO_VAL;
g_nCarGyroVal =(int) (g_nCarGyroVal - CV_GYRO_ZERO);

g_nCarGyroVal = mult_r(g_nCarGyroVal,CAR_GYRO_RATIO_INT);////归化到0-1000
g_nCarAngle =(int) (g_lnCarAngleSigma >>10); //.....

lnDeltaValue = g_nCarAcceVal - g_nCarAngle;//加速度互补
lnDeltaValue = lnDeltaValue * CAR_ACCE_RATIO;//乘比例反馈

g_lnCarAngleSigma += (g_nCarGyroVal + lnDeltaValue);

}
//==============================================================================
//------------------------倾角矫正函数------------------------------------------
//------------------------------------------------------------------------------
//#define MAX_ANGLE 50000
void CarAngleAdjust(void) {
int nLeft, nRight;
int nSpeed;
int nP, nD;

nP = g_nCarAngle;

if(nP > MAX_ANGLE || nP < -MAX_ANGLE) {

g_nLeftMotorOut = 0;
g_nRightMotorOut = 0;
MotorSpeedOut();
return;
}

nP=nP<<3;
nP = (int)mult_r(nP,CAR_AA_P_INT);//加大
nD = g_nCarGyroVal >>3;

nD = (int)mult_r(nD,CAR_AA_D_INT);

nSpeed = nD +nP;                         //  ***** (nP)
anglespeed=nSpeed;
//----------------------速度叠加---------------------------//
nLeft = nSpeed+g_nLeftMotorOutSpeed+g_nMotorLeftRightDiffNew ;//
nRight = nSpeed+g_nRightMotorOutSpeed-g_nMotorLeftRightDiffNew ;

//---------------------------------------------------------//
//  OutData[2]=g_nMotorLeftRightDiffNew;
  zhuanwan=g_nMotorLeftRightDiffNew;
g_nLeftMotorOut = nLeft<<6;
g_nRightMotorOut = nRight<<6 ;

if(g_nLeftMotorOut > MOTOR_OUT_MAX) g_nLeftMotorOut = MOTOR_OUT_MAX;
MotorSpeedOut();

}
//-------------------------电机输出函数----------------------------------
//=======================================================================
void MotorSpeedOut(void)
{
long nLeftVal, nRightVal;

//--------------------------------------------------------------------------
nLeftVal = g_nLeftMotorOut;
nRightVal = g_nRightMotorOut;

nLeftVal = nLeftVal <<3;
nRightVal = nRightVal <<3;

if(nLeftVal > 0) nLeftVal += MOTOR_OUT_DEAD_VAL;
else if(nLeftVal < 0) nLeftVal -= MOTOR_OUT_DEAD_VAL;
if(nRightVal > 0) nRightVal += MOTOR_OUT_DEAD_VAL;
else if(nRightVal < 0) nRightVal -= MOTOR_OUT_DEAD_VAL;

if(nLeftVal > MOTOR_OUT_MAX) nLeftVal = MOTOR_OUT_MAX;
if(nLeftVal < MOTOR_OUT_MIN) nLeftVal = MOTOR_OUT_MIN;
if(nRightVal > MOTOR_OUT_MAX) nRightVal = MOTOR_OUT_MAX;
if(nRightVal < MOTOR_OUT_MIN) nRightVal = MOTOR_OUT_MIN;

g_nLeftVal=nLeftVal;
g_nRightVal=nRightVal;

//--------------------------------------------------------------------------
SetMotorVoltage(nLeftVal, nRightVal);

}
//==============================================================//
//----------------------pwm输出---------------------------------//
//==============================================================//

  void SetMotorVoltage(int nLeftVol,int nRightVol)
{


short nPeriod;
nPeriod = (short)getReg(PWMA_PWMCM);

if(nLeftVol > 0)
{

      setReg(PWMA_PWMVAL0, 0);

      nLeftVol = mult(nLeftVol, nPeriod);
   // if(nLeftVol>=6000) nLeftVol=6000;
      setReg(PWMA_PWMVAL1, nLeftVol);

}
else
   {
nLeftVol = -nLeftVol;
      setReg(PWMA_PWMVAL1, 0);
nLeftVol = mult(nLeftVol, nPeriod);
   // if(nLeftVol>=6000) nLeftVol=6000;
      setReg(PWMA_PWMVAL0, nLeftVol);
}
if( nRightVol > 0)
{

      setReg(PWMA_PWMVAL2, 0);

     nRightVol = mult(nRightVol, nPeriod);
   // if( nRightVol>=6000)  nRightVol=6000;
      setReg(PWMA_PWMVAL3,  nRightVol);

}
else
{
   nRightVol = - nRightVol;
      setReg(PWMA_PWMVAL3, 0);
   nRightVol = mult(nRightVol, nPeriod);
   // if( nRightVol>=6000)  nRightVol=6000;
      setReg(PWMA_PWMVAL2,  nRightVol);
}

   PWM_OUT();
}
//---------------------------------------------------------------------
//----------------------脉冲计数清零和读取函数-------------------------
//=====================================================================
void ClearMotorSpeed(void) {
COUNTER1_Reset();
COUNTER2_Reset();
}
void GetMotorSpeed(unsigned int * pnLeft, unsigned int *pnRight) {
COUNTER1_GetNumEvents(pnLeft);
COUNTER2_GetNumEvents(pnRight);
}
//------------------------------------------------------------------------------
//---------------------  PI电机速度调节函数  -----------------------------------
//==============================================================================
#define mult_r(a,b) __mult_r(a,b)
#define MOTOR_OUT_MAX (int)0x7fff
#define MOTOR_OUT_MIN      (int)(-0x7fff)
//#define MOTOR_SPEED_D          0.0
void  MotorSpeedAdjustCal(void) {
int nLeftSpeed, nRightSpeed;
int nDeltaValue, nP, nI,nD;
int nSpeed;
int lastspeed=0,nowspeed=0,error1=0,error2=0,error=0;
#define mult_r(a,b) __mult_r(a,b)

nLeftSpeed = g_nLeftMotorSpeedCount;
nRightSpeed = g_nRightMotorSpeedCount;

nSpeed=(nLeftSpeed+nRightSpeed)/2;

nDeltaValue = -g_nMotorSpeedSet - nSpeed;
   nDeltaValue=nDeltaValue<<3;
    nP = mult_r(nDeltaValue, MOTOR_SPEED_P_INT);

nI = mult_r(nDeltaValue, MOTOR_SPEED_I_INT);

g_nMotorOutSpeedOld = g_nMotorOutSpeedNew;
lastspeed=nowspeed;
nowspeed=nDeltaValue;
error1=error2;
error2=nowspeed-lastspeed;
error=error2-error1;
nD= error*MOTOR_SPEED_D;
g_nMotorOutSpeedKeep-=nI;
//=====================
if(zhuanwan>15||zhuanwan<-15)
g_nMotorOutSpeedKeep=0;
//=====================
g_nMotorOutSpeedNew = (g_nMotorOutSpeedKeep >>3) - nP-nD;

if(g_nMotorOutSpeedKeep > MOTOR_OUT_MAX) g_nMotorOutSpeedKeep = MOTOR_OUT_MAX;
if(g_nMotorOutSpeedKeep < MOTOR_OUT_MIN) g_nMotorOutSpeedKeep = MOTOR_OUT_MIN;

}
void MotorSpeedAdjust(void)
{
MotorSpeedAdjustCal();                   // 4.2us

}
//-----------------------------------------------------------------------
//---------------------速度调节平滑输出函数 -----------------------------
//=======================================================================
void CalculateMotorOutSpeed(void)
{
float nValue;
nValue = g_nMotorOutSpeedNew - g_nMotorOutSpeedOld;
nValue = nValue * (g_nCarMotionCount + 1) /20+ g_nMotorOutSpeedOld;
g_nLeftMotorOutSpeed = g_nRightMotorOutSpeed = (int)nValue;

}
//------------------------------------------------------------------------------
//-------------------- 方向调节函数 -------------------------------------------
//==============================================================================
#define DIR_P             100

void CarMagneticAdjust(void) {

float nP, nI;
float fLeftRightAdd,fLeftRightSub;
float fValue,fDValue;
float lastdiff=0,nowdiff=0,errordiff1=0,errordiff2=0,errordiff=0;

fLeftRightAdd =(float)(g_nCarMagneticRightAverage + g_nCarMagneticLeftAverage);
fLeftRightSub =(float)(g_nCarMagneticRightAverage - g_nCarMagneticLeftAverage) ;

g_nMotorLeftRightDiffOld = g_nMotorLeftRightDiffNew;

      lastdiff=nowdiff;
         nowdiff=fLeftRightSub;
         errordiff1=errordiff2;
         errordiff2=nowdiff-lastdiff;
         errordiff=errordiff2-errordiff1;

      fValue=(fLeftRightSub*DIR_P) /fLeftRightAdd;

fDValue= (errordiff*DIR_D) /fLeftRightAdd;

   //g_nMotorLeftRightDiffKeep +=fDValue ;
// g_nMotorLeftRightDiffNew = fValue;
// g_nMotorLeftRightDiffNew = (g_nMotorLeftRightDiffKeep/16) + fValue;
g_nMotorLeftRightDiffNew = fDValue + fValue;
// if(g_nMotorLeftRightDiffNew>60)g_nMotorLeftRightDiffNew=60;
// if(g_nMotorLeftRightDiffNew<-60)g_nMotorLeftRightDiffNew=-60;
}
//--------------------------------------------------------------------------
//----------------------- 方向调节平滑输出函数--------------------------------
//==========================================================================
void CalculateMotorLeftRightDiff(void)
{
float nValue;
// if(CAR_STOP_FLAG == 1 || IF_CMA_STOP) {
// return;
// }

nValue = g_nMotorLeftRightDiffNew - g_nMotorLeftRightDiffOld;
nValue = nValue * (DIR_COUNT+ 1) / 2 + g_nMotorLeftRightDiffOld;
g_nMotorLeftRightDiff = nValue;

}
//-----------------------------------------------------------------------------
//---------------------------- 速度设置函数 -----------------------------------
//=============================================================================
void SpeedSet(int value)
{
g_nMotorSpeedSet=0;
}
//-----------------------------------------------------------------------------
//----------------------陀螺仪零点自矫程序------------------------------
//======================================================================
#define CV_GYRO_VAL (g_nCarVoltage[CV_GYRO])
void jiaoling()
{
unsigned char i;
long value;
delays(1000); //先延时一段时间使读的的数据稳定

for(i=0;i<20;i++)
{
ADC_GetValue16(g_nCarVoltage);
value+=(long)CV_GYRO_VAL;
// delays(100);
}
TLYDZ=value/20;
   TLYDZ-=20;    //TLYDZ是一个存储陀螺仪零值的变量（使用时先定义成全局变量，再在程序中使用）

}
//-------------------------------------------------------------------------
//------------------------转弯（减速）控制函数-----------------------------
//=========================================================================
void zhuanwancontrol(int speeds)
{
if(zhuanwan>15||zhuanwan<-15)
{
flag=1;

if((zhuanwan>15&&zhuanwan<30)||(zhuanwan<-15&&zhuanwan>-30))
{
if(g_nMotorSpeedSet>speeds)

     g_nMotorSpeedSet-=(g_nMotorSpeedSet-speeds)/20;

}
     if((zhuanwan>30&&zhuanwan<50)||(zhuanwan<-30&&zhuanwan>-50))
     {
        if(g_nMotorSpeedSet>(speeds/2))
     g_nMotorSpeedSet-=(g_nMotorSpeedSet-(speeds/2))/10;


     }
     if((zhuanwan>50)||(zhuanwan<-50))
     {
     if(g_nMotorSpeedSet>(speeds/5))

g_nMotorSpeedSet-=(g_nMotorSpeedSet-(speeds/5))/3;

     }
}
else
{
flag=0;
/* if(g_nMotorSpeedSet>speed)
g_nMotorSpeedSet += speed/500;
*/
}

}
//---------------------------------------------------------------------
//------------------------key1扫描，直立开始---------------------------
//=====================================================================
#define Key1_GetVal() ((bool)(getRegBits(GPIO_F_DR,Key1_PIN_MASK)))
unsigned m,n;
int g_nMotorSpeedSet1;
#define Key1_GetVal() ((bool)(getRegBits(GPIO_F_DR,Key1_PIN_MASK)))
#define  C_z       310                //角度零偏值 (直立时a1的值)

#define  R_z    0.566          // 角度归一化常量 R_z=(180/(sign1最大值529-sign1最小值195))  (调试)
#define  R_gyro 0.199       //0.385  陀螺仪比例  先临时取T_z=4，然后C_gyro 由0逐渐增大，调试波形
#define  T_z       3       //角度时间补偿常量
#define  F       150       //角速度+角度积分频率=1000/5ms(控制周期)
#define  P       5.25//3.95
#define  D       0.145 //0.145

#define Max    350
#define Min -350
#define N    100 //常数，因为采集到的数值太小，需要放大一定的倍数

#define  MOTOR_OUT_DEAD_VAL  0
#define  SPEED_R       1
#define  SPEED_P       0.035 //0.045
#define  SPEED_I       0.040    //0.025

#define  LEFT_RIGHT_MINIMUM 0
  /*** Processor Expert internal initialization. DON'T REMOVE THIS CODE!!! ***/
int sign0=0,sign2=0;//sign1=0;
float Deltavalue=0;
void carstart()
{
//while(Key1_GetVal());

CarStart=1;          // 中断进入标志
}
void main(void)
  {
  /* Write your local variab
  /*** End of Processor Expert ile definition here */

   PE_low_level_init();/*nternal initialization.                   ***/

  /* Write your code here */

Car_Init();
jiaoling();
  init_5110();
  clear_5110();
PWM_OUT;
  //write_byte_5110()
  for(;;)
  {

   CarAngleCalculate();
   write_5110_num6_string(0,0,CV_ACCE_VAL);//读采集值
   write_5110_num6_string(0,1,TLYDZ);//读采集值

   write_5110_num6_string(0,2,g_fCarAcceVal);//读采集值

   write_5110_num6_string(0,3,angle);//读采集值

   delay_1us();
  }

PWMC1_Init();
setReg16(PWMA_PWMVAL0, 0U);
/* PWMA_PWMVAL1: VAL=6000 */
setReg16(PWMA_PWMVAL1, 8000U);
/* PWMA_PWMVAL2: VAL=0 */
setRegBit(PWMA_PMCTL,LDOK);
setReg16(PWMA_PMOUT,0x8000);
setReg16(PWMA_PWMVAL2, 0U);
/* PWMA_PWMVAL3: VAL=6000 */
setReg16(PWMA_PWMVAL3, 8000U);
setRegBit(PWMA_PMCTL,LDOK);
setReg16(PWMA_PMOUT,0x8000);
  }

穆旦阿斗 · 发表于 2013-5-16 17:18:26

额估计帮你会看程序的人没有几个的

梦想和平 · 发表于 2014-4-8 18:29:43

这也太多了吧，发出点重点啊。。。

zhineng胡淼 · 发表于 2014-4-12 18:32:24

不懂。。。

		自动登录	找回密码
密码			注册

求指教啊？哪位大神帮帮忙啊？

优秀会员奖章

活跃会员奖章