#include typedef unsigned char uint8; /* 定义可移植的无符号8位整数关键字 typedef signed char int8; /* 定义可移植的有符号8位整数关键字 typedef unsigned int uint16; /* 定义可移植的无符号16位整数关键字 typedef signed int int16; /* 定义可移植的有符号16位整数关键字 //typedef unsigned char BYTE; //typedef unsigned int WORD; #define uchar unsigned char #define uint unsigned int #define STP 20 // debug step #define PWM_MAX 3500 #define PWM_MIN 200 #define PWM_MAX_BUF (PWM_MAX-10) #define PWM_MIN_BUF (PWM_MIN+10) #define FOSC 12000000L #define BAUD 9600 #define debug //PWM的输出端口 sbit PWM_OUT0=P1^7; sbit PWM_OUT1=P1^6; sbit PWM_OUT2=P1^5; sbit PWM_OUT3=P1^4; sbit PWM_OUT4=P1^3; sbit PWM_OUT5=P1^2; /******************全局变量*************************/ volatile uint16 ad_speed,li_speed; //控制舵机轮转 unsigned char ReData,SenData; bit busy,data_full; int16 u=0; uint16 PWM_Value[6]={1520,1600,1820,1480,1540,1500}; //舵机实际值 uint16 code PWM_STD[6]={1520,1600,1820,1480,1540,1500}; //舵机初值 unsigned char order=1; /***********************函数声明********************/ void UART_Transimit(unsigned char dat); unsigned char UART_Receive(void); void setspeed(uint16,uint16); void debug_act(void); void Init_System(void); void stand_by(int16 (*p)[6],uint8,uint8); void delay_ms(unsigned int); void roll_forward(void); void roll_backward(void); /**********************动作数组*********************/ int16 code stand_walk[11][6]= { /* stand to walk */ {1520,1580,1800,1820,1160,1600} , //{1520,1920,1480,1820,1200,1540}, {1520,1920,1480,1760,1200,1540}, //? {1520,1920,1480,1480,1560,1500}, //?往左略偏 {1220,2140,1520,1480,1560,1500}, {1160,2100,1620,1480,1560,1500}, //{1140,2140,1600,1480,1560,1500}, //7.18改1 {1140,1980,1760,1480,1560,1500}, //太抖7.19 {1100,2040,1760,1480,1560,1500}, {1140,1980,1760,1480,1220,1880}, //?~{1140,1980,1760,1480,1560,1500},(7.18改1) 7.19改 {1140,1980,1760,1480,1260,1840}, //7.19gai2 {1140,1980,1760,1480,1220,1880}, {1520,1580,1800,1480,1220,1880}, //?往右略偏 //{1520,1580,1800,1480,1120,1920}, {1520,1580,1800,1600,1060,1880}, //{1520,1580,1800,1820,1040,1700}, {1520,1580,1800,1820, 960,1780}, {1520,1580,1800,1820,1160,1600} }; int16 code walk_stand[4][6]= //walk to stand { {1520,1580,1800,1880,1180,1500}, //{1520,1920,1480,1820,1200,1540}, {1520,1920,1480,1760,1200,1540}, {1520,1920,1560,1480,1560,1500}, {1520,1580,1800,1480,1560,1500} }; int16 code stand_rf[10][6]= //stand to walk forward { { 540,1600,1840,2380,1540,1500}, { 540,1600,1160,2380,1540,2200}, //{1520,1600,2120,2380,1540,1900}, { 540,1600,1160,2380,1540,1980}, {1520,1600,2120,2380,1540,1980}, {1520,1580,2060,1480,1560,1220}, //改 36 {1540,2520,2060,1480, 680,1220}, //改 2356 7.24 需要改落地 //{2560,2520,2060, 560, 680,1220}, //{2560,2480,2020, 560, 720,1220}, {2500,2520,2140, 640, 620,1120}, {2560,2480,1840, 560, 680,1500}, {2560,1600,1820, 560,1540,1500}, {1520,1600,1820,1480,1540,1500}, //{1540,1600,1840,1480,1540,1500}, }; int16 code stand_rb[11][6]= //stand to roll backward { {2560,1600,1840, 540,1560,1500}, //{2560,2520,1840, 540, 780,1500}, {2560,2460,1840, 580, 780,1420}, {2560,2520,2020, 580, 640,1280}, //{2560,2520,2020,1480, 640,1280}, //{1540,2520,2020,1480, 640,1280}, //{2560,2520,2020,1620, 640,1280}, //{1380,2520,2020,1620, 640,1280}, {2560,2520,2020,1420,1540,1280}, {2560,2520,2020,1620,1540,1280}, {1380,2520,2020,1620,1540,1280}, {1380, 720,1540,1620,2440,1680}, //2356 { 620, 800,1540,2280,2360,1820}, { 600, 800,1800,2360,2360,1500}, { 560,1600,1840,2360,1540,1500}, {1540,1600,1840,1480,1540,1500}, }; /***********************主函数*************************/ void main(void) { // Init_System(); // // delay_ms(5000); //delay time // #ifdef debug // ///*向前走*/ // setspeed(25,15); // //stand_by(stand_walk,0,19); // //stand_by(go_straight,0,23); // delay_ms(200); // debug_act(); //调试函数 // // #endif // // #ifdef debug // delay_ms(500); // UART_Transimit(' '); // UART_Transimit('O'); // UART_Transimit('K'); // UART_Transimit('!'); // UART_Transimit(0x0d); // UART_Transimit(0x0a); // #endif Init_System(); //EA = 1; delay_ms(3500); //delay time #ifdef debug delay_ms(500); UART_Transimit(' '); UART_Transimit('O'); UART_Transimit('K'); UART_Transimit('!'); UART_Transimit(0x0d); UART_Transimit(0x0a); #endif #ifdef debug debug_act(); #endif setspeed(230,15); stand_by(stand_walk,0,11); //old :15 delay_ms(80); //0->27 stand to walk stand_by(walk_stand,0,3); //walk to stand delay_ms(200); // setspeed(200,15);//350 while(1) { stand_by(stand_rf,0,9); //stand to roll foreward delay_ms(300); stand_by(stand_rf,0,9); //stand to roll foreward delay_ms(300); stand_by(stand_rf,0,9); //stand to roll foreward } while(1) { roll_forward(); roll_forward(); roll_forward(); } delay_ms(300); // // // setspeed(230,15); // stand_by(stand_walk,0,11); // delay_ms(90); // stand_by(walk_stand,0,3); //stand to walk // //walk to stand // // // // delay_ms(300); // while(1) { roll_backward(); delay_ms(100); roll_backward(); delay_ms(100); roll_backward(); } // // //setspeed(150,15);//300 // while(1){ // stand_by(stand_rb,0,9); //stand to roll backward // delay_ms(250); // stand_by(stand_rb,0,9); //stand to roll backward // delay_ms(250); // stand_by(stand_rb,0,9); //stand to roll backward // } delay_ms(200); setspeed(230,15); //250 while(1) { stand_by(stand_walk,0,10); delay_ms(100); } } /*****************功能函数*********************/ void stand_by(int16 (*p)[6],uint8 start,uint8 end) { int16 i,j; uint8 num=0; //count ready motors. int16 temp[6]; //store the actions steps uint16 delay; //for action slow for(i=start;i<=end;i++) { u=i; for(j=0;j<6;j++) //store action steps { temp[j]=p[i][j]-PWM_Value[j]; } while(1) { num=6; for(j=0;j<6;j++) //count each motors step { if(temp[j]==0) { num--; continue; } if(temp[j]>0) { (PWM_Value[j])++; temp[j]--; } else { PWM_Value[j]--; temp[j]++; } } if(num==0) break;//If all the motor is ready, break. for(delay=0;delayPWM_MAX_BUF? PWM_MAX:PWM_Value[0]+STP; break; case 'w': //motor 1 inc PWM_Value[1]= PWM_Value[1]PWM_MAX_BUF? PWM_MAX:PWM_Value[1]+STP; break; case 'd': //motor 2 inc PWM_Value[2]= PWM_Value[2]>PWM_MAX_BUF? PWM_MAX:PWM_Value[2]+STP; break; case 'e': //motor 2 dec PWM_Value[2]= PWM_Value[2]PWM_MAX_BUF? PWM_MAX:PWM_Value[3]+STP; break; case 'f': //motor 3 dec PWM_Value[3]= PWM_Value[3]PWM_MAX_BUF? PWM_MAX:PWM_Value[4]+STP; break; case 'y': //motor 5 inc PWM_Value[5]= PWM_Value[5]>PWM_MAX_BUF? PWM_MAX:PWM_Value[5]+STP; break; case 'h': //motor 5 dec PWM_Value[5]= PWM_Value[5]999) UART_Transimit((uint8)(temp/1000)+'0'); else UART_Transimit(' '); temp%=1000; UART_Transimit((uint8)(temp/100)+'0'); temp%=100; UART_Transimit((uint8)(temp/10)+'0'); temp%=10; UART_Transimit((uint8)temp+'0'); if(i!=5) UART_Transimit(','); } UART_Transimit('}'); UART_Transimit(','); UART_Transimit(0x0d); UART_Transimit(0x0a); break; case 'n'://reset for(i=0;i<6;i++) PWM_Value[i]=PWM_STD[i]; break; /**************设置动作数组**************/ case '{': { for(char0=0;char0<6;char0++) { char1=UART_Receive(); if(char1!=' ') tempwm=((int16)(char1-'0'))*1000; else tempwm=0; tempwm+=((int16)(UART_Receive()-'0'))*100; tempwm+=((int16)(UART_Receive()-'0'))*10; tempwm+=(int16)(UART_Receive()-'0'); UART_Receive(); //if(tempwm>2500 || tempwm<300) continue; PWM_Value[char0]=tempwm; } } break; default: break; } } } /*********************************************** function: set speed ***********************************************/ void setspeed(uint16 adjacent_spd,uint16 line_spd) { ad_speed=adjacent_spd; li_speed=line_spd; } /**********12M晶振1ms延时**********************/ void delay_ms(unsigned int n) { uint16 i,k; for(k=n;k>0;k--) for(i=121;i>0;i--); } /****************初始化设置********************/ void Init_System (void) { SCON = 0x50; //REN=1允许串行接受状态,串口工作模式1 AUXR|=0x01; AUXR&=0xFB; T2L=0xE6; T2H=0xFF; AUXR|=0x10; ES = 1; //开串口中断 EA = 1; //开总中断 order=1; TMOD=0x01; TH0=-2500/256; TL0=-2500%256; ET0=1; TR0=1; } /*************************************************** function: Uart transimit funcition ***************************************************/ void UART_Transimit(unsigned char dat) { while(busy); busy = 1; SBUF = dat; } /*************************************************** function: Uart receive funcition ***************************************************/ unsigned char UART_Receive(void) { while(!data_full); data_full = 0; UART_Transimit(ReData); return ReData; } /*=================================================================================== 定时器T0的中断服务程序 一个循环20MS = 8*2.5ms =====================================================================================*/ void timer0(void) interrupt 1 using 3 { switch(order) { case 1: PWM_OUT0=1; TH0=-PWM_Value[0]/256; TL0=-PWM_Value[0]%256; break; case 2: PWM_OUT0=0; TH0=-(3000-PWM_Value[0])/256; TL0=-(3000-PWM_Value[0])%256; break; case 3: PWM_OUT1=1; TH0=-PWM_Value[1]/256; TL0=-PWM_Value[1]%256; break; case 4: PWM_OUT1=0; TH0=-(3000-PWM_Value[1])/256; TL0=-(3000-PWM_Value[1])%256; break; case 5: PWM_OUT2=1; TH0=-PWM_Value[2]/256; TL0=-PWM_Value[2]%256; break; case 6: PWM_OUT2=0 ; TH0=-(3500-PWM_Value[2])/256; TL0=-(3500-PWM_Value[2])%256; break; case 7: PWM_OUT3=1; TH0=-PWM_Value[3]/256; TL0=-PWM_Value[3]%256; break; case 8: PWM_OUT3=0; TH0=-(3500-PWM_Value[3])/256; TL0=-(3500-PWM_Value[3])%256; break; case 9: PWM_OUT4=1; TH0=-PWM_Value[4]/256; TL0=-PWM_Value[4]%256; break; case 10:PWM_OUT4=0; TH0=-(3500-PWM_Value[4])/256; TL0=-(3500-PWM_Value[4])%256; break; case 11:PWM_OUT5=1; TH0=-PWM_Value[5]/256; TL0=-PWM_Value[5]%256; break; case 12:PWM_OUT5=0; order=0; TH0=-(3500-PWM_Value[5])/256; TL0=-(3500-PWM_Value[5])%256; break; default : break; } order++; } /**************************************************** 串口中断程序 ******************************************************/ void Uart_Isr (void) interrupt 4 using 1 { if(RI) //RI接受中断标志 { RI = 0; //清除RI接受中断标志 ReData = SBUF; //SUBF接受/发送缓冲器 data_full=1; //是否接收到数据 } if(TI) { TI = 0; busy = 0; } } void roll_forward(void) //150,170,150 { setspeed(150,15); //350 150 stand_by(stand_rf,0,5); //stand to roll foreward delay_ms(100); setspeed(170,15); stand_by(stand_rf,6,7); delay_ms(150); setspeed(150,15); //250 stand_by(stand_rf,8,9); // delay_ms(100); // setspeed(150,15); //350 // stand_by(stand_rf,5,5); // // // setspeed(250,15); //250 // stand_by(stand_rf,6,6); // delay_ms(100); // stand_by(stand_rf,7,7); // setspeed(150,15); //250 // // // stand_by(stand_rf,8,10); } void roll_backward(void) { setspeed(150,15); //300 stand_by(stand_rb,0,3); delay_ms(30); setspeed(200,15); //300 stand_by(stand_rb,4,5); //delay_ms(1000); //stand_by(stand_rb,4,4); //delay_ms(500); setspeed(130,15); //300 stand_by(stand_rb,6,10); }