巡線車程式（完整版，也是最先的版本）
阿新 • • 發佈：2019-02-02
#ifndef _Macro.h_ #define _Macro.h_ #include <msp430x14x.h> #include <intrinsics.h> #define uchar unsigned char #define uint unsigned int #define one 11.11 #define LMAX 1999 #define RMAX 3999 #define CPU_F ((double)8000000) #define delay_us(x) __delay_cycles((long)(CPU_F*(double)x/1000000.0)) #define delay_ms(x) __delay_cycles((long)(CPU_F*(double)x/1000.0)) #define PC 20 // 比例放大係數 #define IC 0 //積分放大係數 #define DC 85 //大系數 #define LEFTOUT TACCR1 #define RIGHTOUT TACCR2 #define SensorIn P5IN #define F 5000//5000hz #define Period (8000000/F) #define EnableLeftPos P3OUT|=BIT1 #define UnenableLeftPos P3OUT&=~BIT1 #define EnableLeftNeg P3OUT|=BIT0 #define UnenableLeftNeg P3OUT&=~BIT0 #define EnableRightPos P3OUT|=BIT2 #define UnenableRightPos P3OUT&=~BIT2 #define EnableRightNeg P3OUT|=BIT3 #define UnenableRightNeg P3OUT&=~BIT3 #define Basic_Left 100//百分之八十 #define Basic_Right 100//Basic_Left #define MAX (100) #define MIN (-100) #define foreward 1 #define backward 0 #define max_speed 100 #define min_speed -100 #define key 0 #define left_1 1 #define left_2 2 #define left_3 3 #define left_4 4 #define left_5 5 #define left_6 6 #define left_7 7//右直角 #define right_1 -1 #define right_2 -2 #define right_3 -3 #define right_4 -4 #define right_5 -5 #define right_6 -6 #define right_7 -7//左直角 #endif
#include "Macro.h" #include "sensor.h" void Motorstop() { LEFTOUT=0; RIGHTOUT=0; } void MotorLeft(int speed,int direction) { if(speed>max_speed)speed=max_speed; if(direction==backward)//反轉 { EnableLeftNeg; UnenableLeftPos; } else if(direction==foreward)//正轉 { EnableLeftPos; UnenableLeftNeg; } LEFTOUT=Period/100*speed; } void MotorRight(int speed,int direction) { if(speed>max_speed)speed=max_speed; if(direction==backward)//反轉 { EnableRightNeg; UnenableRightPos; } else if(direction==foreward)//正轉 { EnableRightPos; UnenableRightNeg; } RIGHTOUT=Period/100*speed; } void MotorDrive(int PIDout) { int speedleft,speedright; speedleft=Basic_Left+PIDout; speedright=Basic_Right-PIDout; if(speedleft<0) MotorLeft(speedleft,backward);//反轉 else MotorLeft(speedleft,foreward);//正轉 if(speedright<0) MotorRight(speedright,backward);//反轉 else MotorRight(speedright,foreward);//正轉 } void Rangle(float angle) { // TBCTL|=TBCLR; TBCCR1=LMAX+(unsigned int)(angle*one); }
//下面是小車的程式。。用定時器A來輸出兩路PWM波。 //選用輸出模式7（首先輸出高電平。在TAR=CCRX時，電平自動變低。這樣可以輸出任意佔空比的PWM波） // 感測器接在P5口，電機的驅動接在P3口。 #include "Macro.h" #include "motor.h" typedef struct p { float error_1;//上次的誤差 float sumerror; }PID; PID Pid; PID *pid=&Pid; uchar detection,sample=0,corner,k,flag,tt=0; int weight1[]={-8,-5,-3,-2,0,2,3,5,8};//感測器的權值 float sensorin; int num,tag=1; void IO_inti()//io口初始化 { P1SEL|=BIT2+BIT3;//p1為定時器輸出端。 P1DIR|=BIT2+BIT3;//設定為輸出模式。P5為感測器輸入。 P3DIR|=BIT0+BIT1+BIT2+BIT3+BIT4;//0,1,2,3,作為電機控制的輸出端,其他的作為感測器的輸入端 P5DIR=0X00;//P5為感測器輸入端。設定P5為輸入端。普通IO口。 P5SEL=0x00; P2DIR=0X00;//輸入 P2IE|=BIT3; P2IES|=BIT3; P6DIR=0XFF;//純粹是為了除錯用的。 P6OUT=0X00; } void delay(int t) { unsigned int i; while(t--) { i=65535; while(i--); } } void CLK_inti()//時鐘初始化 { BCSCTL1=0X00;//開啟XT2 do{ IFG1&=~OFIFG; for(int i=0x20;i>0;i--); }while((IFG1&OFIFG)==OFIFG);//如果起震失敗。。繼續起震。知道成功為止 BCSCTL2=0X00; BCSCTL2|=SELM_2|SELS;//mclk,sMCLK的時鐘源為XT2，0分頻。 } void PID_inti()//PID初始化 { pid->error_1=0; pid->sumerror=0; } void PWM_inti()//PWM初始化 { TACTL|=TASSEL_2|ID_0|MC_1|TACLR;//時鐘源採用SMCLK,增計數模式。清空TAR，0分頻 TACCR0=8000000/F;//0分頻。所以Period=8000000/5000 TACCTL1|=OUTMOD_7;//兩個PWM輸出口的輸出模式。 TACCTL2|=OUTMOD_7;// LEFTOUT=0; RIGHTOUT=0; } float abs(float a) { return a<0?-a:a; } void timer_inti()//用定時器B1來確定取樣週期 { TBCTL|=TBSSEL_2|CNTL_0|ID_3|MC_1|TBCLR;//定時器B：SMCLK,16位，8分頻,增計數 TBCCTL0|=CCIE;//使能timerb1的中斷。 TBCCR0=5000;//八分頻，1MHz，5000表示5ms。 _EINT();//開啟總中斷。 } void num_inti() { sensorin=0; num=0; } void inti()//總初始化函式 { PID_inti(); CLK_inti(); PWM_inti(); IO_inti(); timer_inti(); num_inti(); } float ReadSensor2() { int state=0,i,num=0; float sum=0; static float output=0; state=P3IN&0X80; state<<=1; state|=P5IN; for(i=0;i<9;i++) if(((1<<i)&state)==0) { sum+=weight1[i]; num++; } if(num>0&&num<7)//不是全白或者全黑 { corner=0; if(num>=3) { corner=1; if(sum>0) output=10; else output=-10; return output; } output=sum/num; } else if(num>=7)//如果是全黑。 { flag++; return output=0; } return output; } void Avoid(int time,int lspeed,int ldirection,int rspeed,int rdirection) { int state=0; MotorLeft(lspeed,ldirection);//左轉出去，n delay(time); MotorRight(lspeed,ldirection); MotorLeft(rspeed,rdirection); do{//右轉，直到感測器壓線。 state=P3IN&0X80; state<<=1; state|=P5IN; }while(state==0X1FF); PID_inti();//PID初始化 } int PIDCal(float error)//PID計算。位置式。 { float output,derror; pid->sumerror+=error; derror=(error-pid->error_1); output=error*PC+derror*DC; pid->error_1=error; return (int)(output+0.5); } void main( void ) { WDTCTL=WDTHOLD+WDTPW;//close the watch dog; inti(); sample=0; corner=0; flag=0; detection=0; while(1) { if(sample) { sensorin=ReadSensor2(); if(flag==1)//下坡，用定時器定時。到坡底後減速，只有第一次掃到黑線後才要減速。 { // P6OUT=0X81; WDTCTL=(WDTPW+WDTTMSEL+WDTCNTCL); IE1|=WDTIE; flag++; continue; } if(corner)//如果是直角， { int state=0; MotorLeft(100,1);//向前衝一段時間，然後再轉。避免誤判 MotorRight(100,1); delay_ms(20); if(sensorin>0)//如果是右直角 { MotorLeft(100,1); MotorRight(30,0); } else//如果是左直角 { MotorRight(100,1); MotorLeft(30,0); } do { state=0; state=P3IN&0X80; state<<=1; state|=P5IN; }while(state==0x1ff); continue; } MotorDrive(PIDCal(sensorin)); sample=0; } else if(!(P2IN&0X10)&&!detection)//壁障 { detection=1; P6OUT=0XF0; MotorLeft(100,0); MotorRight(100,0); delay_ms(50); if(pid->sumerror<0) Avoid(5,60,0,100,1);//先左轉，再右轉 else Avoid(5,100,1,60,0);//先右轉，再左轉 } } } #pragma vector=TIMERB0_VECTOR __interrupt void TimerB()//控制取樣週期，大概10ms,中斷正常。。 { sample=1; } #pragma vector=PORT2_VECTOR//用外部中斷來判斷並且處理避障 __interrupt void Outside()//最後的抓取。用定時器B才給舵機輸出PWM波 { if(P2IFG&BIT3) { P2IFG&=~BIT3; // P6OUT=0X07; TBCTL|=TBSSEL_2|CNTL_0|ID_3|MC_1|TBCLR|TBIE;//定時器B：SMCLK,16位，4分頻,增計數 TBCCR0=39999;//0分頻。所以Period=8000000/500=16000 TBCCTL1|=OUTMOD_7;//兩個PWM輸出口的輸出模式。 TBCCR1=0; P4DIR|=BIT1; P4SEL|=BIT1; MotorLeft(0,1);//停止 MotorRight(0,1); delay_ms(50);//延遲 Rangle(0);//夾住東西 delay_ms(300); MotorLeft(100,0);//後退 MotorRight(100,0); delay_ms(200);//延遲 MotorLeft(0,0); MotorRight(0,0); while(1);//死迴圈，結束。 } } #pragma vector=WDT_VECTOR __interrupt void wdtch() { static int xx=0; if(++xx==300) { // P6OUT=0x01; MotorLeft(100,0); MotorRight(100,0); delay_ms(160); WDTCTL=WDTHOLD+WDTPW; IE1&=~WDTIE; xx=0; tag=0; } }