前書き
超音波モジュールの測定領域は、ステアリングギアの左右の回転によって制御されます。障害物が発生した場合、つまり検出距離が10cm未満の場合、シャーシは障害物から離れる方向に回転します。
デモ
【Arduinoおもちゃ】見回す障害物回避車
上手
Arduino UNO x 1
超音波レンジングモジュール(HC-SR04)x 1
ステアリングギア(SG 90)x1
赤色LEDx 1
トラックシャーシ(2つのギア付きモーターを含む)(Pololu Zumo)x 1
モーター駆動バージョン(Zumo Shield V1.3) x 1
配線
ArduinoIDEコード
ステアリングギアはATmega328Pのタイマー2を使用しており、コードは少し複雑です。モータードライバボードのライブラリファイル<ZumoShield.h>はタイマー-1を使用しているため、<Servo.h>と競合します。ライブラリファイルを必要としない他のモータードライバーを使用する場合、コードはL298Nなどのはるかに単純になります。
/*
Obstacle Avoidace 避障车
Last Edited: March.18th.2021 by Mun Kim(老金)
Contact: [email protected]
*/
#include <Wire.h>
#include <ZumoShield.h> //马达驱动板需要的库文件
ZumoMotors motors;
#define Echo 2 //HC-SR04 Echo pin//超声波测距模块的“回声”引脚接 D2
#define Trig 3 //HC-SR04 Trig pin//超声波测距模块的“激发”引脚接 D3
#define SERVO 11 //Servo pin//舵机接 D11或 D3,因为要使用 timer-2
#define LED 5 //LED pin//LED接 D5
uint16_t volatile servoTime = 0;
uint16_t volatile servoHighTime = 3000; // Pulse width in units of 0.5us//高电平持续时间
boolean volatile servoHigh = false;
float distance; //distance measured by HC-SR04//超声波模块探测到的距离
void setup(){
Serial.begin(9600);
pinMode(Trig, OUTPUT); pinMode(Echo, INPUT); pinMode(LED, OUTPUT);
servoInit();
}
void loop(){
//Drive Forward//前进*******************************************************************************************
motors.setLeftSpeed(100); motors.setRightSpeed(100); //0-25
//Look Right//向右看*********************************************************************************************
servoSetPosition(1000); //Send 1000us pulses to servo//1000us的脉冲,转动舵机
distance = MeasureDistance(); //HC-SR04//超声波模块测距
while (distance <10 ) {
//turn left, till the obstacle is more than 10cm away
motors.setLeftSpeed(-100); motors.setRightSpeed( 100); //底盘向左转动,直到障碍物处于10cm开外
digitalWrite(LED, HIGH);
distance = MeasureDistance();}
digitalWrite(LED, LOW);
delay(700); //for stable HC-SR04 readings
//700ms毫秒的延迟貌似会让超声波模块的读数稳定一些
//Look Forward//向前看********************************************************************************************
servoSetPosition(1500);
distance = MeasureDistance();
while (distance <15) {
motors.setLeftSpeed(-100); motors.setRightSpeed( 100);
digitalWrite(LED, HIGH);
distance = MeasureDistance();}
digitalWrite(LED, LOW);
delay(700);
//Look Forward//向右看********************************************************************************************
servoSetPosition(2000);
distance = MeasureDistance();
while(distance <10) {
motors.setLeftSpeed( 100); motors.setRightSpeed(-100);
digitalWrite(LED, HIGH);
distance = MeasureDistance();}
digitalWrite(LED, LOW);
delay(700);
//Look Forward//向前看********************************************************************************************
servoSetPosition(1500);
distance = MeasureDistance();
while (distance <15 ) {
motors.setLeftSpeed( 100); motors.setRightSpeed(-100);
digitalWrite(LED, HIGH);
distance = MeasureDistance();}
digitalWrite(LED, LOW);
delay(700);
}
//Functions//函数****************************************************************************************************
float MeasureDistance(){
//测距
float d;
digitalWrite(Trig, LOW); delayMicroseconds(2);
digitalWrite(Trig, HIGH); delayMicroseconds(10);
digitalWrite(Trig, LOW);
d = (pulseIn(Echo, HIGH) / 2) * 343/10000; //测距公式,"d" 的单位是 "cm"
Serial.println(d);
return d; }
//在Timer 2 达到 OCR2A 并重置后启动ISR
ISR(TIMER2_COMPA_vect){
//Timer/Counter2 Compare Match A//中断向量
servoTime += OCR2A + 1;
static uint16_t highTimeCopy = 3000;
static uint8_t interruptCount = 0;
if(servoHigh){
if(++interruptCount == 2){
OCR2A = 255;}
if(servoTime >= highTimeCopy){
//Servo引脚处在高电平状态
digitalWrite(SERVO, LOW); //做一个下降沿
servoHigh = false;
interruptCount = 0;}
}
else{
//Servo引脚处在低电平状态
if(servoTime >= 40000){
highTimeCopy = servoHighTime;
digitalWrite(SERVO, HIGH); //做一个上升沿
servoHigh = true; servoTime = 0; interruptCount = 0;
OCR2A = ((highTimeCopy % 256) + 256)/2 - 1;}
}
}
void servoInit(){
digitalWrite(SERVO, LOW); pinMode(SERVO, OUTPUT);
//开启CTC模式,Timer 2 会增加到 OCR2A 然后重置,造成中断
TCCR2A = (1 << WGM21);
TCCR2B = (1 << CS21); //0.5us分辨率
TCNT2 = 0; OCR2A = 255;
TIMSK2 |= (1 << OCIE2A); // 开启 timer compare interrupt.
sei(); // 开启中断
}
void servoSetPosition(uint16_t highTimeMicroseconds){
TIMSK2 &= ~(1 << OCIE2A); // 关闭 timer compare interrupt.
servoHighTime = highTimeMicroseconds * 2;
TIMSK2 |= (1 << OCIE2A); // 开启 timer compare interrupt.
}
参照
- 割り込み処理チュートリアル
- ATmega328Pデータシート
- Polulu Zumoシャーシ、ドライバーバージョンマニュアル