基于MSP430的悬挂控制系统报告(3)

附录 系统完整程序 函数主程序：

#include \#include\

unsigned char table[8]={0X04 , 0x14 , 0x10 , 0x12 , 0x02 , 0x0a , 0x08 , 0x0c}; void init(void);

void draw_line1(float x0,float y0,float x1,float y1);

void draw(float x0,float y0,float x1,float y1);

void draw_cycle(float r,float x0,float y0); void motor_step(float lx,float ly); void left_1(unsigned int count); void left_0(unsigned int count); void right_1(unsigned int count); void right_0(unsigned int count); extern float zx,zy,ox,oy,qx,qy; float rx,lx; float

length_r=0.117,length_l=0.117,pai=3.14159; int dir_l,dir_r; int main( void ) {

// Stop watchdog timer to prevent time out reset

WDTCTL = WDTPW + WDTHOLD; init(); Init_Port1(); Init_Lcd_12864(); set_up();

Display_now_lcd12864();

draw_cycle( 10, ox, oy);//画圆,按键设定圆心与半径

//draw(qx,qy,zx,zy);//画线，按键设定起点与终点

//draw(25.0,50.0,40.0,10.0); //draw(40.0,10.0,10.0,40.0); //draw(10.0,40.0,40.0,40.0); //draw(40.0,40.0,10.0,10.0); }

void init(void)

10

{

char i=0;

BCSCTL1 &=~XT2OFF;// 开启 XT2 do {

IFG1 &= ~OFIFG;

for (i = 0xFF; i > 0; i--); }

while( (IFG1&OFIFG) ); //XT2异常

BCSCTL2=SELM_2+SELS; //MCLK = SMCLK = XT2 }

void draw_line1(float x0,float y0,float x1,float y1) {

float l0,l1,r0,r1,dl,dr,dl0,dr0;

l0=sqrt((x0+5)*(x0+5)+(107-y0)*(107-y0)); l1=sqrt((5+x1)*(5+x1)+(107-y1)*(107-y1));

r0=sqrt((85-x0)*(85-x0)+(107-y0)*(107-y0));

r1=sqrt((85-x1)*(85-x1)+(107-y1)*(107-y1)); dl=l1-l0; dr=r1-r0; if(dl>0) {

dir_l=1; //表示长度增加，为反转 dl0=(dl); } else {

dir_l=-1; //表示长度减小，为正转 dl0=-(dl); } if(dr>0) {

dir_r=1; //表示长度增加，为反转 dr0=dr; } else {

dir_r=-1; //表示长度减小，为正转

dr0=-(dr); }

motor_step(dl0,dr0); }

void motor_step(float lx,float rx) {

unsigned int step_l,step_r,first ,second,kv; unsigned int i,j;

LEFT_down(1); }

RIGHT_down(second); } } }

if((dir_l==1)&&(dir_r==-1)) {

if(step_l>=step_r) {

step_l=( int)(lx/length_l+(dir_l?0.5:-0.5));

step_r=( int)(rx/length_r+(dir_r?0.5:-0.5)); if(step_l>=step_r) {

first=step_r;

second=step_l%step_r; kv=step_l/step_r; } else {

first=step_l;

second=step_r%step_l; kv=step_r/step_l; }

if((dir_l==1)&&(dir_r==1)) {

if(step_l>=step_r) {

for(j=0;j<8;j++) {

for(i=0;i

RIGHT_down(1); LEFT_down(kv); }

LEFT_down(second); }} else {

for(j=0;j<10;j++) {

for(i=0;i

RIGHT_down(kv);

11

for(j=0;j<10;j++) {

for(i=0;i

LEFT_down(kv); RIGHT_up(1); }

LEFT_down(second); }} else {

for(j=0;j<10;j++) {

for(i=0;i

LEFT_down(1); RIGHT_up(kv); }

RIGHT_up(second); } }}

if((dir_l==-1)&&(dir_r==1)) {

if(step_l>=step_r) {

for(j=0;j<10;j++) {

for(i=0;i

LEFT_up(kv); RIGHT_down(1); }

LEFT_up(second); }} else

{

for(j=0;j<10;j++) {

for(i=0;i

LEFT_up(1); RIGHT_down(kv); }

RIGHT_down(second); } } }

if((dir_l==-1)&&(dir_r==-1)) {

if(step_l>=step_r) {

for(j=0;j<10;j++) {

for(i=0;i

LEFT_up(kv); RIGHT_up(1); }

LEFT_up(second); } } else {

for(j=0;j<10;j++) {

for(i=0;i

LEFT_up(1); RIGHT_up(kv); }

RIGHT_up(second); } } } }

void draw(float x0,float y0,float x1,float y1) {

float inc_x,inc_y,x0_0,y0_0,x2,y2; int i=20; inc_x=(x1-x0)/i;

12

inc_y=(y1-y0)/i; x0_0=x0; y0_0=y0; while(i--) {

x2=x0_0+inc_x; y2=y0_0+inc_y;

draw_line1( x0_0, y0_0, x2, y2);

Display_Hex2Dec_Lcd_12864(6,8,(uint)x2); Display_Hex2Dec_Lcd_12864(6,12,(uint)x2); x0_0=x2; y0_0=y2; } }

void draw_cycle(float r,float x0,float y0) { int i;

float now_x,now_y,next_x,next_y; now_x=x0; now_y=y0-r;

for(i=-90.0;i<=275.0;i+=5) {

if((i>=-90.0)&(i<0.0)) {

next_x=r*cos(i*pai/180.0)+x0; next_y=r*(sin(i*pai/180.0))+y0; }

if((i>=0.0)&(i<90.0)) {

next_x=r*(cos(i*pai/180.0))+x0; next_y=r*sin(i*pai/180.0)+y0; }

if((i>=90.0)&(i<180.0)) {

next_x=r*cos(i*pai/180.0)+x0; next_y=r*(sin(i*pai/180.0))+y0; }

if((i>=180.0)&(i<=275.0)) {

next_x=r*(cos(i*pai/180.0))+x0; next_y=r*sin(i*pai/180.0)+y0; }

draw_line1(now_x,now_y,next_x,next_y);

Display_Hex2Dec_Lcd_12864(6,8,(uint)next_x);

Display_Hex2Dec_Lcd_12864(6,12,(uint)next_y);

now_x=next_x; now_y=next_y; } }

/******************说明 p3口送两个电机的数据 p2使能驱动

测试左右电机收放线的情况 ********************/

#include \#include \

#define CPU_F ((double)8000000) #defindelay_nus(x)__delay_cycles((long)(CPU_F*(double)x/1000000.0));

#definedelay_nms(x)__delay_cycles((long)(CPU_F*(double)x/1000.0));

unsigned int Lcount,Rcount,Bothcount; unsigned xa0,0x80,0x90}; unsigned x0a,0x08,0x09}; unsigned

2,0xaa,0x88,0x99};

13

char

Bothcontrolstep[8]={0x11,0x55,0x44,0x66,0x2

char

Lcontrolstep[8]={0x01,0x05,0x04,0x06,0x02,0

char

Rcontrolstep[8]={0x10,0x50,0x40,0x60,0x20,0

void LEFT_down(unsigned int count) //左边电机放线 {

unsigned int i; for(i=1;i<=count;i++) {

if(Lcount==7)Lcount=0; else Lcount++;

P3OUT|=Lcontrolstep[Lcount]; P3OUT&=Lcontrolstep[Lcount]|0Xf0; delay_nms(1); } }

void LEFT_up(unsigned int count) //左边电机收线 {

unsigned int i; for(i=1;i<=count;i++) {

if(Lcount==0)Lcount=7; else Lcount--;

P3OUT|=Lcontrolstep[Lcount];

P3OUT&=Lcontrolstep[Lcount]|0XF0; delay_nms(3); } }

void RIGHT_down(unsigned int count) //右边电机放线 {

unsigned int i; for(i=1;i<=count;i++) {

if(Rcount==7)Rcount=0; else Rcount++;

P3OUT|=Rcontrolstep[Rcount]; P3OUT&=Rcontrolstep[Rcount]|0X0F; delay_nms(3); } }

void RIGHT_up(unsigned int count) //右边电机收线

{

unsigned int i; for(i=1;i<=count;i++) {

if(Rcount==0)Rcount=7; else Rcount--;

P3OUT|=Rcontrolstep[Rcount]; P3OUT&=Rcontrolstep[Rcount]|0X0F; delay_nms(3); } }

void Together_down(unsigned int count) //左右边电机收线 {

unsigned int i; for(i=1;i<=count;i++) {

if(Bothcount==0)Bothcount=7; else Bothcount--;

P3OUT|=Bothcontrolstep[Bothcount];

P3OUT&=Bothcontrolstep[Bothcount]|0X00; delay_nms(1); } }

/*按键程序*/

#include\#include\/*

端口是P1.0-P1.5 */ /*

KEY6 --------------------P1.0 KEY5 --------------------P1.1 KEY4 --------------------P1.2 kEY3---------------------P1.3 KEY2 --------------------P1.4

KEY1--------------------P1.5 */

float ox,oy,zx,zy,qx,qy; int key_num,num_x,num_y; void set_up() {

while(P1IE); }

#pragma vector=PORT1_VECTOR __interrupt void Port_1() {

delays(); switch(P1IFG) {

case 0x01:

key_num++; key_show();break; case 0x02:

key_num--; key_num+=4; key_show();break; case 0x04:num_x=5;

key_show();break; case 0x08:num_y=5;

key_show();break; case 0x10:num_x=-5; num_y=-5; key_show();break; case 0x20:P1IE&=~0XFF;break; default: break; }

P1IFG&=~0XFF; }

void delays() {

uint i,j;

for(i=500;i>0;i--) for(j=1000;j>0;j--); }

void key_show() {

key_num=key_num%4; Clr_Screen_Lcd_12864();

14