基于KEA128处理器的二轮直立程序

ID:427725 · 发表于 2018-11-16 15:02

简单粗暴，直接上代
#include "headfile.h"
#define    PIT_FlAG_CLR(PITn)       PIT->CHANNEL[PITn].TFLG |= PIT_TFLG_TIF_MASK       //清中断标志（写1 清空标志位）
#define zhongzhi          -17.4          //小车的目标角度
#define PonitSpeed       130
#define Induct_add       200
#define PWMMAX       1500//1800
#define PWMMIN       500//200
void SensorStructurePointerInit(Acc_Type_Def *Acc, Gyro_Type_Def *Gyro, Angle_Type_Def *Angle, Speed_Type_Def *Speed ){ //结构体指针初始化成员

  /*Gyro*/
  Gyro->X_Final=0;
  Gyro->X_Offset=0;
  Gyro->X_Value[0]=0;
  Gyro->X_Value[1]=0;
  Gyro->X_Value[2]=0;

  Gyro->Z_Final=0;
  Gyro->Z_Offset=0;
  Gyro->Z_Value[0]=0;
  Gyro->Z_Value[1]=0;
  Gyro->Z_Value[2]=0;

  //角度目标值  也就是平衡值  没有转换成角度值但是是线性关系。
  Angle->PiontValue = zhongzhi;

  //速度目标值  没有转换成真实速度  与速度线性关系
  Speed->PiontSpeed = PonitSpeed;

  Speed->PiontSpeedMax=600;

  Speed->PiontSpeedMin=-600;

}

void CarBalanceSystemInit(Gyro_Type_Def *Gyro){//系统初始化
  adc_init(ADC0_SE0);
  adc_init(ADC0_SE1);
  adc_init(ADC0_SE2);
  adc_init(ADC0_SE3);
  adc_init(ADC0_SE6);
  adc_init(ADC0_SE7);

  OLED_Init();                      //OLED初始化

  IIC_init();                         // 使用数字陀螺仪要初始化IIC
  InitMPU6050();                //MPU6050的初始化函数
  systick_delay_ms(10);
  gpio_init(C7,GPO,1);                // 使能FTM
  //左电机
  ftm_pwm_init(ftm2,ftm_ch0,13000,0);
  ftm_pwm_init(ftm2,ftm_ch1,13000,0);

  //右电机
  ftm_pwm_init(ftm2,ftm_ch2,13000,0);
  ftm_pwm_init(ftm2,ftm_ch3,13000,0);

  systick_delay_ms(10);

  //ftm的计数引脚可查看KEA128_port_cfg.h文件内的FTM0_COUNT_PIN与FTM1_COUNT_PIN的宏定义得知
  //一个ftm同一时间只能实现一种功能，如果用于测速就不能用于输出PWM
  ///////////////////////////////////////////////////////////////////////////////////////////////
  ftm_count_init(ftm0); //对引脚输入的脉冲进行计数       接编码器LSB
  gpio_init(D3,GPI,0); //用于判断方向                   接编码器DIR
  port_pull(D3);       //IO上拉

  ftm_count_init(ftm1); //对引脚输入的脉冲进行计数       接编码器LSB
  gpio_init(D4,GPI,0); //用于判断方向                   接编码器DIR
  port_pull(D4);       //IO上拉
  //////////////////////////////////////////////////////////////////////////////////////////////

  //定时器中断
  pit_init(pit0,0x9000);//1.539ms

  systick_delay_ms(10);

}

//绝对值函数必须要取地址放入形参
void AbsoluteValue(float *Value, float *Return){

  if( (*Value) <0) (*Return) = -1*(*Value);
  else *Value = *Return;
}

/*原始数据获取部分*/
void GetAccelerationValue(Acc_Type_Def *Acc){ //获取ACC原始数据
  Get_AccData();
  Acc->Z_Value[0] = Acc->Z_Value[1];
  Acc->Z_Value[1] = Acc->Z_Value[2];
  Acc->Z_Value[2] = ACC_Z;//本句是获取当前加速度  上两句都是上次值和上上次值
}

void GetAngleSpeedValue(Gyro_Type_Def *Gyro){  //获取Gyro原始数据
  Get_Gyro();
  Gyro->Z_Value[0] = Gyro->Z_Value[1];
  Gyro->Z_Value[1] = Gyro->Z_Value[2];
  Gyro->Z_Value[2] = GYRO_Z ;          //本句是获取当前Gyro上两句都是上次值和上上次值

  Gyro->X_Value[0] = Gyro->X_Value[1]; //前前次
  Gyro->X_Value[1] = Gyro->X_Value[2]; //前次
  Gyro->X_Value[2] = GYRO_X ;       //本次
}

//FTM0-E0  FTM1-E7  左右重新确认
void GetSpeedValue(Speed_Type_Def *Speed){ //获取Speed原始数据

  for(uint8 n=0; n<15 ; n++){  //循环误差保存
//存储前14次的值
Speed->ActualLiftPulse[n] = Speed->ActualLiftPulse[n+1]; //0~14位赋值
if( n==14 ){ //当前值 15次  最新值
   Speed->ActualLiftPulse[n+1] = ftm_count_get(ftm1)*(-1); //本次 15位赋值
   ftm_count_clean(ftm1);  //清除ftm1计数值
}//此处一定要修改好根据实际的硬件  对应好左右轮子  因为编码器是对着安装的  那么
//在车子往前走的时候  左右是相反的  所以要把其中一个负值编码器变为正的
//两个电机存在同样的问题  都是对着安装的
  }
  //同上
  for(uint8 n=0; n<15 ; n++){  //循环保存

Speed->ActualRightPulse[n] = Speed->ActualRightPulse[n+1]; //0~14位赋值
if( n==14 ){
   Speed->ActualRightPulse[n+1] = ftm_count_get(ftm0);  //本次 15位赋值
   ftm_count_clean(ftm0);  //清除ftm0计数值
}

  }

  if(gpio_get(D4)) //检测 ftm1的转向 H6  E1是针对左右编码器的  要弄清楚不要到时候改过以后
//左边编码器的方向  去控制右边编码器的正负号那就完蛋了
Speed->ActualLiftPulse[15] = -Speed->ActualLiftPulse[15];
  else
Speed->ActualLiftPulse[15] = Speed->ActualLiftPulse[15];

  if(gpio_get(D3))  //检测 ftm0的转向
Speed->ActualRightPulse[15] = -Speed->ActualRightPulse[15];
  else
Speed->ActualRightPulse[15] = Speed->ActualRightPulse[15]; //这个负号取决于你车向前走时  编码器值是否为正

}

void AngleSoftwareFilter(Acc_Type_Def *Acc, Gyro_Type_Def *Gyro , Angle_Type_Def *Angle)
{ //本函数用于数字陀螺仪通过软件互补融合滤波解算角度

//加速度  角速度采集
  Acc->Z_Final = 0.9*Acc->Z_Value[2] + 0.05*Acc->Z_Value[1] + 0.05*Acc->Z_Value[0];    //权值低通滤波
  Gyro->Z_Final = 0.9*Gyro->Z_Value[2] + 0.05*Gyro->Z_Value[1] + 0.05*Gyro->Z_Value[0];//权值低通滤波
  Gyro->X_Final = 0.92*Gyro->X_Value[2] + 0.05*Gyro->X_Value[1] + 0.03*Gyro->X_Value[0];//权值低通滤波
  //此处参数需要根据  清华方案里的方法去调节参数  看波形法
  Angle->FinalValue = 0.98*(Angle->FinalValue -0.05*Gyro->Z_Final) + 0.02*(Acc->Z_Final);//融合滤波

}

//void SpeedSoftwareFilter(Speed_Type_Def *Speed){ //速度低通滤波
//
//  /*警告无视*////编码器收到的数据  在文件前面有函数读取
//  Speed->ActualLiftPulse[15]=0.33*Speed->ActualLiftPulse[15] + 0.33*Speed->ActualLiftPulse[14] + 0.33*Speed->ActualLiftPulse[13];
//  Speed->ActualRightPulse[15]=0.33*Speed->ActualRightPulse[15] + 0.33*Speed->ActualRightPulse[14] + 0.33*Speed->ActualRightPulse[13];
//  // 6 2 2
//
//  //实际速度 = R+L
//  Speed->ActualSpeed[0] = Speed->ActualSpeed[1];
//  Speed->ActualSpeed[1] = Speed->ActualSpeed[2];
//  Speed->ActualSpeed[2] =
// ((Speed->ActualRightPulse[0]+Speed->ActualLiftPulse[0])+
// (Speed->ActualRightPulse[1]+Speed->ActualLiftPulse[1])+
// (Speed->ActualRightPulse[2]+Speed->ActualLiftPulse[2])+
// (Speed->ActualRightPulse[3]+Speed->ActualLiftPulse[3])+
// (Speed->ActualRightPulse[4]+Speed->ActualLiftPulse[4])+
// (Speed->ActualRightPulse[5]+Speed->ActualLiftPulse[5])+
// (Speed->ActualRightPulse[6]+Speed->ActualLiftPulse[6])+
// (Speed->ActualRightPulse[7]+Speed->ActualLiftPulse[7])+
// (Speed->ActualRightPulse[8]+Speed->ActualLiftPulse[8])+
// (Speed->ActualRightPulse[9]+Speed->ActualLiftPulse[9])+
// (Speed->ActualRightPulse[10]+Speed->ActualLiftPulse[10])+
// (Speed->ActualRightPulse[11]+Speed->ActualLiftPulse[11])+
// (Speed->ActualRightPulse[12]+Speed->ActualLiftPulse[12])+
// (Speed->ActualRightPulse[13]+Speed->ActualLiftPulse[13])+
// (Speed->ActualRightPulse[14]+Speed->ActualLiftPulse[14])+
// (Speed->ActualRightPulse[15]+Speed->ActualLiftPulse[15]))/32;
//
//  //滤波
//  Speed->ActualSpeed[2] = 0.85*Speed->ActualSpeed[2]+0.1*Speed->ActualSpeed[1]+0.05*Speed->ActualSpeed[0];
//  //7 2 1
//
//  //实际差速  = L-R
//  Speed->ActualDifferential[0] = Speed->ActualDifferential[1];
//  Speed->ActualDifferential[1] = Speed->ActualDifferential[2];
//  Speed->ActualDifferential[2] = Speed->ActualLiftPulse[15]-Speed->ActualRightPulse[15];
//  Speed->ActualDifferential[2] = 0.8*Speed->ActualDifferential[2]+0.15*Speed->ActualDifferential[1]+0.05*Speed->ActualDifferential[0];
//  //实际差速
//  Speed->ActualInstantaneous[0] = Speed->ActualInstantaneous[1];
//  Speed->ActualInstantaneous[1] = Speed->ActualInstantaneous[2];
//  Speed->ActualInstantaneous[2] = (Speed->ActualLiftPulse[15]+Speed->ActualRightPulse[15] + (Speed->ActualLiftPulse[14]+Speed->ActualRightPulse[14])) ;
//  //瞬时速度计算
//  Speed->ActualAcceleration = Speed->ActualInstantaneous[2] - Speed->ActualInstantaneous[0];
//  //加速度计算
//
//}

void MotorControlFlow(PWM_Type_Def *PWM, Ramp_Type_Def *Ramp)
{
//电机直接控制

//如测试线序在此处对*PWM赋值测试完毕再注释

//PWM->LiftValue = 1200;//左轮正传  往前走
////PWM->LiftValue = 1000;
////PWM->LiftValue = 800;

//PWM->RightValue =1200;//右轮正传往前走
// PWM->RightValue = 1000; //PWM->RightValue = 800;

//左边右边都给 1000的时候应该是不转的

  if(PWM->LiftValue>(PWMMAX))PWM->LiftValue = (PWMMAX);
  if(PWM->LiftValue<(PWMMIN))PWM->LiftValue = (PWMMIN);
  if(PWM->RightValue>(PWMMAX))PWM->RightValue = (PWMMAX);
  if(PWM->RightValue<(PWMMIN))PWM->RightValue = (PWMMIN);

  //此处必须先调整好  保证 PWM值是 >1000时  是前进
  //PWM值< 1000 是后退而且两轮必须一致
  //能改硬件就改驱动线  和电机引线  例如
  //给同样都是  1200  左边电机慢速往前  右边慢速往后这时候调换右电机线
  //如果出现如下情况  给1200  理论上是正转 20%占空比  但出现转速很快说明
  //驱动线序不对可以在这里调换通道  或者直接改线序

  if(PWM->LiftValue>1000){ //左轮前进  1000~2000
   ftm_pwm_duty(ftm2,ftm_ch1,PWM->LiftValue-1000);
   ftm_pwm_duty(ftm2,ftm_ch0,0);
  }
  else if(PWM->LiftValue<1000){ //左轮是后退  0~1000
   ftm_pwm_duty(ftm2,ftm_ch1,0);
   ftm_pwm_duty(ftm2,ftm_ch0,1000-PWM->LiftValue);
  }  //如果发现此处给定的是  控制右轮转动则调换下硬件驱动线线序  注意是两根两根的调换

  if(PWM->RightValue>1000){ //右轮是前进  1000~2000
   ftm_pwm_duty(ftm2,ftm_ch3,PWM->RightValue-1000);
   ftm_pwm_duty(ftm2,ftm_ch2,0);
  }
  else if(PWM->RightValue<1000){ //右轮是后退  0~1000
   ftm_pwm_duty(ftm2,ftm_ch3,0);
   ftm_pwm_duty(ftm2,ftm_ch2,1000-PWM->RightValue);
  } //如果发现此处给定的是  控制左边轮转动则调换下硬件驱动线线序  注意是两根两根的调换

}

/*被控制量处理部分*/
void BalanceFeedbackControl(PID_Type_Def *PID, Angle_Type_Def *Angle, Gyro_Type_Def *Gyro, Ramp_Type_Def *Ramp){ //直立反馈控制

  PID->err = -(Angle->PiontValue-Angle->FinalValue);//硬件
  //偏差  =  角度目标-角度反馈值
  PID->out = 1000 + PID->Kp * PID->err + PID->Kd * Gyro->Z_Final;
  //1000是为了给一个PWM中点 Gyro->Z_Final就是直立的角速度  也就是D控制  PD
  //一般情况下  调直立时先给一个P参数  D给0 就能立起来如果发现不能立疯转
  //多半是 pid方向相反把(Angle->PiontValue-Angle->FinalValue)  里面调换下位置  即可
};

//牛顿迭代法开方运算
float newtonSqrt(float a){
  int i;
  float x;
  x=a;
  for(i=1;i<=10;i++)
x=(x+a/x)/2;
  return x;
}

//void DirectionFeedbackControl(PID_Type_Def *PID, Inductor_Type_Def *InductorA, Inductor_Type_Def *InductorB, Gyro_Type_Def *Gyro, Speed_Type_Def *Speed, Ramp_Type_Def *Ramp){ //方向反馈控制
//
//  //调节offset 在中间位置的时候让左边的电感等于右边的电感
//  InductorB->LeftFinal = PID->offset*InductorB->LeftFinal;
//  InductorA->LeftFinal = (2-PID->offset)*InductorA->LeftFinal;
//  //调整零偏
//
//  //主要是左右两个电感
//  PID->err = 10*((newtonSqrt(InductorB->LeftFinal)-newtonSqrt(InductorA->LeftFinal ))/ (InductorB->LeftFinal+InductorA->LeftFinal));
//  //如果和值小于一个数值可认为没有偏差车子继续沿着中间路线行驶
//  if(InductorA->AndValues<Induct_add)  PID->err = 0;
//
// //*************注意PD的极性也就是正负*******************
//  PID->out = PID->Kp * PID->err +PID->Kd * Gyro->X_Final;
//  //转向PID输出计算 Gyro->X_Final; 是转向角速度也是D控制
// //*************注意PD的极性也就是正负******************
//
//  Speed->PiontDifferential = (int16)(PID->out);  //给定差速环目标值
//  //把转向环计算出来的输出值给差速目标环
//
//
//  for(uint8 n=0; n<9 ; n++){  //循环误差保存
//
// PID->err_k[n] = PID->err_k[n+1]; //0~8位赋值
//
// if( n==8 )
//    PID->err_k[n+1] = PID->err;  //本次 9位赋值
//
//  }
//
//};

////差速环就是角速度和编码器的值一起控制
//void DifferentialFeedbackControl(PID_Type_Def *PID, Speed_Type_Def *Speed, Gyro_Type_Def *Gyro){//差速反馈控制
//
//  //PID->offset只是一个比例分配  PID->offset=2
//  int16 SpeedDifferentialValue = PID->offset * Speed->ActualDifferential[2] + (1-PID->offset)*(-0.18*Gyro->X_Final);
//  //得到差速值是编码器和转向角速度共同决定差速反馈值
//
//  //差速目标-  差速实际值
//  PID->err = ( Speed->PiontDifferential -  SpeedDifferentialValue);
//
// //误差积分
//  PID->integral += PID->err;
//
//  //积分上下限
//  if(PID->integral>50000)PID->integral=50000;
//
//  if(PID->integral<-50000)PID->integral=-50000;
//
//  if(Speed->ActualSpeed[2]< 50)PID->integral = 0;
//  //低速积分清零
//
//  PID->out =  PID->Kp * PID->err + PID->Kd * ( SpeedDifferentialValue + PID->err_k[0]);// + PID->Ki * PID->integral;
//  //PID计算
//
//
//
//  for(uint8 n=0; n<9 ; n++){  //循环误差保存
// PID->err_k[n] = PID->err_k[n+1]; //0~8位赋值
// if( n==8 )
//    PID->err_k[n+1] = PID->err;  //本次 9位赋值
//  }
//
//
//
//  PID->err_k[0] = SpeedDifferentialValue;//借用一下PID->err_k[0]咯  存入上次值差速
//
//  //输出幅度限制
//  if(PID->out>2200)PID->out=2200;//1800
//  if(PID->out<-2200)PID->out=-2200;
//
//};

//void SpeedFeedbackControl(PID_Type_Def *PID, Speed_Type_Def *Speed, Ramp_Type_Def *Ramp, Switch_Type_Def *Switch){ //速度反馈控制
//
//
//  PID->out_k[9] = PID->out;//保存成上一次输出值
//
//  PID->out_k[0] = 0;
//  //误差=实际速度-目标速度
//  PID->err = Speed->ActualSpeed[2] - Speed->PiontSpeed;
//  //误差积分
//  PID->integral = PID->err_k[0]+PID->err_k[1]+PID->err_k[2]+PID->err_k[3]+PID->err_k[4]+
//
//                PID->err_k[5]+PID->err_k[6]+PID->err_k[7]+PID->err_k[8]+PID->err_k[9]+PID->err;
//
//  PID->out = PID->Kp * PID->err ;//+ PID->Ki * PID->integral;  //速度环
//
//  PID->out *= 0.02; //缩小速度环输出
//
//  if(PID->out > 1000) PID->out= 1000 ;
//
//  if(PID->out < -1000) PID->out= -1000;
//
//
//  for(uint8 n=0; n<9 ; n++){  //循环误差保存
//
// PID->err_k[n] = PID->err_k[n+1]; //0~8位赋值
// if( n==8 )
//    PID->err_k[n+1] = PID->err;  //本次 9位赋值
//  }
//
//};

void PWMFeedbackControl(PWM_Type_Def *PWM, PID_Type_Def *PID_Balance, PID_Type_Def *PID_Speed, PID_Type_Def *PID_Path, PID_Type_Def *PID_Diff, Speed_Type_Def *Speed, Switch_Type_Def *Switch){ //PWM叠加反馈控制

PID_Speed->out_k[0] += (PID_Speed->out - PID_Speed->out_k[9]) * 0.0625;//分段成1/16分增量
  //PID_Speed->out_k[9]是上次值  ， PID_Speed->out是本次值， PID_Speed->out_k[0] 是平滑输出增量值
  //叠加的时候把平滑输出增量值 + 上次的值得到16个周期内每4ms周期应给的值

  PWM->PiontValue  = (int16)(PID_Balance->out)-(int16)(PID_Speed->out_k[0] + PID_Speed->out_k[9]) ;  //把所有环输出相加

  PWM->Differential = (int16)(PID_Diff->out);    //差速PWM
  PWM->Differential = 0;    //差速PWM

  PWM->LiftValue = PWM->PiontValue +  PWM->Differential;

  PWM->RightValue = PWM->PiontValue  -  PWM->Differential;

};

//uint8 dat;
////车子失控停车
//void OutOfControlSignal(Inductor_Type_Def *InductorA, Inductor_Type_Def *InductorB){
//    //蓝牙控制
//
//    //1的ascall码是49 收到1停止
// if(dat==49){
//
// ftm_pwm_duty(ftm2,ftm_ch0,0);
// ftm_pwm_duty(ftm2,ftm_ch1,0);
// ftm_pwm_duty(ftm2,ftm_ch2,0);
// ftm_pwm_duty(ftm2,ftm_ch3,0);
//
// while(1)
// { //2的ascall码是50 收到2启动
//         if(dat==50)break;
//    };
//
// dat=0; //清除标志位
//
// }
//
// //保护程序，没有检测到电感值就停下来
// if(InductorA->LeftFinal<3&&InductorB->LeftFinal<3)
//{
//    ftm_pwm_duty(ftm2,ftm_ch0,0);
// ftm_pwm_duty(ftm2,ftm_ch1,0);
// ftm_pwm_duty(ftm2,ftm_ch2,0);
// ftm_pwm_duty(ftm2,ftm_ch3,0);
//
//
//}
//
//};

/////////////////////////////////////////////////////////////////////////
  uint8 timer3ms = 0, timer4_5ms=0,timer24ms=0 ,timer64ms=0;
  float  OutData[3];                                                 //山外上位机显示的数据
//定义结构体
  PID_Type_Def  PID_Balance, PID_Speed, PID_Path, PID_Diff;
  RUN_Type_Def    RUN;
  Stop_Type_Def    Stop;
  Ramp_Type_Def    Ramp;
  Curve_Type_Def    Curve;
  Acc_Type_Def    Acc;
  Gyro_Type_Def    Gyro;
  Angle_Type_Def    Angle;
  PWM_Type_Def    PWM;
  Speed_Type_Def    Speed;
  Button_Type_Def Button;
  Switch_Type_Def Switch;
  Inductor_Type_Def  InductorA, InductorB;
///////////////////////////////////////////////////////////////////////

int main(void)
{
  //由于本程序闭环较多  调试过程比较繁琐  建议一开始的时候屏蔽掉其他环
  //只调节最基础的环节  平衡车主要就是对于两个电机的控制  其中有一个函数
  //MotorControlFlow函数是直接控制电机的  这个函数需要调试好  其要求就是
  //占空比0~1000退后 1000~2000向前  通过调换驱动线  和调换电机线  或者
  //调换程序中 ch2 3 4 5 的  23顺序  45顺序来  达到最终的效果  两个电机
  //给1000以上是向前走 1000一下向后走  2000往前最快 0往后最快

  //当电机输出调整好了以后  再调节编码器反馈  我们在MotorControlFlow函数里
  //直接给  PWM->LiftValue = 1200;PWM->RightValue = 1200;  让两个电机固定占空比
  //转动  这时候通过IAR在线调试观察  编码器测速值是否正确  是否对应左轮右轮
  //(左电机转动对应左编码器  右电机转动对应右边编码器  不能错！)
  //用手阻止一个电机观察是否是那个电机的编码器转速下降  确保编码器的方向一致！

  //编码器和电机调整好以后就方便了很多  接下来就是直立  前提是陀螺仪参数已经整定好
  //我们调试直立的时候  要把其他环都关闭方法如下  函数PWMFeedbackControl 是把所有环输出
  //整合到电机PWM上的函数其中  PWM->PiontValue  = (int16)(PID_Balance->out)+ (int16)(PID_Speed->out_k[0] + PID_Speed->out_k[9]) ;
  //这句话是把  三个环的输出叠加在一起  我们只需要保留直立环  这样调节直立比较顺手
  //其中还有一个差速也要给0  对直立就无影响

  //当直立调节完毕后  速度环和直立的融合是比较难的  直立和速度是并环需要精心调试，也可以
  //跳过速度环的调试  直接进入转向环  与差速环他们之间是串级PID的关系

  //如果出现中断  跑飞或者  卡住在ADC死循环里  说明堆栈溢出  调整KEA128.ICF文件的  堆栈值扩大  不超过12K即可
get_clk();             //获取时钟频率必须执行

//MPU6050与单片机接线请查看IIC文件最上方注释
//OLED接线查看OLED文件最上方
IIC_init();
InitMPU6050();
SensorStructurePointerInit( &Acc, &Gyro, &Angle, &Speed);//传感器参数初始化
OLED_Init();
   CarBalanceSystemInit( &Gyro ); //系统.初始化
//在线调试查看mpu_gyro_x mpu_gyro_y mpu_gyro_z mpu_acc_x mpu_acc_y mpu_acc_z三个变量即可
   RUN.Flag[4] = 1;//运行出界标志位

  RUN.Flag[5] = 0;//启动电机标志位

  Stop.Flag[0] = 0;//停止线标志位延时启动标志位

  Stop.Flag[1] = 0;//停止标志位

  Ramp.Flag[0] = 0;//接近坡道标志位

  Ramp.Flag[1] = 0;//上坡道标志位

  Curve.Flag[1] = 0;//正差速标志位

  Curve.Flag[2] = 0;//反差速标志位

  Curve.Flag[3] = 0;//差速数目
   while(1){

   //  if( timer4_5ms  ){
//    ftm_pwm_duty(ftm2,ftm_ch3,100);
//    ftm_pwm_duty(ftm2,ftm_ch2,0);
   GetAccelerationValue( &Acc );                                        //加速度原始数据采集
   GetAngleSpeedValue( &Gyro );                                        //角速度原始数据采集
      GetSpeedValue(&Speed);                                                 //获取原始数据
//       GetInductorValue( &InductorA, &InductorB );                         //电感电压原始数据采集

AngleSoftwareFilter(&Acc,&Gyro ,&Angle);                            //软件角度滤波处理
//       SpeedSoftwareFilter( &Speed );                                     //速度递归权值滤波
// Get_AD_data(&InductorA, &InductorB);

      BalanceFeedbackControl( &PID_Balance, &Angle, &Gyro, &Ramp );       //平衡度反馈计算
      //DirectionFeedbackControl( &PID_Path, &InductorA, &InductorB, &Gyro, &Speed, &Ramp );//路径反馈计算
   //DifferentialFeedbackControl( &PID_Diff, &Speed, &Gyro);             //差速反馈计算

   PWMFeedbackControl( &PWM, &PID_Balance, &PID_Speed, &PID_Path, &PID_Diff, &Speed, &Switch );//PWM叠加直立方向速度PWM
   MotorControlFlow( &PWM, &Ramp );                                     //电机PWM输出控制
//       OutOfControlSignal(&InductorA, &InductorB);                            //失控保护
// OutData[0]=Gyro.X_Final;                                           //山外上位机显示的数据
// OutData[1]=Gyro.Z_Final;                                              //山外上位机显示的数据
// OutData[2]=Angle.FinalValue;                                        //山外上位机显示的数据
// vcan_sendware((uint8 *)OutData,sizeof(OutData));                   //山外调试助手
   timer4_5ms = 0;                                                    //清除时间标志位
  //    }

if(timer64ms && timer4_5ms != 1 ){

//       人机交互界面高速运行不执行
//    OLED_Print_Num1(7, 6,Angle.FinalValue*100);
//       OLED_Print_Num1(7, 4,    Speed.ActualLiftPulse[15]);
//       OLED_Print_Num1(7, 5,    Speed.ActualRightPulse[15]);
   // SpeedFeedbackControl( &PID_Speed, &Speed, &Ramp, &Switch ); //速度环反馈控制

//    OLED_Print_Num1(7, 1, InductorA.LeftValue[2]);
//       OLED_Print_Num1(7, 2, InductorB.LeftValue[2]);
   OLED_Print_Num1(7, 3, InductorA.LeftFinal);
      OLED_Print_Num1(7, 4, InductorB.LeftFinal);

   timer64ms = 0;
};

  }

}

uint8 count = 0;

void PIT_CH0_IRQHandler(void)
{

  if( count<84 )
count++;
  else count = 0;

//  if( count%2==0 ) timer3ms = 1;

  if( count%3==0 ) timer4_5ms = 1;

  if( count%16==0 ) timer24ms = 1;

  if( count%42==0 ) timer64ms=1;

  PIT_FlAG_CLR(pit0);//清除中断标志位

}

ID:1 · 发表于 2018-11-16 15:32

补全头文件工程包者详细说明一下电路连接即可获得100+黑币

帐号		自动登录	找回密码
密码			立即注册