广电组.docx
- 文档编号:15364163
- 上传时间:2023-07-03
- 格式:DOCX
- 页数:18
- 大小:18.19KB
广电组.docx
《广电组.docx》由会员分享,可在线阅读,更多相关《广电组.docx(18页珍藏版)》请在冰点文库上搜索。
广电组
#include"common.h"
#include"include.h"
#defineZOUTADC0_SE17
#defineGyroADC1_SE16
/***********************直立控制参数********************/
#defineGYRO_VAL1923//陀螺仪中值
#defineMMA7361_VAL2343//Z中值//加大向后,减少向前
#defineMMA7361_ratio0.15//归一化比例
#defineGyro_ratio0.25
#defineANGLE_CONTROL_P80
#defineANGLE_CONTROL_D2.98
#defineSPEED_CONTROL_P10
#defineSPEED_CONTROL_I0
#defineSPEED_CONTROL_D0
/*角度控制变量*/
floatg_fGyroscopeAngleSpeed;//陀螺仪转化后的角速度
floatg_fGyroscopeAngleIntegral;
floatg_fCarAngle;//归一化的角度-90+90
floatg_fGravityAngle;
floatg_fAngleControlOut;
volatileintMMA7361,ENC03;//加速度计AD,陀螺仪AD,模块输出的角度
/*角度融合控制参数*/
#defineCONTROL_PERIOD5
#defineGRAVITY_ADJUST_TIME_CONSTANT1.12//时间常数Tz
#defineGYROSCOPE_ANGLE_SIGMA_FREQUENCY((float)(1000/CONTROL_PERIOD))
/*角度与角速度设定*/
#defineCAR_ANGLE_SET0.0
#defineCAR_ANGLE_SPEED_SET0.0
/*速度控制参数*/
#defineCAR_SPEED_SET0
#defineMOTOR_OUT_DEAD_VAL0//150//150
#defineMOTOR_LEFT_SPEED_POSITIVE(g_fLeftMotorOut>0)
#defineMOTOR_RIGHT_SPEED_POSITIVE(g_fRightMotorOut>0)
#defineCAR_SPEED_CONSTANT1000.0/(SPEED_CONTROL_COUNT*CONTROL_PERIOD)/(float)OPTICAL_ENCODE_CONSTANT
#defineOPTICAL_ENCODE_CONSTANT157//
#defineSPEED_CONTROL_COUNT20//20ms*5ms
#defineSPEED_CONTROL_PERIOD(SPEED_CONTROL_COUNT*CONTROL_PERIOD)
/*速度控制变量*/
intg_nSpeedControlFlag,g_nSpeedControlPeriod,g_nSpeedControlCount;
floatg_fCarSpeed,g_fSpeedControlOutOld,g_fSpeedControlOutNew,g_fSpeedControlOut,g_fSpeedControlIntegral;
floatg_fLeftMotorOut,g_fRightMotorOut;
longg_nLeftMotorPulse,g_nRightMotorPulse,g_nLeftMotorPulseSigma,g_nRightMotorPulseSigma;
/*各项输出限幅*/
#defineMOTOR_OUT_MAX1000
#defineMOTOR_OUT_MIN-1000
#defineANGLE_CONTROL_OUT_MAXMOTOR_OUT_MAX*10
#defineANGLE_CONTROL_OUT_MINMOTOR_OUT_MIN*10
#defineSPEED_CONTROL_OUT_MAXMOTOR_OUT_MAX*10
#defineSPEED_CONTROL_OUT_MINMOTOR_OUT_MIN*10
#defineDIRECTION_CONTROL_OUT_MAXMOTOR_OUT_MAX
#defineDIRECTION_CONTROL_OUT_MINMOTOR_OUT_MIN
externvoidPIT0_IRQHandler(void);
externvoidPIT1_IRQHandler(void);//左编码器中断
/////////////////////////////////////////////////////////////////////////////////////
#defineTSL1401_SI(x)gpio_set(PTE6,x)/////////////
#defineTSL1401_CLK(x)gpio_set(PTE7,x)/////////////
voidinit_gpio(void);//GPIO初始化
voiddelay(void);//CLK延时
voidTSL1401_GetLine_Oneshot(uint8*);//单次采集
intCCDLineGet();
uint8gPixel[128]={0};
voidsendccd(void*ccdaddr,uint32_tccdsize)
{
#defineCMD_CCD2
uint8_tcmdf[2]={CMD_CCD,~CMD_CCD};//开头命令
uint8_tcmdr[2]={~CMD_CCD,CMD_CCD};//结尾命令
uart_putbuff(UART4,cmdf,sizeof(cmdf));//先发送命令
uart_putbuff(UART4,(uint8_t*)ccdaddr,ccdsize);//再发送图像
uart_putbuff(UART4,cmdr,sizeof(cmdr));//再发送命令
}
///////////////////////////////////////////////////////////////////////////////////////
voidRd_Ad_Value(void)
{
MMA7361=adc_once(ZOUT,ADC_12bit);//Z
ENC03=adc_once(Gyro,ADC_12bit);//gyro
}
voidAngleCalculate(void)
{
floatfDeltaValue;
Rd_Ad_Value();//采集AD
g_fGravityAngle=(MMA7361_VAL-MMA7361)*MMA7361_ratio;
g_fGyroscopeAngleSpeed=(int)(GYRO_VAL-ENC03)*Gyro_ratio;//陀螺仪新值=陀螺仪中值-AD采集的陀螺仪角度
/*if(g_fGravityAngle>90)
g_fGravityAngle=90;
if(g_fGravityAngle<-90)
g_fGravityAngle=-90;*/
g_fCarAngle=g_fGyroscopeAngleIntegral;
fDeltaValue=(g_fGravityAngle-g_fCarAngle)/GRAVITY_ADJUST_TIME_CONSTANT;
g_fGyroscopeAngleIntegral+=(g_fGyroscopeAngleSpeed+fDeltaValue)/GYROSCOPE_ANGLE_SIGMA_FREQUENCY;
/*****************************串口看波形(选择使用)****************************///宏条件编译选择是否使用虚拟示波器
OutData[2]=g_fGravityAngle;
OutData[3]=g_fCarAngle;
//OutData[0]=g_fGyroscopeAngleSpeed;
//OutData[3]=MMA7361;
//OutPut_Data();
}
//直立控制函数
voidAngleControl(void){
floatfValue;
fValue=(CAR_ANGLE_SET-g_fCarAngle)*ANGLE_CONTROL_P+
(CAR_ANGLE_SPEED_SET-g_fGyroscopeAngleSpeed)*ANGLE_CONTROL_D;
if(fValue>ANGLE_CONTROL_OUT_MAX)
fValue=ANGLE_CONTROL_OUT_MAX;
elseif(fValue fValue=ANGLE_CONTROL_OUT_MIN; g_fAngleControlOut=fValue; //OutData[0]=g_fAngleControlOut; //OutPut_Data(); } voidSpeedControl(void){ floatfDelta; floatfP,fI; g_fCarSpeed=(g_nLeftMotorPulseSigma+g_nRightMotorPulseSigma)/2; g_nLeftMotorPulseSigma=g_nRightMotorPulseSigma=0; g_fCarSpeed*=CAR_SPEED_CONSTANT; fDelta=CAR_SPEED_SET-g_fCarSpeed; fP=fDelta*SPEED_CONTROL_P; fI=fDelta*SPEED_CONTROL_I; g_fSpeedControlIntegral+=fI; if(g_fSpeedControlIntegral>SPEED_CONTROL_OUT_MAX) g_fSpeedControlIntegral=SPEED_CONTROL_OUT_MAX; if(g_fSpeedControlIntegral g_fSpeedControlIntegral=SPEED_CONTROL_OUT_MIN; g_fSpeedControlOutOld=g_fSpeedControlOutNew; g_fSpeedControlOutNew=fP+g_fSpeedControlIntegral; } voidSpeedControlOutput(void){ floatfValue; fValue=g_fSpeedControlOutNew-g_fSpeedControlOutOld; g_fSpeedControlOut=fValue*(g_nSpeedControlPeriod+1)/SPEED_CONTROL_PERIOD+g_fSpeedControlOutOld; //OutData[1]=g_fSpeedControlOut; //OutPut_Data(); } voidCarSubInit(void) { g_fGyroscopeAngleSpeed=0; g_fGyroscopeAngleIntegral=0; g_fCarAngle=0; g_fGravityAngle=0; g_nLeftMotorPulseSigma=g_nRightMotorPulseSigma=0; g_fAngleControlOut=g_fSpeedControlOut=0; g_fSpeedControlOutOld=g_fSpeedControlOutNew=0; g_fSpeedControlIntegral=0; g_nSpeedControlCount=g_nSpeedControlPeriod=0; } voidSetMotorVoltage(floatfLeftVoltage,floatfRightVoltage) { if(fLeftVoltage>0) { ftm_pwm_duty(FTM0,FTM_CH4,((int)fLeftVoltage)); ftm_pwm_duty(FTM0,FTM_CH2,0); } else { fLeftVoltage=-fLeftVoltage; ftm_pwm_duty(FTM0,FTM_CH2,((int)fLeftVoltage)); ftm_pwm_duty(FTM0,FTM_CH4,0); } //-------------------------------------------------------------------------- if(fRightVoltage>0) { ftm_pwm_duty(FTM0,FTM_CH6,((int)fRightVoltage)); ftm_pwm_duty(FTM0,FTM_CH5,0); } else { fRightVoltage=-fRightVoltage; ftm_pwm_duty(FTM0,FTM_CH5,((int)fRightVoltage)); ftm_pwm_duty(FTM0,FTM_CH6,0); } /*OutData[0]=g_fGravityAngle; OutData[1]=g_fCarAngle; OutData[2]=fRightVoltage; OutData[3]=fLeftVoltage; OutPut_Data();*/ //MOTOR_SETLOAD;//ReloadthePWMvalue } voidMotorSpeedOut(void) { floatfLeftVal,fRightVal; fLeftVal=g_fLeftMotorOut; fRightVal=g_fRightMotorOut; if(fLeftVal>0)fLeftVal+=MOTOR_OUT_DEAD_VAL; elseif(fLeftVal<0)fLeftVal-=MOTOR_OUT_DEAD_VAL; if(fRightVal>0)fRightVal+=MOTOR_OUT_DEAD_VAL; elseif(fRightVal<0)fRightVal-=MOTOR_OUT_DEAD_VAL; if(fLeftVal>MOTOR_OUT_MAX)fLeftVal=MOTOR_OUT_MAX; if(fLeftVal if(fRightVal>MOTOR_OUT_MAX)fRightVal=MOTOR_OUT_MAX; if(fRightVal SetMotorVoltage((fLeftVal),(fRightVal)); } voidMotorOutput(void) { floatfLeft,fRight; fLeft=g_fAngleControlOut; fRight=g_fAngleControlOut;///////////// if(fLeft>MOTOR_OUT_MAX)fLeft=MOTOR_OUT_MAX; if(fLeft if(fRight>MOTOR_OUT_MAX)fRight=MOTOR_OUT_MAX; if(fRight g_fLeftMotorOut=fLeft; g_fRightMotorOut=fRight; //OutData[0]=g_fRightMotorOut*100; //OutData[1]=g_fLeftMotorOut*100; MotorSpeedOut(); } voidmain() { /*DisableInterrupts; gpio_init(PTA4,GPO,0); uart_init(VCAN_PORT,9600); adc_init(ZOUT); adc_init(Gyro);//角加速度Angular1 //adc_init(Ang); ftm_pwm_init(FTM0,FTM_CH2,10000,1000); ftm_pwm_init(FTM0,FTM_CH4,10000,1000); ftm_pwm_init(FTM0,FTM_CH5,10000,1000); ftm_pwm_init(FTM0,FTM_CH6,10000,1000); CarSubInit(); ftm_quad_init(FTM1); ftm_quad_init(FTM2); pit_init_ms(PIT0,5); pit_init_ms(PIT1,5); set_vector_handler(PIT0_VECTORn,PIT0_IRQHandler);//设置PIT0的中断复位函数为PIT0_IRQHandler set_vector_handler(PIT1_VECTORn,PIT1_IRQHandler); enable_irq(PIT0_IRQn);//使能PIT0中断 enable_irq(PIT1_IRQn); EnableInterrupts;//中断允许*/ uart_init(UART4,115200); init_gpio(); inti; for(i=0;i<128;i++)gPixel[i]=i; while (1){ TSL1401_GetLine_Oneshot(gPixel); sendccd(gPixel,128); } } intCCDLineGet() { inti; intCCDDiffd=0,CCDDiffd2=0,CCDDiffd3=0;//保存i和i+5的差值 intg_CCDLiftDiff=0,g_CCDRightDiff=0,g_CCDDiffMIN=40;//左边的差值,右边的差值,最小差值 intCCDLineR=0,CCDLineL=0;//记录位置 intCCDOnLift=0;//记录左边是否有线 intCCDOnRight=0;//记录右边是否有线 intg_LineWidth=0,g_LineWidthMIN=30,g_Center=0;//线宽,最小线宽,中心点 intg_CCDTurnOut=0;//转向输出 intCCDOnBLift=0,CCDOnBRight=0; intCCDLineBR=0,CCDLineBL=0; for(i=0;i<123;i++){ if(gPixel[i]-gPixel[i+5]>=40)printf("1"); elseprintf(""); } printf("\n"); for(i=64;i>=1;i--) { CCDDiffd=gPixel[i+5]-gPixel[i]; CCDDiffd2=gPixel[i+4]-gPixel[i]; CCDDiffd3=gPixel[i+3]-gPixel[i]; if(CCDDiffd>g_CCDDiffMIN&&CCDDiffd2>g_CCDDiffMIN&&CCDDiffd3>g_CCDDiffMIN)//g_CCDDiffMINCCD黑白线差值最小值 { g_CCDLiftDiff=CCDDiffd; CCDLineL=i+5; CCDOnLift=1; //printf("左边有线\n"); } } for(i=65;i<=122;i++) { CCDDiffd=gPixel[i]-gPixel[i+5]; CCDDiffd2=gPixel[i]-gPixel[i+4]; CCDDiffd3=gPixel[i]-gPixel[i+3]; if(CCDDiffd>g_CCDDiffMIN&&CCDDiffd2>g_CCDDiffMIN&&CCDDiffd3>g_CCDDiffMIN)//g_CCDDiffMINCCD黑白线差值最小值 { g_CCDRightDiff=CCDDiffd; CCDLineR=i; CCDOnRight=1; //printf("右边有线\n"); } } if(CCDOnLift==1&&CCDOnRight==1) { g_LineWidth=CCDLineR-CCDLineL; if(g_LineWidth>g_LineWidthMIN) { g_Center=(CCDLineL+CCDLineR)/2; g_CCDTurnOut=64-g_Center;//64_g_CCDLineCenter printf("中间: %d\n",64-g_Center); } } if(CCDOnLift==1&&CCDOnRight==0
- 配套讲稿:
如PPT文件的首页显示word图标,表示该PPT已包含配套word讲稿。双击word图标可打开word文档。
- 特殊限制:
部分文档作品中含有的国旗、国徽等图片,仅作为作品整体效果示例展示,禁止商用。设计者仅对作品中独创性部分享有著作权。
- 关 键 词:
- 广电