#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; }}
