C51 小车代码：带舵机和超声波避障功能

#include<reg51.h> #include<intrins.h>

#define uint unsigned int #define uchar unsigned char

sbit Trig = P1^0; sbit Echo = P1^1; sbit Servo = P1^2; sbit IN1 = P2^0; sbit IN2 = P2^1; sbit IN3 = P2^2; sbit IN4 = P2^3;

void delayms(uint xms) { uint i,j; for(i=xms;i>0;i--) for(j=110;j>0;j--); }

void ServoWrite(uchar angle) { uchar i; for(i=0;i<angle;i++) { Servo = 1; delayms(1); Servo = 0; delayms(19); } }

void Forward() { IN1 = 1; IN2 = 0; IN3 = 1; IN4 = 0; }

void Backward() { IN1 = 0; IN2 = 1; IN3 = 0; IN4 = 1; }

void TurnLeft() { IN1 = 0; IN2 = 1; IN3 = 1; IN4 = 0; }

void TurnRight() { IN1 = 1; IN2 = 0; IN3 = 0; IN4 = 1; }

void Stop() { IN1 = 0; IN2 = 0; IN3 = 0; IN4 = 0; }

uint UltrasonicWave() { uint distance; Trig = 0; nop(); nop(); nop(); nop(); Trig = 1; nop(); nop(); nop(); nop(); Trig = 0; while(!Echo); TR0 = 1; while(Echo); TR0 = 0; distance = TH0*256 + TL0; distance = distance/58; return distance; }

void main() { TMOD = 0x09; TH0 = 0; TL0 = 0; ServoWrite(90); while(1) { if(UltrasonicWave()<30) { Stop(); ServoWrite(0); delayms(500); if(UltrasonicWave()>UltrasonicWave()) TurnRight(); else TurnLeft(); delayms(500); ServoWrite(90); } else { Forward(); } } }