通过前边对于电机驱动部分以及超声波测距部分的调试，今天做了一个避障的测试，效果还不错哦。
代码如下：
- [code]
- //************* Sensor Test **********************//
- // Variables defination
- int SenTrig = 2;
- int SenEcho = 3;
- float Distance = 0;
- // PWM pin 4&5 to drive motor 1
- // PWM pin 6&7 to drive motor 2
- int Dir1 = 4;
- int Spd1 = 5;
- int Dir2 = 7;
- int Spd2 = 6;
- // System initialize
- void setup()
- {
- // Configure I/O port
- pinMode(SenTrig, OUTPUT);
- pinMode(SenEcho, INPUT);
-
- // Initialize I/O port
- digitalWrite(SenTrig, LOW);
-
- // Congfigure Motor1
- pinMode(Dir1, OUTPUT);
- pinMode(Spd1, OUTPUT);
-
- // Congfigure Motor2
- pinMode(Dir2, OUTPUT);
- pinMode(Spd2, OUTPUT);
- }
- void loop()
- {
- // Triger the sensor
- delayMicroseconds(3);
- digitalWrite(SenTrig, HIGH);
- delayMicroseconds(12);
- digitalWrite(SenTrig, LOW);
- // Read the echo
- Distance = pulseIn(SenEcho, HIGH);
- Distance = Distance/5.8; // 5.8 is the convert ratio to mm
- // Run forward
- if(Distance >= 60)
- {
- digitalWrite(Dir1, LOW);
- analogWrite(Spd1, 980);
- digitalWrite(Dir2, LOW);
- analogWrite(Spd2, 1023);
- }
- else
- {
- analogWrite(Spd1, 0);
- analogWrite(Spd2, 0);
- delay(1000); // Stop 1000mS
-
- digitalWrite(Dir1, HIGH);
- analogWrite(Spd1, 980);
- digitalWrite(Dir2, HIGH);
- analogWrite(Spd2, 1023);
- delay(600);
-
-
- digitalWrite(Dir1,HIGH);
- analogWrite(Spd1,980);
- digitalWrite(Dir2,LOW);
- analogWrite(Spd2, 1023);
- delay(500); // Turn right 500mS
- }
- }
- //**************** End of file ******************//
[/code]试验中遇到的问题总结如下：
1.两个电机的一致性不是很好，所以要通过调节调速参数来使小车可以直着行驶
2.电机的驱动电压即PWM的占空比不能低于40%，否则电机会堵转。
