|
- #include "msp430g2553.h"
- #include "uart.h"
- #include "mpu6050.h"
- #include "stdio.h"
- #include "string.h"
- static unsigned char ms10;
- unsigned int second,ms11;
- static unsigned char EnableMeasureSpeed=0; //周期性测速标志//
- static unsigned int LeftMotorSpeed,RightMotorSpeed; //测量电机速度缓存//
- unsigned int SystemLeftSpeed,SystemRightSpeed; //实际左右电机速度//
- //int PidOut,Error;
- int LeftPIDOut,RightPIDOut; //左右电机PID计算结果//
- //左电机方向控制接口
- //做电机方向控制接线:
- //绿线:P23, 蓝线:P25
- //左电机速度输出线为棕色 接在P13
- //左电机速度控制线为黄色,在P24
- #define LeftMotorForward {P2OUT &= ~BIT3;P2OUT |= BIT5;}
- #define LeftMotorReverse {P2OUT |= BIT3;P2OUT &= ~BIT5;}
-
- //右电机方向控制接口:
- //右电机方向控制接线:
- //蓝线:P22 黄线:P20
- //右电机速度输出线颜色为:黑色 接在P14
- //右电机速度控制线为绿色,在P21//
- #define RightMotorForward {P2OUT &= ~BIT0;P2OUT |= BIT2;}
- #define RightMotorReverse {P2OUT |= BIT0;P2OUT &= ~BIT2;}
- //电机方向控制IO初始化//
- void InitMotorControlIO()
- {
- P2DIR |= BIT0 + BIT2 + BIT3 +BIT5; //设置为输出//
- //P2REN |= BIT0 + BIT2 + BIT3 +BIT5; //开启上拉//
-
- }
- //看门狗定时器产生1ms中断//
- void WDTplus_graceInit(void)
- {
- WDTCTL = WDTPW | WDTTMSEL | WDTIS0;
- IE1 |= WDTIE;
- //P1DIR |=BIT0; //在P10测试脉冲周期为500hz//
- }
- //mclk=16MHz DCO
- //SMCLK=8MHz
- //ACLK=32.768KHz
- void BCSplus_graceInit(void)
- {
- /* USER CODE START (section: BCSplus_graceInit_prologue) */
- /* User initialization code */
- /* USER CODE END (section: BCSplus_graceInit_prologue) */
- /*
- * Basic Clock System Control 2
- *
- * SELM_0 -- DCOCLK
- * DIVM_0 -- Divide by 1
- * ~SELS -- DCOCLK
- * DIVS_1 -- Divide by 2
- * ~DCOR -- DCO uses internal resistor
- *
- * Note: ~<BIT> indicates that <BIT> has value zero
- */
- BCSCTL2 = SELM_0 | DIVM_0 | DIVS_1;
- if (CALBC1_16MHZ != 0xFF) {
- /* Adjust this accordingly to your VCC rise time */
- __delay_cycles(100000);
- /* Follow recommended flow. First, clear all DCOx and MODx bits. Then
- * apply new RSELx values. Finally, apply new DCOx and MODx bit values.
- */
- DCOCTL = 0x00;
- BCSCTL1 = CALBC1_16MHZ; /* Set DCO to 16MHz */
- DCOCTL = CALDCO_16MHZ;
- }
- /*
- * Basic Clock System Control 1
- *
- * XT2OFF -- Disable XT2CLK
- * ~XTS -- Low Frequency
- * DIVA_0 -- Divide by 1
- *
- * Note: ~XTS indicates that XTS has value zero
- */
- BCSCTL1 |= XT2OFF | DIVA_0;
- /*
- * Basic Clock System Control 3
- *
- * XT2S_0 -- 0.4 - 1 MHz
- * LFXT1S_2 -- If XTS = 0, XT1 = VLOCLK ; If XTS = 1, XT1 = 3 - 16-MHz crystal or resonator
- * XCAP_1 -- ~6 pF
- */
- BCSCTL3 = XT2S_0 | LFXT1S_2 | XCAP_1;
- /* USER CODE START (section: BCSplus_graceInit_epilogue) */
- /* User code */
- /* USER CODE END (section: BCSplus_graceInit_epilogue) */
- }
- /*void UpdataMotorSpeed(unsigned char channel,unsigned int MotorSpeed)
- {
- unsigned char SpeedH,SpeedL;
- SpeedL=(MotorSpeed);
- SpeedH=MotorSpeed>>=8;
- UARTPutChar(0x88);
- UARTPutChar(channel);
- UARTPutChar(2);
- UARTPutChar(SpeedH);
- UARTPutChar(SpeedL);
- UARTPutChar(SpeedL+SpeedH+0x88+0xa1+2);
- }*/
- //ccr1对应着P21的输出
- //CCR2对应着P24的输出//
- void Timer1_A3_graceInit(void)
- {
-
-
- P2DIR |= BIT1+BIT4; //设置为输出方向//
- P2OUT &= ~(BIT1+BIT4);
- P2SEL |= BIT1+BIT4; //选择为第二功能//
-
- /* USER CODE START (section: Timer1_A3_graceInit_prologue) */
- /* User initialization code */
- /* USER CODE END (section: Timer1_A3_graceInit_prologue) */
-
- /*
- * TA1CCTL1, Capture/Compare Control Register 1
- *
- * CM_0 -- No Capture
- * CCIS_0 -- CCIxA
- * ~SCS -- Asynchronous Capture
- * ~SCCI -- Latched capture signal (read)
- * ~CAP -- Compare mode
- * OUTMOD_7 -- PWM output mode: 7 - PWM reset/set
- *
- * Note: ~<BIT> indicates that <BIT> has value zero
- */
- TA1CCTL1 = CM_0 | CCIS_0 | OUTMOD_7;
- /*
- * TA1CCTL2, Capture/Compare Control Register 2
- *
- * CM_0 -- No Capture
- * CCIS_0 -- CCIxA
- * ~SCS -- Asynchronous Capture
- * ~SCCI -- Latched capture signal (read)
- * ~CAP -- Compare mode
- * OUTMOD_7 -- PWM output mode: 7 - PWM reset/set
- *
- * Note: ~<BIT> indicates that <BIT> has value zero
- */
- TA1CCTL2 = CM_0 | CCIS_0 | OUTMOD_7;
- /* TA1CCR0, Timer_A Capture/Compare Register 0 */
- TA1CCR0 = 15999;
- /* TA1CCR1, Timer_A Capture/Compare Register 1 */
- //TA1CCR1 = 16000;
- TA1CCR1 = 1600;
- /* TA1CCR2, Timer_A Capture/Compare Register 2 */
- TA1CCR2 = 15999;
- /*
- * TA1CTL, Timer_A3 Control Register
- *
- * TASSEL_2 -- SMCLK
- * ID_0 -- Divider - /1
- * MC_1 -- Up Mode
- */
- TA1CTL = TASSEL_2 | ID_0 | MC_1;
- /* USER CODE START (section: Timer1_A3_graceInit_epilogue) */
- /* User code */
- /* USER CODE END (section: Timer1_A3_graceInit_epilogue) */
- }
- //------------------------------------------------------------------------------//
- //PID控制电机结构体//
- typedef struct PID {
- int SetPoint; // 设定目标 Desired Value
- int Proportion; // 比例常数 Proportional Const
- int Integral; // 积分常数 Integral Const
- int Derivative; // 微分常数 Derivative Const
- int LastError; // Error[-1]
- int PrevError; // Error[-2]
- int SumError; // Sums of Errors
- } PID;
- PID LeftPID,RightPID; // LeftMotor PID Control Structure
- //---------------------------------------------//
- //PID计算//
- int PIDCalc( PID *pp, int NextPoint )
- {
- static int dError,Error;
-
- Error = pp->SetPoint - NextPoint; // 偏差
-
- pp->SumError += Error; // 积分
-
- dError = pp->LastError - pp->PrevError; // 当前微分
-
- pp->PrevError = pp->LastError;
- pp->LastError = Error;
-
- return (pp->Proportion * Error // 比例项
- + pp->Integral * pp->SumError // 积分项
- + pp->Derivative * dError // 微分项
- );
- }
- //----------------------------------------//
- void PIDInit (PID *pp)
- {
- memset ( pp,0,sizeof(PID));//清零PID存储//
- }
- //-----------------------------------------//
- //--------------------------------------------------------------------------------//
- int main( void )
- {
- // unsigned int CheckUart;
-
- WDTplus_graceInit();
- BCSplus_graceInit();
- UartInit();
- //MPU6050Init();
- Timer1_A3_graceInit();
- InitMotorControlIO();
- LeftMotorReverse;
- RightMotorReverse;
-
- PIDInit ( &LeftPID ); // Initialize Structure
- PIDInit (&RightPID); // Initialize Structure
- LeftPID.Proportion =900; // Set PID Coefficients
- LeftPID.Integral = 50;
- LeftPID.Derivative = 0;
- LeftPID.SetPoint = 1; // Set LeftMotor PID Setpoint
-
- RightPID.Proportion =900;
- RightPID.Integral =50;
- RightPID.Derivative =0;
- RightPID.SetPoint =2;
-
- P1DIR |= BIT0; //P10设置为输出//
- P1DIR &= ~BIT3; //P13设置为输入//
- P1DIR &= ~BIT4; //P14设置为输入//
- P1IES |= BIT3; // 上升沿中断 P1IES &= ~BIT3;下降沿中断//
- P1IES |= BIT4; //P14上升沿中断//
- P1SEL &= ~BIT3; //P13选择为IO//
- P1SEL &= ~BIT4;
- P1REN |= BIT3; //开启上拉//
- P1REN |= BIT4; //p14开启上拉//
- //P1IE |= BIT3; //中断使能//
- P1IFG =0; //中断标志位清零//
- _EINT(); //全局中断使能//
- while(1)
- {
- Channel_monitor(0xa1,SystemLeftSpeed*400,SystemRightSpeed*400);
- Channel_monitor(0xa2,LeftPIDOut,RightPIDOut);
- }
- }
- #pragma vector=PORT1_VECTOR
- __interrupt void Port_1(void)
- {
- if(EnableMeasureSpeed==BIT0)
- {
- switch(P1IFG)
- {
- case BIT3: //左轮速度输出脉冲接P13//
- {
- LeftMotorSpeed++;
- P1OUT ^= BIT0;
- P1IFG =0; // 清除中断标志//
- break;
- }
- case BIT4: //右轮速度输出脉冲接P14//
- {
- RightMotorSpeed++;
- P1OUT ^= BIT0;
- P1IFG =0; // 清除中断标志//
- break;
- }
- case (BIT3+BIT4): //同时中断//
- {
- LeftMotorSpeed++;
- RightMotorSpeed++;
- P1IFG =0; // 清除中断标志//
- break;
- }
- default:break;
- }
- }
- P1IFG =0; // 清除中断标志//
- }
- //每1ms中断//
- #pragma vector=WDT_VECTOR
- __interrupt void WDT_ISR_HOOK(void)
- {
- ms10++;
- second++;
- ms11++;
- if(ms10==10)
- {
- //P1OUT ^= BIT0;
- ms10=0;
- EnableMeasureSpeed^=BIT0;
- if(EnableMeasureSpeed==BIT0) //开启测速//
- {
- LeftMotorSpeed=0; //清零左轮速度//
- RightMotorSpeed=0; //清零右轮速度//
- P1IE |= BIT3; //中断使能//
- P1IE |= BIT4;
- }
- else
- if(EnableMeasureSpeed==0) //测速完成//
- {
- SystemLeftSpeed=LeftMotorSpeed; //更新左轮速度值//
- SystemRightSpeed=RightMotorSpeed; //更新右轮速度值//
- //Updata_Left(SystemLeftSpeed);
- LeftMotorSpeed=0; //清空左轮速度缓冲//
- RightMotorSpeed=0; //清空右轮速度缓冲//
- P1IE &=~(BIT3+BIT4); //停止测速,直至下一个测速周期//
- }
- }
-
- if(second==1000) //一秒中断执行程序//
- {
- second=0;
- }
-
- if(ms11==11)
- {
- ms11=0;
- LeftPIDOut =PIDCalc(&LeftPID,SystemLeftSpeed);
- RightPIDOut =PIDCalc(&RightPID,SystemRightSpeed);
-
- if(LeftPIDOut>=16000)
- TA1CCR2=16000;
- if(LeftPIDOut<=0)
- TA1CCR2=0;
- else
- {
- TA1CCR2=LeftPIDOut;
- }
-
- if(RightPIDOut>=16000)
- TA1CCR1=16000;
- if(RightPIDOut<=0)
- TA1CCR1=0;
- else
- {
- TA1CCR1=RightPIDOut;
- }
- }
- }
复制代码
|
|