Lecture 11/28 – ABB Car
FC Tien, YP Liu
Dept. IE&M, Taipei Tech
ABB Car
安裝控制板
安裝車體
安裝伺服馬達
安裝電池盒
安裝輪子
完成
伺服馬達接線
ABB Car Servo 旋轉的問題
直線行走
左右輪旋轉方向相反,左右輪速度差異,需補償修正
轉彎
轉彎角度控制
ABB Car 藍牙遙控行走
+3.3V GND P8 P9
+5V GND
P11 Servo
白 紅 黑
+5V GND
P12 Servo
白 紅 黑
Servo Arduino 程式 1/2
前進
後退
左轉
#include <Servo.h>
Servo servoL;
Servo servoR;
int servoPinL = 12;
int servoPinR = 11;
void setup() {
servoL.attach(servoPinL);
servoR.attach(servoPinR);
GoStop();
}
void loop() { GoStraight(10);
delay(3000);
GoBack(10);
delay(3000);
servoL.detach();
servoR.detach();
} void GoStraight(int speed)
{int offset = speed * 0.6;
servoL.writeMicroseconds(1500 + (speed*2)-offset);
servoR.writeMicroseconds(1500 - (speed*2)+offset);
}
void GoBack(int speed)
{servoL.writeMicroseconds(1500 - (speed*2));
servoR.writeMicroseconds(1500 + (speed*2));
}
void GoLeft(int speed)
{servoL.writeMicroseconds(1500 - (speed*2));
servoR.writeMicroseconds(1500 - (speed*2));
}
Test02
Servo Arduino 程式 2/2
右轉
停止
#include <Servo.h>
Servo servoL;
Servo servoR;
int servoPinL = 12;
int servoPinR = 11;
void setup() {
servoL.attach(servoPinL);
servoR.attach(servoPinR);
GoStop();
}
void loop() { GoStraight(10);
delay(3000);
GoBack(10);
delay(3000);
servoL.detach();
servoR.detach();
} void GoRight(int speed)
{servoL.writeMicroseconds(1500 + (speed*2));
servoR.writeMicroseconds(1500 + (speed*2));
}
void GoStop()
{servoL.writeMicroseconds(1500);
servoR.writeMicroseconds(1500);
}
Test02
藍牙 Android 遙控程式 – 蓝牙串口助手增强版
Bluetooth Arduino 程式
void loop() {
if (BT.available()) { cmd = BT.read();
BT.print(cmd);
switch(cmd) { case '1':
GoStraight(defSpeed);
break;
case '2':
GoBack(defSpeed);
break;
case '3':
GoLeft(defSpeed);
break;
case '4':
GoRight(defSpeed);
break;
case '5':
GoStop();
break;
} } }
#include <SoftwareSerial.h>
#include <Servo.h>
#define defSpeed 50 //接收腳, 傳送腳
SoftwareSerial BT(8, 9);
//儲存接收資料的變數 char cmd;
Servo servoL;
Servo servoR;
int servoPinL = 12;
int servoPinR = 11;
void setup() {
// 設定藍牙模組的連線速率 BT.begin(9600);
servoL.attach(servoPinL);
servoR.attach(servoPinR);
GoStop();
}
Test03
ABB Car 控制策略 1/2
間歇控制、連續控制?
switch(cmd) { case '1':
GoStraight(10);
break;
case '2':
GoBack(10);
break;
case '3':
GoLeft(20);
break;
case '4':
GoRight(20);
break;
case '5':
GoStop();
break;
}
void GoStraight(int speed) {nowSpeed += (speed*2);
if (nowSpeed > 200) nowSpeed = 200;
servoL.writeMicroseconds(1500 + nowSpeed);
servoR.writeMicroseconds(1500 - nowSpeed);
}
void GoBack(int speed) {nowSpeed -= (speed*2);
if (nowSpeed < 0) nowSpeed = 0;
servoL.writeMicroseconds(1500 + nowSpeed);
servoR.writeMicroseconds(1500 - nowSpeed);
}
#define servoPinL 12
#define servoPinR 11 //接收腳, 傳送腳
SoftwareSerial BT(8, 9);
//儲存接收資料的變數 char cmd;
Servo servoL;
Servo servoR;
int nowSpeed;
void setup() {
// 設定藍牙模組的連線速率 BT.begin(9600);
servoL.attach(servoPinL);
servoR.attach(servoPinR);
GoStop();
}
Test04
ABB Car 控制策略 2/2
間歇控制、連續控制?
switch(cmd) { case '1':
GoStraight(10);
break;
case '2':
GoBack(10);
break;
case '3':
GoLeft(20);
break;
case '4':
GoRight(20);
break;
case '5':
GoStop();
break;
}
void GoLeft(int speed)
{servoL.writeMicroseconds(1500 - (speed*2));
servoR.writeMicroseconds(1500 - (speed*2));
delay(200);
servoL.writeMicroseconds(1500 + nowSpeed);
servoR.writeMicroseconds(1500 - nowSpeed);
}
void GoRight(int speed)
{servoL.writeMicroseconds(1500 + (speed*2));
servoR.writeMicroseconds(1500 + (speed*2));
delay(200);
servoL.writeMicroseconds(1500 + nowSpeed);
servoR.writeMicroseconds(1500 - nowSpeed);
}
void GoStop() {nowSpeed = 0;
servoL.writeMicroseconds(1500);
servoR.writeMicroseconds(1500);
}
Test04
ABB Car 追蹤行走
紅外線
光敏電阻、光電晶體
ABB Car 追蹤行走 – 光敏電阻
+3.3V GND P8 P9
+5V GND
P11 Servo
白 紅 黑
+5V GND
P12 Servo
白 紅 黑
ABB Car Arduino 紅外線追蹤感應程式
int irDetectL(int irLedPin, int irReceiverPin, long frequency) { tone(irLedPin, frequency, 20); //tone(pin, frequency, duration) delay(2);
int ir = digitalRead(irReceiverPin);
return ir;
}
void ReadSensor() { int level = 20;
int v1;
int v2;
v1 = analogRead(sensorPinL);
v2 = analogRead(sensorPinR);
Serial.print("ReadV :");
Serial.print(v1, DEC);
Serial.print(" ,");
Serial.println(v2, DEC);
}
Test05
ABB Car 追蹤行走的問題
環境光線的影響
開機後自動校正(Calibration) void Calibration(){
int val1 = 0;
int val2 = 0;
delay(2000);
for(int i=0;i<100;i++)
{val1 += analogRead(sensorPinL);
val2 += analogRead(sensorPinR);
delay(20);
CDS1 = val1 / 100; } CDS2 = val2 / 100;
Serial.print("Read :");
Serial.print(CDS1, DEC);
Serial.print(" ,");
Serial.println(CDS2, DEC);
} void ReadSensor()
{ int level = 20;
int v1;
int v2;
v1 = analogRead(sensorPinL) - CDS1;
v2 = analogRead(sensorPinR) - CDS2;
Serial.print("ReadV :");
Serial.print(v1, DEC);
Serial.print(" ,");
Serial.println(v2, DEC);
.. .
ABB Car Arduino 光源追蹤程式 1/2
#include <SoftwareSerial.h>
#include <Servo.h>
#define defSpeed 50
#define servoPinL 12
#define servoPinR 11
#define ledPin 13
#define sensorPinL 1
#define sensorPinR 2 //接收腳, 傳送腳
SoftwareSerial BT(8, 9);
//儲存接收資料的變數 char cmd;
Servo servoL;
Servo servoR;
int turnLeftSpeed;
int turnRightSpeed;
int nowSpeed;
int value;
int CDS1;
int CDS2;
int modeBT;
void setup() {
Serial.begin(9600);
BT.begin(9600);
servoL.attach(servoPinL);
servoR.attach(servoPinR);
GoStop();
pinMode(3, INPUT);
pinMode(4, INPUT);
pinMode(5, OUTPUT);
pinMode(6, OUTPUT);
pinMode(ledPin, OUTPUT);
Calibration();
modeBT = 0;
digitalWrite(ledPin, 1);
}
void Calibration(){
int val1 = 0;
int val2 = 0;
delay(2000);
for(int i=0;i<100;i++)
{val1 += analogRead(sensorPinL);
val2 += analogRead(sensorPinR);
delay(20);
CDS1 = val1 / 100; } CDS2 = val2 / 100;
Serial.print("Read :");
Serial.print(CDS1, DEC);
Serial.print(" ,");
Serial.println(CDS2, DEC);
}
Test06
ABB Car Arduino 光源追蹤程式 2/2
void ReadSensor() {
int level = 20;
int v1;
int v2;
v1 = analogRead(sensorPinL) - CDS1;
v2 = analogRead(sensorPinR) - CDS2;
Serial.print("ReadV :");
Serial.print(v1, DEC);
Serial.print(" ,");
Serial.println(v2, DEC);
if ( (v1 > level) && ( (v1-v2) > level) ) {
GoLeft(10);
Serial.println("Left");
else if ( (v2 > level) && ( (v2-v1) > level) )} {
GoRight(10);
Serial.println("Right");
else if ( (v1 > level) && (v2 > level))} {
GoStraight(10);
Serial.println("Straight");
else} {
GoStop();
Serial.println("Stop");
delay(500);} }
void loop() {
if (modeBT == 0) ReadSensor();
if (BT.available()) { cmd = BT.read();
BT.print(cmd);
switch(cmd) { case '1':
GoStraight(10);
break;
case '2':
GoBack(10);
break;
case '3':
GoLeft(20);
break;
case '4':
GoRight(20);
break;
case '5':
GoStop();
break;
case '8':
modeBT = 1;
break;
case '9':
modeBT = 0;
break;
}} }