//this file is a example for motor control board
//Control LeftFrontWheel
int MosfetLeftFrontWheel0 = 8; //Left Front Wheel
int MosfetLeftFrontWheel1 = 9; //Left Front Wheel
int MosfetRightFrontWheel0 = 2; //right front wheel
int MosfetRightFrontWheel1 = 3; //right front wheel
int MosfetLeftTailWheel0 = 4;
int MosfetLeftTailWheel1 = 5;
int MosfetRightTailWheel0 = 6;
int MosfetRightTailWheel1 = 7;
//超声传感器部分
const int TrigPin = 49;
const int EchoPin = 48;
float cm;
#define MAX_CAR_SPAN_CM 40 //CM
void setup() {
pinMode(MosfetLeftFrontWheel0, OUTPUT);
pinMode(MosfetRightFrontWheel0, OUTPUT);
pinMode(MosfetLeftTailWheel0, OUTPUT);
pinMode(MosfetRightTailWheel0, OUTPUT);
pinMode(MosfetLeftFrontWheel1, OUTPUT);
pinMode(MosfetRightFrontWheel1, OUTPUT);
pinMode(MosfetLeftTailWheel1, OUTPUT);
pinMode(MosfetRightTailWheel1, OUTPUT);
//超声传感器部分
Serial.begin(9600);
pinMode(TrigPin, OUTPUT);
pinMode(EchoPin, INPUT);
}
void loop()
{
digitalWrite(TrigPin, LOW); //低高低电平发一个短时间脉冲去TrigPin
delayMicroseconds(2);
digitalWrite(TrigPin, HIGH);
delayMicroseconds(10);
digitalWrite(TrigPin, LOW);
cm = pulseIn(EchoPin, HIGH) / 58.0; //将回波时间换算成cm
cm = (int(cm * 100.0)) / 100.0; //保留两位小数
Serial.print(cm);
Serial.print("cm");
Serial.println();
while(cm>MAX_CAR_SPAN_CM)
{
//init MOSFE Pin
digitalWrite(MosfetLeftFrontWheel0, HIGH);
digitalWrite(MosfetRightFrontWheel0, HIGH);
digitalWrite(MosfetLeftTailWheel0, HIGH);
digitalWrite(MosfetRightTailWheel0, HIGH);
digitalWrite(MosfetLeftFrontWheel1, LOW);
digitalWrite(MosfetRightFrontWheel1, LOW);
digitalWrite(MosfetLeftTailWheel1, LOW);
digitalWrite(MosfetRightTailWheel1, LOW);
delay(200);
//停车
digitalWrite(MosfetLeftFrontWheel0, LOW);
digitalWrite(MosfetRightFrontWheel0, LOW);
digitalWrite(MosfetLeftTailWheel0, LOW);
digitalWrite(MosfetRightTailWheel0, LOW);
digitalWrite(MosfetLeftFrontWheel1, LOW);
digitalWrite(MosfetRightFrontWheel1, LOW);
digitalWrite(MosfetLeftTailWheel1, LOW);
digitalWrite(MosfetRightTailWheel1, LOW);
//delay(200);
digitalWrite(TrigPin, LOW); //低高低电平发一个短时间脉冲去TrigPin
delayMicroseconds(2);
digitalWrite(TrigPin, HIGH);
delayMicroseconds(10);
digitalWrite(TrigPin, LOW);
cm = pulseIn(EchoPin, HIGH) / 58.0; //将回波时间换算成cm
cm = (int(cm * 100.0)) / 100.0; //保留两位小数
Serial.print(cm);
Serial.print("cm");
Serial.println();
}
//前方有障碍物,转弯
if(cm<=MAX_CAR_SPAN_CM)
{
digitalWrite(MosfetLeftFrontWheel0, LOW);
//digitalWrite(MosfetRightFrontWheel0, LOW);
// digitalWrite(MosfetLeftTailWheel0, LOW);
digitalWrite(MosfetRightTailWheel0, LOW);
digitalWrite(MosfetLeftFrontWheel1, HIGH);
// digitalWrite(MosfetRightFrontWheel1, HIGH);
// digitalWrite(MosfetLeftTailWheel1, HIGH);
digitalWrite(MosfetRightTailWheel1, HIGH);
delay(200);
}
}