owned this note
owned this note
Published
Linked with GitHub
# 2023/05/11 MicroBit 線迷宮自走車-程式實作
[TOC]
## 自走車主板電路解析
### 電池充電/保護線路
### 5V 升壓電路
### 3.3V 穩壓線路
### 馬達驅動線路
### MicroBit 接線圖
## 紅外線循跡感測板
---
## Arduino IDE/MicroBit開發環境安裝
### 安裝Arduino IDE/MicroBit 函式庫
**MicroBit 開發板管理員安裝說明連結:**
https://hackmd.io/u8hCtMgnSXamJ6mguK2GkQ?view
---
### MicroBit 接腳定義
---
### V1 及V2 接腳功能比較
### MicroBit V2 測試程式
#### 5X5 LED 矩陣接線圖
---
#### LED 閃爍程式碼
```cpp=
const int COL1 = 4; // Column #1 control
const int LED = 21; // 'row 1' led
void setup() {
Serial.begin(9600);
Serial.println("microbit is ready!");
// because the LEDs are multiplexed, we must ground the opposite side of the LED
pinMode(COL1, OUTPUT);
digitalWrite(COL1, LOW);
pinMode(LED, OUTPUT);
}
void loop(){
Serial.println("blink!");
digitalWrite(LED, HIGH);
delay(500);
digitalWrite(LED, LOW);
delay(500);
}
```
---
## 主板測試程式
### MicroBit 自走車接腳定義
```cpp=
//Micro:bit I/O port function define
//TB6612 Motor controller
#define M_PWMA 8
#define M_PWMB 13
#define M_BIN1 14
#define M_AIN1 12
//HC04
#define US_TRIG 15
#define US_ECHO 16
//Line sensor
#define LS_RR 0
#define LS_RF 1
#define LS_C 2
#define LS_LF 3
#define LS_LL 10
//Start Button
#define STR_BTN 5
#define STOP_BTN 11
//I2C
#define SCL 19
#define SDA 20
//SPI
#define CS 19
#define SCK 13
#define MISO 14
#define MOSI 15
```
### 馬達轉向定義
```cpp=
byte m1Speed = 200;
byte m2Speed = 200;
const bool m1Clockwise = 0;
const bool m1Unclockwise = 1;
const bool m2Clockwise = 1;
const bool m2Unclockwise = 0;
```
### 車體控制函式
#### 前進
```cpp=
void carForward(){
digitalWrite(M_AIN1,m1Clockwise);
digitalWrite(M_BIN1,m2Clockwise);
analogWrite(M_PWMA,m1Speed);
analogWrite(M_PWMB,m2Speed);
}
```
#### 停止
```cpp=
void carStop(){
analogWrite(M_PWMA,0);
analogWrite(M_PWMB,0);
}
```
#### 右轉(原地轉)
```cpp=
void carTurnRight(){
digitalWrite(M_AIN1,m1Unclockwise);
digitalWrite(M_BIN1,m2Clockwise);
analogWrite(M_PWMA,m1Speed);
analogWrite(M_PWMB,m2Speed);
}
```
#### 左轉(原地轉)
```cpp=
void carTurnLeft(){
digitalWrite(M_AIN1,m1Clockwise);
digitalWrite(M_BIN1,m2Unclockwise);
analogWrite(M_PWMA,m1Speed);
analogWrite(M_PWMB,m2Speed);
}
```
### 車體控制測試程式
```cpp=
//Micro:bit I/O port function define
//TB6612 Motor controller
#define M_PWMA 8
#define M_PWMB 13
#define M_BIN1 14
#define M_AIN1 12
//HC04
#define US_TRIG 15
#define US_ECHO 16
//Line sensor
#define LS_RR 0
#define LS_RF 1
#define LS_C 2
#define LS_LF 3
#define LS_LL 10
//Start Button
#define STR_BTN 5
#define STOP_BTN 11
//I2C
#define SCL 19
#define SDA 20
//SPI
#define CS 19
#define SCK 13
#define MISO 14
#define MOSI 15
byte m1Speed = 200;
byte m2Speed = 200;
const bool m1Clockwise = 1;
const bool m1Unclockwise = 0;
const bool m2Clockwise = 0;
const bool m2Unclockwise = 1;
void carForward(){
digitalWrite(M_AIN1,m1Clockwise);
digitalWrite(M_BIN1,m2Clockwise);
analogWrite(M_PWMA,m1Speed);
analogWrite(M_PWMB,m2Speed);
}
void carStop(){
analogWrite(M_PWMA,0);
analogWrite(M_PWMB,0);
}
void carTurnRight(){
digitalWrite(M_AIN1,m1Unclockwise);
digitalWrite(M_BIN1,m2Clockwise);
analogWrite(M_PWMA,m1Speed);
analogWrite(M_PWMB,m2Speed);
}
void carTurnLeft(){
digitalWrite(M_AIN1,m1Clockwise);
digitalWrite(M_BIN1,m2Unclockwise);
analogWrite(M_PWMA,m1Speed);
analogWrite(M_PWMB,m2Speed);
}
void readLS(){
}
void dispLS(){
}
void setup() {
pinMode(M_PWMA,OUTPUT);
pinMode(M_PWMB,OUTPUT);
pinMode(M_AIN1,OUTPUT);
pinMode(M_BIN1,OUTPUT);
pinMode(US_TRIG,OUTPUT);
pinMode(US_ECHO,INPUT);
pinMode(LS_RR,INPUT);
pinMode(LS_RF,INPUT);
pinMode(LS_C,INPUT);
pinMode(LS_LF,INPUT);
pinMode(LS_LL,INPUT);
pinMode(STR_BTN,INPUT);
pinMode(STOP_BTN,INPUT);
}
void loop() {
carForward();
delay(1000);
carStop();
delay(1000);
carTurnRight();
delay(1000);
carStop();
delay(1000);
carTurnLeft();
delay(1000);
carStop();
delay(1000);
}
```
---
### 按鍵測試
1.Polling (輪詢法) :
微控器主動檢查輸入腳的狀態變化, 亦即在 loop() 主迴圈中不斷地檢查輸入腳, 若發現有變化就處理, 耗費系統資源較多.
2.Interrupt (中斷法) :
微控器不須在主迴圈檢查周邊設備狀態, 而是被動因應周邊中斷的觸發, 將目前的狀態存入堆疊, 暫停現在執行中的程序去處理中斷事件, 控制權移轉到中斷處理函數, 處理完再從堆疊取回被中斷程序繼續執行原先的程序, 因此所耗費之系統資源少.
Arduino 的中斷相關函數如下表 :
| 函數 | 說明 | |
| -------- | -------- | -------- |
|attachInterrupt(int, ISR, mode)| 指派中斷服務函式| int=中斷編號, 0 或 1 ISR=中斷服務函式名稱 mode=中斷模式 (LOW, CHANGE, RISING, FALLING)|
| detachInterrupt(int) | 移除指定腳位之中斷功能| int=中斷編號, 0 或 1|
| noInterrupt() | 停止全部中斷功能 (除 reset 外)||
| interrupts() | 重新啟用全部中斷功能|
|digitalPinToInterrupt(I/O Pin)|中斷對應到數位接腳||
```cpp=
//Micro:bit I/O port function define
//TB6612 Motor controller
#define M_PWMA 8
#define M_PWMB 13
#define M_BIN1 14
#define M_AIN1 12
//HC04
#define US_TRIG 15
#define US_ECHO 16
//Line sensor
#define LS_RR 0
#define LS_RF 1
#define LS_C 2
#define LS_LF 3
#define LS_LL 10
//Start Button
#define STR_BTN 5
#define STOP_BTN 11
//I2C
#define SCL 19
#define SDA 20
//SPI
#define CS 19
#define SCK 13
#define MISO 14
#define MOSI 15
byte m1Speed = 255;
byte m2Speed = 255;
const bool m1Clockwise = 0;
const bool m1Unclockwise = 1;
const bool m2Clockwise = 1;
const bool m2Unclockwise = 0;
//Button Status
bool strBtnStatus = 0;
bool strBtnPreStatus =0;
bool stopBtnstatus = 0;
bool stopBtnPrestatus = 0;
void carForward(){
digitalWrite(M_AIN1,m1Clockwise);
digitalWrite(M_BIN1,m2Clockwise);
analogWrite(M_PWMA,m1Speed);
analogWrite(M_PWMB,m2Speed);
}
void carStop(){
analogWrite(M_PWMA,0);
analogWrite(M_PWMB,0);
}
void carTurnRight(){
digitalWrite(M_AIN1,m1Unclockwise);
digitalWrite(M_BIN1,m2Clockwise);
analogWrite(M_PWMA,m1Speed);
analogWrite(M_PWMB,m2Speed);
}
void carTurnLeft(){
digitalWrite(M_AIN1,m1Clockwise);
digitalWrite(M_BIN1,m2Unclockwise);
analogWrite(M_PWMA,m1Speed);
analogWrite(M_PWMB,m2Speed);
}
void readLS(){
}
void dispLS(){
}
void setup() {
Serial.begin(115200);
pinMode(M_PWMA,OUTPUT);
pinMode(M_PWMB,OUTPUT);
pinMode(M_AIN1,OUTPUT);
pinMode(M_BIN1,OUTPUT);
pinMode(US_TRIG,OUTPUT);
pinMode(US_ECHO,INPUT);
pinMode(LS_RR,INPUT);
pinMode(LS_RF,INPUT);
pinMode(LS_C,INPUT);
pinMode(LS_LF,INPUT);
pinMode(LS_LL,INPUT);
pinMode(STR_BTN,INPUT);
pinMode(STOP_BTN,INPUT);
}
void loop() {
Serial.println(digitalRead(STR_BTN));
carForward();
delay(1000);
carStop();
delay(1000);
carTurnRight();
delay(1000);
carStop();
delay(1000);
carTurnLeft();
delay(1000);
carStop();
delay(1000);
// put your main code here, to run repeatedly:
}
```
---
#### 按鍵狀態偵測變數
```cpp=
//Button Status
bool strBtnStatus = 0;
bool strBtnPreStatus =0;
bool stopBtnStatus = 0;
bool stopBtnPreStatus = 0;
bool carRunFlag = false;
```
#### 按鍵狀態初值設定
```cpp=
pinMode(STR_BTN,INPUT);
pinMode(STOP_BTN,INPUT);
strBtnStatus = digitalRead(STR_BTN);
strBtnPreStatus = strBtnStatus;
stopBtnStatus = digitalRead(STOP_BTN);
stopBtnPreStatus = stopBtnStatus;
```
#### 按鍵按下動作偵測程式--Polling (輪詢法)
```cpp=
strBtnStatus = digitalRead(STR_BTN);
if (strBtnStatus != strBtnPreStatus){
if (strBtnStatus == 0){
carRunFlag = true;
}
strBtnPreStatus = strBtnStatus;
}
stopBtnStatus = digitalRead(STOP_BTN);
if (stopBtnStatus != stopBtnPreStatus){
if (stopBtnStatus == 0){
carRunFlag = false;
}
stopBtnPreStatus = stopBtnStatus;
}
```
#### STOP BTN 按下偵測--Interrupt (中斷法)
**1.中斷服務函式**
```cpp=
void int0(){
carStop();
carRunFlag = false;
}
```
**2.中斷宣告**
```cpp=
attachInterrupt(digitalPinToInterrupt(STOP_BTN),int0,RISING);
```
### 紅外線感測器 "正規化"
#### 黑線白線判定值
```cpp=
int irRRLevel = 0;
int irRFLevel = 0;
int irCFLevel = 0;
int irLFLevel = 0;
int irLLLevel = 0;
```
#### 紅外線感測器最大值
```cpp=
int irRRMax = 0;
int irRFMax = 0;
int irCFMax = 0;
int irLFMax = 0;
int irLLMax = 0;
```
#### 紅外線感測器最小值
```cpp=
int irRRMin = 500;
int irRFMin = 500;
int irCFMin = 500;
int irLFMin = 500;
int irLLMin = 500;
```
#### 紅外線感測器校正旗標
```cpp=
bool irCalibrated = false;
byte strBtnCount = 0;
long timer1 = 0;
```
#### 讀取全部紅外線感測值
```cpp=
void readAllLS(){
irRR = analogRead(LS_RR);
irRF = analogRead(LS_RF);
irCF = analogRead(LS_CF);
irLF = analogRead(LS_LF);
irLL = analogRead(LS_LL);
}
```
### 紅外線感測器測試程式碼
```cpp=
//Micro:bit I/O port function define
//TB6612 Motor controller
#define M_PWMA 8
#define M_PWMB 13
#define M_BIN1 14
#define M_AIN1 12
//HC04
#define US_TRIG 15
#define US_ECHO 16
//Line sensor
#define LS_RR 0
#define LS_RF 1
#define LS_CF 2
#define LS_LF 3
#define LS_LL 10
//Start Button
#define STR_BTN 5
#define STOP_BTN 11
//I2C
#define SCL 19
#define SDA 20
//SPI
#define CS 19
#define SCK 13
#define MISO 14
#define MOSI 15
byte m1Speed = 200;
byte m2Speed = 200;
const bool m1Clockwise = 0;
const bool m1Unclockwise = 1;
const bool m2Clockwise = 1;
const bool m2Unclockwise = 0;
//-----------紅外線感測器感測值變數-------------
int irRR = 0; // 最右邊紅外線感測器 U1
int irRF = 0; // 右前方紅外線感測器 U3
int irCF = 0; // 中心紅外線感測器 U4
int irLF = 0; // 左前方紅外線感測器 U5
int irLL = 0; // 最左邊紅外線感測器 U7
void carForward(){
digitalWrite(M_AIN1,m1Clockwise);
digitalWrite(M_BIN1,m2Clockwise);
analogWrite(M_PWMA,m1Speed);
analogWrite(M_PWMB,m2Speed);
}
void carStop(){
analogWrite(M_PWMA,0);
analogWrite(M_PWMB,0);
}
void carTurnRight(){
digitalWrite(M_AIN1,m1Unclockwise);
digitalWrite(M_BIN1,m2Clockwise);
analogWrite(M_PWMA,m1Speed);
analogWrite(M_PWMB,m2Speed);
}
void carTurnLeft(){
digitalWrite(M_AIN1,m1Clockwise);
digitalWrite(M_BIN1,m2Unclockwise);
analogWrite(M_PWMA,m1Speed);
analogWrite(M_PWMB,m2Speed);
}
void readLS(){
}
void readAllLS(){
irRR = analogRead(LS_RR);
irRF = analogRead(LS_RF);
irCF = analogRead(LS_CF);
irLF = analogRead(LS_LF);
irLL = analogRead(LS_LL);
}
void dispLS(){
Serial.print("LS_RR =");
Serial.println(irRR);
Serial.print("LS_RF =");
Serial.println(irRF);
Serial.print("LS_CF =");
Serial.println(irCF);
Serial.print("LS_LF =");
Serial.println(irLF);
Serial.print("LS_LL =");
Serial.println(irLL);
}
void setup() {
Serial.begin(9600);
pinMode(M_PWMA,OUTPUT);
pinMode(M_PWMB,OUTPUT);
pinMode(M_AIN1,OUTPUT);
pinMode(M_BIN1,OUTPUT);
pinMode(US_TRIG,OUTPUT);
pinMode(US_ECHO,INPUT);
pinMode(LS_RR,INPUT);
pinMode(LS_RF,INPUT);
pinMode(LS_CF,INPUT);
pinMode(LS_LF,INPUT);
pinMode(LS_LL,INPUT);
pinMode(STR_BTN,INPUT);
pinMode(STOP_BTN,INPUT);
}
void loop() {
// carForward();
// delay(1000);
// carStop();
// delay(1000);
// carTurnRight();
// delay(1000);
// carStop();
// delay(1000);
// carTurnLeft();
// delay(1000);
// carStop();
// delay(1000);
//
readAllLS();
dispLS();
delay(1000);
}
```
#### 正規化函式--irLevelCalibration()
**1.當irCalibration = false ,自走車原地旋轉5秒鐘**
**2.讀取全部紅外線感測值,取出最大及最小值.**
**3.在OLED 秀出最後紅外線感測值**
**4.黑白線判定值Level = Min + [50],為判定基礎值**
```cpp=
void irLevelCalibration(){
int irRRMax = 0,irRFMax = 0,irCFMax = 0,irLFMax = 0,irLLMax = 0;
int irRRMin = 500,irRFMin = 500,irCFMin = 500,irLFMin = 500,irLLMin = 500;
timer1 = millis();
while ((millis() - timer1 < 5000) && (irCalibration == false)){
carTurnRight(150,150);
readAllLS();
if (irRR > irRRMax) irRRMax = irRR;
else if (irRR < irRRMin) irRRMin = irRR;
if (irRF > irRFMax) irRFMax = irRF;
else if (irRF < irRFMin) irRFMin = irRF;
if (irCF > irCFMax) irCFMax = irCF;
else if (irCF < irCFMin) irCFMin = irCF;
if (irLF > irLFMax) irLFMax = irLF;
else if (irLF < irLFMin) irLFMin = irLF;
if (irLL > irLLMax) irLLMax = irLL;
else if (irLL < irLLMin) irLLMin = irLL;
}
/*
irRRLevel = irRRMin + (irRRMax - irRRMin)/2;
irRFLevel = irRFMin + (irRFMax - irRFMin)/2;
irCFLevel = irCFMin + (irCFMax - irCFMin)/2;
irLFLevel = irLFMin + (irLFMax - irLFMin)/2;
irLLLevel = irLLMin + (irLLMax - irLLMin)/2;
*/
irRRLevel = irRRMin + IRCABLEVEL;
irRFLevel = irRFMin + IRCABLEVEL;
irCFLevel = irCFMin + IRCABLEVEL;
irLFLevel = irLFMin + IRCABLEVEL;
irLLLevel = irLLMin + IRCABLEVEL;
display.clearDisplay();
display.setTextSize(1); // Draw 1X-scale text
display.setTextColor(SSD1306_WHITE);
display.setCursor(0, 0);
display.println("Max Min Level");
display.print(irRRMax);
display.print(" ");
display.print(irRRMin);
display.print(" ");
display.println(irRRLevel);
display.print(irRFMax);
display.print(" ");
display.print(irRFMin);
display.print(" ");
display.println(irRFLevel);
display.print(irCFMax);
display.print(" ");
display.print(irCFMin);
display.print(" ");
display.println(irCFLevel);
display.print(irLFMax);
display.print(" ");
display.print(irLFMin);
display.print(" ");
display.println(irLFLevel);
display.print(irLLMax);
display.print(" ");
display.print(irLLMin);
display.print(" ");
display.println(irLLLevel);
display.display();
carStop();
irCalibrated = true;
delay(2000);
}
```
### 右手法則測試程式
```cpp=
#include <Adafruit_SSD1306.h>
#include <Wire.h>
#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels
#define OLED_RESET 4 // Reset pin # (or -1 if sharing Arduino reset pin)
#define SCREEN_ADDRESS 0x3C ///< See datasheet for Address; 0x3D for 128x64, 0x3C for 128x32
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
//Micro:bit I/O port function define
//TB6612 Motor controller
#define M_PWMA 8
#define M_PWMB 13
#define M_BIN1 14
#define M_AIN1 12
//HC04
#define US_TRIG 15
#define US_ECHO 16
//Line sensor
#define LS_RR 0
#define LS_RF 1
#define LS_CF 2
#define LS_LF 3
#define LS_LL 10
//Start Button
#define STR_BTN 5
#define STOP_BTN 11
//I2C
#define SCL 19
#define SDA 20
//SPI
#define CS 19
#define SCK 13
#define MISO 14
#define MOSI 15
//Car Status code
#define carStopCode '0'
#define carForwardCode 'S'
#define carTurnRightCode 'R'
#define carTurnLeftCode 'L'
#define carBackCode 'B'
byte m1Speed = 150;
byte m2Speed = 150;
//-------馬達正反轉---------
const bool m1Clockwise = 0; //馬達1正轉
const bool m1Unclockwise = 1; //馬達1反轉
const bool m2Clockwise = 1; //馬達2正轉
const bool m2Unclockwise = 0; //馬達2反轉
String routeRecord = "" ;
//Button Status
bool strBtnStatus = 0;
bool strBtnPreStatus =0;
bool stopBtnStatus = 0;
bool stopBtnPreStatus = 0;
bool carRunFlag = false;
bool carCalibration = true;
//-----------紅外線感測器感測值變數-------------
int irRR = 0; //最右邊紅外線感測器
int irRF = 0; //右前方紅外線感測器
int irCF = 0; //中心紅外線感測器
int irLF = 0; //左前方紅外線感測器
int irLL = 0; //最左邊紅外線感測器
#define IRCABLEVEL 50 //判斷白線感測值的基準
//----------輪差轉的馬達參數------------
#define TRM1 0 //右轉馬達1 轉速
#define TRM2 120 //右轉馬達2 轉速
#define TLM1 120 //左轉馬達1 轉速
#define TLM2 0 //左轉馬達2 轉速
#define TBM1 100 //迴轉馬達1 轉速
#define TBM2 100 //迴轉馬達2 轉速
//--------------白線判定值---------------------
int irRRLevel = 0; //最右邊紅外線感測器
int irRFLevel = 0; //右前方紅外線感測器
int irCFLevel = 0; //中心紅外線感測器
int irLFLevel = 0; //左前方紅外線感測器
int irLLLevel = 0; //最左邊紅外線感測器
bool irCalibrated = false;
byte strBtnCount = 0;
long timer1 = 0;
void carForward(int speed1,int speed2){
digitalWrite(M_AIN1,m1Clockwise);
digitalWrite(M_BIN1,m2Clockwise);
analogWrite(M_PWMA,speed1);
analogWrite(M_PWMB,speed2);
carCalibration = true;
}
void carStop(){
analogWrite(M_PWMA,0);
analogWrite(M_PWMB,0);
carRunFlag = false;
}
void carTurnRight(int speed1,int speed2){
digitalWrite(M_AIN1,m1Clockwise);
digitalWrite(M_BIN1,m2Clockwise);
analogWrite(M_PWMA,speed1);
analogWrite(M_PWMB,speed2);
}
void carTurnAround(int speed1,int speed2){
digitalWrite(M_AIN1,m1Unclockwise);
digitalWrite(M_BIN1,m2Clockwise);
analogWrite(M_PWMA,speed1);
analogWrite(M_PWMB,speed2);
}
void carTurnLeft(int speed1,int speed2){
digitalWrite(M_AIN1,m1Clockwise);
digitalWrite(M_BIN1,m2Clockwise);
analogWrite(M_PWMA,speed1);
analogWrite(M_PWMB,speed2);
}
void readAllLS(){
irRR = analogRead(LS_RR);
irRF = analogRead(LS_RF);
irCF = analogRead(LS_CF);
irLF = analogRead(LS_LF);
irLL = analogRead(LS_LL);
}
void displayLS(String s){
Serial.print("LS_RR =");
Serial.println(irRR);
Serial.print("LS_RF =");
Serial.println(irRF);
Serial.print("LS_CF =");
Serial.println(irCF);
Serial.print("LS_LF =");
Serial.println(irLF);
Serial.print("LS_LL =");
Serial.println(irLL);
display.clearDisplay();
display.setTextSize(1); // Draw 1X-scale text
display.setTextColor(SSD1306_WHITE);
display.setCursor(0, 0);
display.print("LS_RR =");
display.println(irRR);
display.print("LS_RF =");
display.println(irRF);
display.print("LS_CF =");
display.println(irCF);
display.print("LS_LF =");
display.println(irLF);
display.print("LS_LL =");
display.println(irLL);
display.println("");
display.setTextSize(2); // Draw 1X-scale text
display.println(s);
display.display();
}
/*void dispLSMaxMin(){
display.clearDisplay();
display.setTextSize(1); // Draw 1X-scale text
display.setTextColor(SSD1306_WHITE);
display.setCursor(0, 0);
display.println("Max Min Level");
display.print(irRRMax);
display.print(" ");
display.print(irRRMin);
display.print(" ");
display.println(irRRLevel);
display.print(irRFMax);
display.print(" ");
display.print(irRFMin);
display.print(" ");
display.println(irRFLevel);
display.print(irCFMax);
display.print(" ");
display.print(irCFMin);
display.print(" ");
display.println(irCFLevel);
display.print(irLFMax);
display.print(" ");
display.print(irLFMin);
display.print(" ");
display.println(irLFLevel);
display.print(irLLMax);
display.print(" ");
display.print(irLLMin);
display.print(" ");
display.println(irLLLevel);
display.display();
}
*/
//-------------紅外線感測器--黑白線感測值校正---紅外線感測器正規化-------------
void irLevelCalibration(){
int irRRMax = 0,irRFMax = 0,irCFMax = 0,irLFMax = 0,irLLMax = 0;
int irRRMin = 500,irRFMin = 500,irCFMin = 500,irLFMin = 500,irLLMin = 500;
timer1 = millis();
while ((millis() - timer1 < 5000) && (irCalibrated == false)){
carTurnAround(150,150);
readAllLS();
if (irRR > irRRMax) irRRMax = irRR;
else if (irRR < irRRMin) irRRMin = irRR;
if (irRF > irRFMax) irRFMax = irRF;
else if (irRF < irRFMin) irRFMin = irRF;
if (irCF > irCFMax) irCFMax = irCF;
else if (irCF < irCFMin) irCFMin = irCF;
if (irLF > irLFMax) irLFMax = irLF;
else if (irLF < irLFMin) irLFMin = irLF;
if (irLL > irLLMax) irLLMax = irLL;
else if (irLL < irLLMin) irLLMin = irLL;
}
/*
irRRLevel = irRRMin + (irRRMax - irRRMin)/2;
irRFLevel = irRFMin + (irRFMax - irRFMin)/2;
irCFLevel = irCFMin + (irCFMax - irCFMin)/2;
irLFLevel = irLFMin + (irLFMax - irLFMin)/2;
irLLLevel = irLLMin + (irLLMax - irLLMin)/2;
*/
irRRLevel = irRRMin + IRCABLEVEL;
irRFLevel = irRFMin + IRCABLEVEL;
irCFLevel = irCFMin + IRCABLEVEL;
irLFLevel = irLFMin + IRCABLEVEL;
irLLLevel = irLLMin + IRCABLEVEL;
display.clearDisplay();
display.setTextSize(1); // Draw 1X-scale text
display.setTextColor(SSD1306_WHITE);
display.setCursor(0, 0);
display.println("Max Min Level");
display.print(irRRMax);
display.print(" ");
display.print(irRRMin);
display.print(" ");
display.println(irRRLevel);
display.print(irRFMax);
display.print(" ");
display.print(irRFMin);
display.print(" ");
display.println(irRFLevel);
display.print(irCFMax);
display.print(" ");
display.print(irCFMin);
display.print(" ");
display.println(irCFLevel);
display.print(irLFMax);
display.print(" ");
display.print(irLFMin);
display.print(" ");
display.println(irLFLevel);
display.print(irLLMax);
display.print(" ");
display.print(irLLMin);
display.print(" ");
display.println(irLLLevel);
display.display();
carStop();
irCalibrated = true;
delay(2000);
}
void firstRun(){
while(carRunFlag){
carForward(m1Speed,m2Speed);
readAllLS();
if (irRF < irRFLevel && irCF < irCFLevel) {analogWrite(M_PWMB,m2Speed += 5);}
else if (irLF < irLFLevel && irCF < irCFLevel) {analogWrite(M_PWMB,m2Speed -= 5);}
readAllLS();
//if (irRF > irRFLevel && irCF > irCFLevel && irLF > irLFLevel) carStop();
if (irRR < irRRLevel && irRF <irRFLevel && irCF < irCFLevel && irLF < irLFLevel && irLL < irLLLevel) goal();
else if (irRR < irRRLevel) {
turnRight90D();
}
else if (irLL < irLLLevel && (irRR > irRRLevel && irCF > irCFLevel)) turnLeft90D();
else if (irCF > irCFLevel && irRF > irRFLevel && irLF >irLFLevel) {
delay(15);
if (analogRead(LS_RR) > irRRLevel && analogRead(LS_LL) > irLLLevel){
turn180D();
}
}
readAllLS();
if (irRF < irRFLevel && irCF < irCFLevel) {m2Speed -= 5;}
else if (irLF < irLFLevel && irCF < irCFLevel) {m2Speed += 5;}
}
}
void turnRight90D(){
if (analogRead(LS_RR) > irRRLevel) return;
displayLS("turnRight");
carTurnRight(TRM1,TRM2);
long elapsedTime = millis();
delay(50);
while(!(analogRead(LS_LF) > irLFLevel && analogRead(LS_CF) < irCFLevel && analogRead(LS_RF) > irRFLevel)){
//if (analogRead(LS_RF) < irRFLevel && analogRead(LS_CF) < irCFLevel) break;
}
carSuspend();
elapsedTime = millis() - elapsedTime; //計算轉向的時間
showElapsedTime(elapsedTime); //秀出轉向的時間
}
void turnLeft90D(){
if (analogRead(LS_LL) > irLLLevel) return;
displayLS("turnRight");
carTurnLeft(TLM1,TLM2);
long elapsedTime = millis();
delay(50);
while(!(analogRead(LS_LF) > irLFLevel && analogRead(LS_CF) < irCFLevel && analogRead(LS_RF) > irRFLevel)){
}
carSuspend();
elapsedTime = millis() - elapsedTime;
showElapsedTime(elapsedTime);
}
void turn180D(){
displayLS("turnBack");
carTurnAround(TBM1,TBM2);
long elapsedTime = millis();
delay(10);
while(!(analogRead(LS_LF) > irLFLevel && analogRead(LS_CF) < irCFLevel && analogRead(LS_RF) > irRFLevel)){
}
carSuspend();
elapsedTime = millis() - elapsedTime;
showElapsedTime(elapsedTime);
}
void carSuspend(){
analogWrite(M_PWMA,0);
analogWrite(M_PWMB,0);
m1Speed = 150;
m2Speed = 150;
}
void goal(){
carStop();
display.clearDisplay();
display.setTextSize(3); // Draw 1X-scale text
display.setTextColor(SSD1306_WHITE);
display.setCursor(10, 20);
display.println("--Goal--");
display.display();
}
void showElapsedTime(long elTime){
display.clearDisplay();
display.setTextSize(2); // Draw 1X-scale text
display.setTextColor(SSD1306_WHITE);
display.setCursor(10,20);
display.println(elTime);
display.display();
}
void secondRun(){
}
void int0(){
carStop();
carRunFlag = false;
strBtnCount = 0;
}
void setup() {
Serial.begin(115200);
pinMode(M_PWMA,OUTPUT);
pinMode(M_PWMB,OUTPUT);
pinMode(M_AIN1,OUTPUT);
pinMode(M_BIN1,OUTPUT);
pinMode(US_TRIG,OUTPUT);
pinMode(US_ECHO,INPUT);
pinMode(LS_RR,INPUT);
pinMode(LS_RF,INPUT);
pinMode(LS_CF,INPUT);
pinMode(LS_LF,INPUT);
pinMode(LS_LL,INPUT);
pinMode(STR_BTN,INPUT);
pinMode(STOP_BTN,INPUT);
strBtnStatus = digitalRead(STR_BTN);
strBtnPreStatus = strBtnStatus;
stopBtnStatus = digitalRead(STOP_BTN);
stopBtnPreStatus = stopBtnStatus;
display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS);
attachInterrupt(digitalPinToInterrupt(STOP_BTN),int0,RISING);
display.clearDisplay();
display.setTextSize(2); // Draw 1X-scale text
display.setTextColor(SSD1306_WHITE);
display.setCursor(10, 10);
display.println("--Hello--");
display.println("");
display.println("--My Car--");
display.display();
}
void loop() {
strBtnStatus = digitalRead(STR_BTN);
if (strBtnStatus != strBtnPreStatus){
if (strBtnStatus == 0){
strBtnCount ++;
delay(500);
if (strBtnCount >= 2) carRunFlag = true;
}
strBtnPreStatus = strBtnStatus;
}
/*stopBtnStatus = digitalRead(STOP_BTN);
if (stopBtnStatus != stopBtnPreStatus){
if (stopBtnStatus == 0){
carRunFlag = false;
}
stopBtnPreStatus = stopBtnStatus;
}*/
if (strBtnCount == 1 && irCalibrated == false) irLevelCalibration();
else if (strBtnCount >= 2) firstRun();
else if (strBtnCount >= 3) secondRun();
readAllLS();
displayLS("LOOP");
// put your main code here, to run repeatedly:
}
```
### 右手法則2
```cpp=
#include <Adafruit_SSD1306.h>
#include <Wire.h>
#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels
#define OLED_RESET 4 // Reset pin # (or -1 if sharing Arduino reset pin)
#define SCREEN_ADDRESS 0x3C ///< See datasheet for Address; 0x3D for 128x64, 0x3C for 128x32
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
//Micro:bit I/O port function define
//TB6612 Motor controller
#define M_PWMA 8
#define M_PWMB 13
#define M_BIN1 14
#define M_AIN1 12
//HC04
#define US_TRIG 15
#define US_ECHO 16
//Line sensor
#define LS_RR 0
#define LS_RF 1
#define LS_CF 2
#define LS_LF 3
#define LS_LL 10
//Start Button
#define STR_BTN 5
#define STOP_BTN 11
//I2C
#define SCL 19
#define SDA 20
//SPI
#define CS 19
#define SCK 13
#define MISO 14
#define MOSI 15
//Car Status code
#define carStopCode '0'
#define carForwardCode 'S'
#define carTurnRightCode 'R'
#define carTurnLeftCode 'L'
#define carBackCode 'B'
byte m1Speed = 150;
byte m2Speed = 150;
//-------馬達正反轉---------
const bool m1Clockwise = 0; //馬達1正轉
const bool m1Unclockwise = 1; //馬達1反轉
const bool m2Clockwise = 1; //馬達2正轉
const bool m2Unclockwise = 0; //馬達2反轉
String routeRecord = "" ;
//Button Status
bool strBtnStatus = 0;
bool strBtnPreStatus =0;
bool stopBtnStatus = 0;
bool stopBtnPreStatus = 0;
bool carRunFlag = false;
bool carCalibration = true;
//-----------紅外線感測器感測值變數-------------
int irRR = 0; //最右邊紅外線感測器 U1
int irRF = 0; //右前方紅外線感測器 U3
int irCF = 0; //中心紅外線感測器 U4
int irLF = 0; //左前方紅外線感測器 U5
int irLL = 0; //最左邊紅外線感測器 U7
#define IRCABLEVEL 80 //判斷白線感測值的基準
//----------輪差轉的馬達參數------------
#define TRM1 0 //右轉馬達1 轉速
#define TRM2 120 //右轉馬達2 轉速
#define TLM1 120 //左轉馬達1 轉速
#define TLM2 0 //左轉馬達2 轉速
#define TBM1 100 //迴轉馬達1 轉速
#define TBM2 100 //迴轉馬達2 轉速
//--------------白線判定值---------------------
int irRRLevel = 0; //最右邊紅外線感測器
int irRFLevel = 0; //右前方紅外線感測器
int irCFLevel = 0; //中心紅外線感測器
int irLFLevel = 0; //左前方紅外線感測器
int irLLLevel = 0; //最左邊紅外線感測器
bool irCalibrated = false;
byte strBtnCount = 0;
long timer1 = 0;
void carForward(int speed1,int speed2){
digitalWrite(M_AIN1,m1Clockwise);
digitalWrite(M_BIN1,m2Clockwise);
analogWrite(M_PWMA,speed1);
analogWrite(M_PWMB,speed2);
carCalibration = true;
}
void carStop(){
analogWrite(M_PWMA,0);
analogWrite(M_PWMB,0);
carRunFlag = false;
}
void carTurnRight(int speed1,int speed2){
digitalWrite(M_AIN1,m1Clockwise);
digitalWrite(M_BIN1,m2Clockwise);
analogWrite(M_PWMA,speed1);
analogWrite(M_PWMB,speed2);
}
void carTurnAround(int speed1,int speed2){
digitalWrite(M_AIN1,m1Unclockwise);
digitalWrite(M_BIN1,m2Clockwise);
analogWrite(M_PWMA,speed1);
analogWrite(M_PWMB,speed2);
}
void carTurnLeft(int speed1,int speed2){
digitalWrite(M_AIN1,m1Clockwise);
digitalWrite(M_BIN1,m2Clockwise);
analogWrite(M_PWMA,speed1);
analogWrite(M_PWMB,speed2);
}
void readAllLS(){
irRR = analogRead(LS_RR);
irRF = analogRead(LS_RF);
irCF = analogRead(LS_CF);
irLF = analogRead(LS_LF);
irLL = analogRead(LS_LL);
}
void displayLS(String s){
Serial.print("LS_RR =");
Serial.println(irRR);
Serial.print("LS_RF =");
Serial.println(irRF);
Serial.print("LS_CF =");
Serial.println(irCF);
Serial.print("LS_LF =");
Serial.println(irLF);
Serial.print("LS_LL =");
Serial.println(irLL);
display.clearDisplay();
display.setTextSize(1); // Draw 1X-scale text
display.setTextColor(SSD1306_WHITE);
display.setCursor(0, 0);
display.print("LS_RR =");
display.println(irRR);
display.print("LS_RF =");
display.println(irRF);
display.print("LS_CF =");
display.println(irCF);
display.print("LS_LF =");
display.println(irLF);
display.print("LS_LL =");
display.println(irLL);
display.println("");
display.setTextSize(2); // Draw 1X-scale text
display.println(s);
display.display();
}
/*void dispLSMaxMin(){
display.clearDisplay();
display.setTextSize(1); // Draw 1X-scale text
display.setTextColor(SSD1306_WHITE);
display.setCursor(0, 0);
display.println("Max Min Level");
display.print(irRRMax);
display.print(" ");
display.print(irRRMin);
display.print(" ");
display.println(irRRLevel);
display.print(irRFMax);
display.print(" ");
display.print(irRFMin);
display.print(" ");
display.println(irRFLevel);
display.print(irCFMax);
display.print(" ");
display.print(irCFMin);
display.print(" ");
display.println(irCFLevel);
display.print(irLFMax);
display.print(" ");
display.print(irLFMin);
display.print(" ");
display.println(irLFLevel);
display.print(irLLMax);
display.print(" ");
display.print(irLLMin);
display.print(" ");
display.println(irLLLevel);
display.display();
}
*/
//-------------紅外線感測器--黑白線感測值校正---紅外線感測器正規化-------------
void irLevelCalibration(){
int irRRMax = 0,irRFMax = 0,irCFMax = 0,irLFMax = 0,irLLMax = 0;
int irRRMin = 500,irRFMin = 500,irCFMin = 500,irLFMin = 500,irLLMin = 500;
timer1 = millis();
while ((millis() - timer1 < 5000) && (irCalibrated == false)){
carTurnAround(150,150);
readAllLS();
if (irRR > irRRMax) irRRMax = irRR;
else if (irRR < irRRMin) irRRMin = irRR;
if (irRF > irRFMax) irRFMax = irRF;
else if (irRF < irRFMin) irRFMin = irRF;
if (irCF > irCFMax) irCFMax = irCF;
else if (irCF < irCFMin) irCFMin = irCF;
if (irLF > irLFMax) irLFMax = irLF;
else if (irLF < irLFMin) irLFMin = irLF;
if (irLL > irLLMax) irLLMax = irLL;
else if (irLL < irLLMin) irLLMin = irLL;
}
/*
irRRLevel = irRRMin + (irRRMax - irRRMin)/2;
irRFLevel = irRFMin + (irRFMax - irRFMin)/2;
irCFLevel = irCFMin + (irCFMax - irCFMin)/2;
irLFLevel = irLFMin + (irLFMax - irLFMin)/2;
irLLLevel = irLLMin + (irLLMax - irLLMin)/2;
*/
irRRLevel = irRRMin + IRCABLEVEL;
irRFLevel = irRFMin + IRCABLEVEL;
irCFLevel = irCFMin + IRCABLEVEL;
irLFLevel = irLFMin + IRCABLEVEL;
irLLLevel = irLLMin + IRCABLEVEL;
display.clearDisplay();
display.setTextSize(1); // Draw 1X-scale text
display.setTextColor(SSD1306_WHITE);
display.setCursor(0, 0);
display.println("Max Min Level");
display.print(irRRMax);
display.print(" ");
display.print(irRRMin);
display.print(" ");
display.println(irRRLevel);
display.print(irRFMax);
display.print(" ");
display.print(irRFMin);
display.print(" ");
display.println(irRFLevel);
display.print(irCFMax);
display.print(" ");
display.print(irCFMin);
display.print(" ");
display.println(irCFLevel);
display.print(irLFMax);
display.print(" ");
display.print(irLFMin);
display.print(" ");
display.println(irLFLevel);
display.print(irLLMax);
display.print(" ");
display.print(irLLMin);
display.print(" ");
display.println(irLLLevel);
display.display();
carStop();
irCalibrated = true;
delay(2000);
}
void firstRun(){
while(carRunFlag){
carForward(m1Speed,m2Speed);
readAllLS();
if (irRF < irRFLevel && irCF < irCFLevel) {analogWrite(M_PWMB,m2Speed += 5);}
else if (irLF < irLFLevel && irCF < irCFLevel) {analogWrite(M_PWMB,m2Speed -= 5);}
readAllLS();
//if (irRF > irRFLevel && irCF > irCFLevel && irLF > irLFLevel) carStop();
if (irRR < irRRLevel && irRF <irRFLevel && irCF < irCFLevel && irLF < irLFLevel && irLL < irLLLevel) goal();
else if (irRR < irRRLevel) {
turnRight90D();
}
else if (irLL < irLLLevel && (irRR > irRRLevel && irCF > irCFLevel)) turnLeft90D();
else if (irCF > irCFLevel && irRF > irRFLevel && irLF >irLFLevel) {
delay(15);
if (analogRead(LS_RR) > irRRLevel && analogRead(LS_LL) > irLLLevel){
turn180D();
}
}
readAllLS();
if (irRF < irRFLevel && irCF < irCFLevel) {m2Speed -= 5;}
else if (irLF < irLFLevel && irCF < irCFLevel) {m2Speed += 5;}
}
}
void turnRight90D(){
if (analogRead(LS_RR) > irRRLevel) return;
displayLS("turnRight");
carTurnRight(TRM1,TRM2);
long elapsedTime = millis();
delay(50);
while(!(analogRead(LS_LF) > irLFLevel && analogRead(LS_CF) < irCFLevel && analogRead(LS_RF) > irRFLevel)){
//if (analogRead(LS_RF) < irRFLevel && analogRead(LS_CF) < irCFLevel) break;
}
carSuspend();
elapsedTime = millis() - elapsedTime; //計算轉向的時間
showElapsedTime(elapsedTime); //秀出轉向的時間
}
void turnLeft90D(){
if (analogRead(LS_LL) > irLLLevel) return;
displayLS("turnRight");
carTurnLeft(TLM1,TLM2);
long elapsedTime = millis();
delay(50);
while(!(analogRead(LS_LF) > irLFLevel && analogRead(LS_CF) < irCFLevel && analogRead(LS_RF) > irRFLevel)){
}
carSuspend();
elapsedTime = millis() - elapsedTime;
showElapsedTime(elapsedTime);
}
void turn180D(){
displayLS("turnBack");
carTurnAround(TBM1,TBM2);
long elapsedTime = millis();
delay(10);
while(!(analogRead(LS_LF) > irLFLevel && analogRead(LS_CF) < irCFLevel && analogRead(LS_RF) > irRFLevel)){
}
carSuspend();
elapsedTime = millis() - elapsedTime;
showElapsedTime(elapsedTime);
}
void carSuspend(){
analogWrite(M_PWMA,0);
analogWrite(M_PWMB,0);
m1Speed = 150;
m2Speed = 150;
}
void goal(){
carStop();
displayLS("--Goal---");
delay(5000);
}
void showElapsedTime(long elTime){
display.clearDisplay();
display.setTextSize(2); // Draw 1X-scale text
display.setTextColor(SSD1306_WHITE);
display.setCursor(10,20);
display.println(elTime);
display.display();
}
void secondRun(){
}
void int0(){
carStop();
carRunFlag = false;
strBtnCount = 0;
}
void setup() {
Serial.begin(115200);
pinMode(M_PWMA,OUTPUT);
pinMode(M_PWMB,OUTPUT);
pinMode(M_AIN1,OUTPUT);
pinMode(M_BIN1,OUTPUT);
pinMode(US_TRIG,OUTPUT);
pinMode(US_ECHO,INPUT);
pinMode(LS_RR,INPUT);
pinMode(LS_RF,INPUT);
pinMode(LS_CF,INPUT);
pinMode(LS_LF,INPUT);
pinMode(LS_LL,INPUT);
pinMode(STR_BTN,INPUT);
pinMode(STOP_BTN,INPUT);
strBtnStatus = digitalRead(STR_BTN);
strBtnPreStatus = strBtnStatus;
stopBtnStatus = digitalRead(STOP_BTN);
stopBtnPreStatus = stopBtnStatus;
display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS);
attachInterrupt(digitalPinToInterrupt(STOP_BTN),int0,RISING);
display.clearDisplay();
display.setTextSize(2); // Draw 1X-scale text
display.setTextColor(SSD1306_WHITE);
display.setCursor(10, 10);
display.println("--Hello--");
display.println("");
display.println("--My Car--");
display.display();
}
void loop() {
strBtnStatus = digitalRead(STR_BTN);
if (strBtnStatus != strBtnPreStatus){
if (strBtnStatus == 0){
strBtnCount ++;
delay(500);
if (strBtnCount >= 2) carRunFlag = true;
}
strBtnPreStatus = strBtnStatus;
}
/*stopBtnStatus = digitalRead(STOP_BTN);
if (stopBtnStatus != stopBtnPreStatus){
if (stopBtnStatus == 0){
carRunFlag = false;
}
stopBtnPreStatus = stopBtnStatus;
}*/
if (strBtnCount == 1 && irCalibrated == false) irLevelCalibration();
else if (strBtnCount >= 2) firstRun();
else if (strBtnCount >= 3) secondRun();
readAllLS();
displayLS("LOOP");
// put your main code here, to run repeatedly:
}
```
## 2021/12/10 更新
### 直線校正測試程式
```cpp=
#include <Adafruit_SSD1306.h>
#include <Wire.h>
#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels
#define OLED_RESET 4 // Reset pin # (or -1 if sharing Arduino reset pin)
#define SCREEN_ADDRESS 0x3C ///< See datasheet for Address; 0x3D for 128x64, 0x3C for 128x32
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
//Micro:bit I/O port function define
//TB6612 Motor controller
#define M_PWMA 8
#define M_PWMB 13
#define M_BIN1 14
#define M_AIN1 12
//HC04
#define US_TRIG 15
#define US_ECHO 16
//Line sensor
#define LS_RR 0
#define LS_RF 1
#define LS_CF 2
#define LS_LF 3
#define LS_LL 10
//Start Button
#define STR_BTN 5
#define STOP_BTN 11
//LED
#define R_LED 6
#define L_LED 7
//I2C
#define SCL 19
#define SDA 20
//SPI
#define CS 19
#define SCK 13
#define MISO 14
#define MOSI 15
//Car Status code
#define carStopCode '0'
#define carForwardCode 'S'
#define carTurnRightCode 'R'
#define carTurnLeftCode 'L'
#define carBackCode 'B'
int m1Speed = 150;
int m2Speed = 150;
//-------馬達正反轉---------
const bool m1Clockwise = 1; //馬達1正轉
const bool m1Unclockwise = 0; //馬達1反轉
const bool m2Clockwise = 0; //馬達2正轉
const bool m2Unclockwise = 1; //馬達2反轉
String routeRecord = "" ;
//Button Status
bool strBtnStatus = 0;
bool strBtnPreStatus =0;
bool stopBtnStatus = 0;
bool stopBtnPreStatus = 0;
bool carRunFlag = false;
bool carCalibration = true;
//-----------紅外線感測器感測值變數-------------
int irRR = 0; //最右邊紅外線感測器 U1
int irRF = 0; //右前方紅外線感測器 U3
int irCF = 0; //中心紅外線感測器 U4
int irLF = 0; //左前方紅外線感測器 U5
int irLL = 0; //最左邊紅外線感測器 U7
#define IRCABLEVEL 100 //判斷白線感測值的基準
//----------輪差轉的馬達參數------------
#define TRM1 0 //右轉馬達1 轉速
#define TRM2 120 //右轉馬達2 轉速
#define TLM1 120 //左轉馬達1 轉速
#define TLM2 0 //左轉馬達2 轉速
#define TBM1 100 //迴轉馬達1 轉速
#define TBM2 100 //迴轉馬達2 轉速
//-----------直線校正參數-------------
#define SCAL 2
//--------------白線判定值---------------------
int irRRLevel = 0; //最右邊紅外線感測器
int irRFLevel = 0; //右前方紅外線感測器
int irCFLevel = 0; //中心紅外線感測器
int irLFLevel = 0; //左前方紅外線感測器
int irLLLevel = 0; //最左邊紅外線感測器
bool irCalibrated = false;
byte strBtnCount = 0;
long timer1 = 0;
void carForward(int speed1,int speed2){
digitalWrite(M_AIN1,m1Clockwise);
digitalWrite(M_BIN1,m2Clockwise);
analogWrite(M_PWMA,speed1);
analogWrite(M_PWMB,speed2);
carCalibration = true;
}
void carStop(){
analogWrite(M_PWMA,0);
analogWrite(M_PWMB,0);
carRunFlag = false;
}
void carTurnRight(int speed1,int speed2){
digitalWrite(M_AIN1,m1Clockwise);
digitalWrite(M_BIN1,m2Clockwise);
analogWrite(M_PWMA,speed1);
analogWrite(M_PWMB,speed2);
}
void carTurnAround(int speed1,int speed2){
digitalWrite(M_AIN1,m1Unclockwise);
digitalWrite(M_BIN1,m2Clockwise);
analogWrite(M_PWMA,speed1);
analogWrite(M_PWMB,speed2);
}
void carTurnLeft(int speed1,int speed2){
digitalWrite(M_AIN1,m1Clockwise);
digitalWrite(M_BIN1,m2Clockwise);
analogWrite(M_PWMA,speed1);
analogWrite(M_PWMB,speed2);
}
void readAllLS(){
irRR = analogRead(LS_RR);
irRF = analogRead(LS_RF);
irCF = analogRead(LS_CF);
irLF = analogRead(LS_LF);
irLL = analogRead(LS_LL);
}
void displayLS(String s){
/*
Serial.print("LS_RR =");
Serial.println(irRR);
Serial.print("LS_RF =");
Serial.println(irRF);
Serial.print("LS_CF =");
Serial.println(irCF);
Serial.print("LS_LF =");
Serial.println(irLF);
Serial.print("LS_LL =");
Serial.println(irLL);
*/
display.clearDisplay();
display.setTextSize(1); // Draw 1X-scale text
display.setTextColor(SSD1306_WHITE);
display.setCursor(0, 0);
display.print("LS_RR =");
display.println(irRR);
display.print("LS_RF =");
display.println(irRF);
display.print("LS_CF =");
display.println(irCF);
display.print("LS_LF =");
display.println(irLF);
display.print("LS_LL =");
display.println(irLL);
display.println("");
display.setTextSize(2); // Draw 1X-scale text
display.println(s);
display.display();
}
/*void dispLSMaxMin(){
display.clearDisplay();
display.setTextSize(1); // Draw 1X-scale text
display.setTextColor(SSD1306_WHITE);
display.setCursor(0, 0);
display.println("Max Min Level");
display.print(irRRMax);
display.print(" ");
display.print(irRRMin);
display.print(" ");
display.println(irRRLevel);
display.print(irRFMax);
display.print(" ");
display.print(irRFMin);
display.print(" ");
display.println(irRFLevel);
display.print(irCFMax);
display.print(" ");
display.print(irCFMin);
display.print(" ");
display.println(irCFLevel);
display.print(irLFMax);
display.print(" ");
display.print(irLFMin);
display.print(" ");
display.println(irLFLevel);
display.print(irLLMax);
display.print(" ");
display.print(irLLMin);
display.print(" ");
display.println(irLLLevel);
display.display();
}
*/
//-------------紅外線感測器--黑白線感測值校正---紅外線感測器正規化-------------
void irLevelCalibration(){
int irRRMax = 0,irRFMax = 0,irCFMax = 0,irLFMax = 0,irLLMax = 0;
int irRRMin = 500,irRFMin = 500,irCFMin = 500,irLFMin = 500,irLLMin = 500;
timer1 = millis();
while ((millis() - timer1 < 5000) && (irCalibrated == false)){
carTurnAround(150,150);
readAllLS();
if (irRR > irRRMax) irRRMax = irRR;
else if (irRR < irRRMin) irRRMin = irRR;
if (irRF > irRFMax) irRFMax = irRF;
else if (irRF < irRFMin) irRFMin = irRF;
if (irCF > irCFMax) irCFMax = irCF;
else if (irCF < irCFMin) irCFMin = irCF;
if (irLF > irLFMax) irLFMax = irLF;
else if (irLF < irLFMin) irLFMin = irLF;
if (irLL > irLLMax) irLLMax = irLL;
else if (irLL < irLLMin) irLLMin = irLL;
}
irRRLevel = irRRMin + IRCABLEVEL;
irRFLevel = irRFMin + IRCABLEVEL;
irCFLevel = irCFMin + IRCABLEVEL;
irLFLevel = irLFMin + IRCABLEVEL;
irLLLevel = irLLMin + IRCABLEVEL;
display.clearDisplay();
display.setTextSize(1); // Draw 1X-scale text
display.setTextColor(SSD1306_WHITE);
display.setCursor(0, 0);
display.println("Max Min Level");
display.print(irRRMax);
display.print(" ");
display.print(irRRMin);
display.print(" ");
display.println(irRRLevel);
display.print(irRFMax);
display.print(" ");
display.print(irRFMin);
display.print(" ");
display.println(irRFLevel);
display.print(irCFMax);
display.print(" ");
display.print(irCFMin);
display.print(" ");
display.println(irCFLevel);
display.print(irLFMax);
display.print(" ");
display.print(irLFMin);
display.print(" ");
display.println(irLFLevel);
display.print(irLLMax);
display.print(" ");
display.print(irLLMin);
display.print(" ");
display.println(irLLLevehttps://hackmd.io/tcXO5ItySa-kec7XxgMIgA?both#l);
display.display();
carStop();
irCalibrated = true;
delay(10000);
}
void firstRun(){
while(carRunFlag){
readAllLS();
if (irRF < (irRFLevel - IRCABLEVEL)){
m2Speed += SCAL; // 直線校正 往右
if (m2Speed > 255) m2Speed = 255;
digitalWrite(R_LED,HIGH);
digitalWrite(L_LED,LOW);
// displayLS("R"+String(m2Speed));
}else if (irLF < (irLFLevel - IRCABLEVEL)){
m2Speed -= SCAL; // 直線校正 往左
if (m2Speed <= 0) m2Speed =0;
digitalWrite(L_LED,HIGH);
digitalWrite(R_LED,LOW);
// displayLS("L"+String(m2Speed));
}
carForward(m1Speed,m2Speed);
/* readAllLS();
//if (irRF > irRFLevel && irCF > irCFLevel && irLF > irLFLevel) carStop();
if (irRR < irRRLevel && irRF <irRFLevel && irCF < irCFLevel && irLF < irLFLevel && irLL < irLLLevel) goal();
else if (irRR < irRRLevel) {
displayLS("turnRight");
turnRight90D();
delay(5000);
}
else if (irLL < irLLLevel && (irRR > irRRLevel && irCF > irCFLevel)) turnLeft90D();
else if (irCF > irCFLevel && irRF > irRFLevel && irLF >irLFLevel) {
delay(15);
readAllLS();
if ((irCF > irCFLevel && irRF > irRFLevel && irLF >irLFLevel)&& (irRR > irRRLevel && irLL > irLLLevel)){
displayLS("turnBack");
carSuspend();
delay(5000);
turn180D();
}
}
*/
}
}
void turnRight90D(){
if (analogRead(LS_RR) > irRRLevel) return;
displayLS("turnRight");
carTurnRight(TRM1,TRM2);
long elapsedTime = millis();
delay(100);
readAllLS();
while(!(irCF <(irCFLevel + 200))){
readAllLS();
displayLS("KeepTurnRight");
}
carSuspend();
elapsedTime = millis() - elapsedTime; //計算轉向的時間
showElapsedTime(elapsedTime); //秀出轉向的時間
delay(5000);
}
void turnLeft90D(){
if (analogRead(LS_LL) > irLLLevel) return;
displayLS("turnLeft");
carTurnLeft(TLM1,TLM2);
long elapsedTime = millis();
delay(500);
while(!(analogRead(LS_LF) > irLFLevel && analogRead(LS_CF) < irCFLevel && analogRead(LS_RF) > irRFLevel)){
}
carSuspend();
elapsedTime = millis() - elapsedTime;
showElapsedTime(elapsedTime);
}
void turn180D(){
displayLS("turnBack");
carTurnAround(TBM1,TBM2);
long elapsedTime = millis();
delay(10);
readAllLS();
while(!(irCF <(irCFLevel + 200))){
readAllLS();
}
carSuspend();
elapsedTime = millis() - elapsedTime;
showElapsedTime(elapsedTime);
}
void carSuspend(){
analogWrite(M_PWMA,0);
analogWrite(M_PWMB,0);
m1Speed = 150;
m2Speed = 150;
}
void goal(){
carStop();
displayLS("--Goal---");
delay(10000);
}
void showElapsedTime(long elTime){
display.clearDisplay();
display.setTextSize(2); // Draw 1X-scale text
display.setTextColor(SSD1306_WHITE);
display.setCursor(10,20);
display.println(elTime);
display.display();
}
void secondRun(){
}
void int0(){
carStop();
carRunFlag = false;
strBtnCount = 0;
}
void setup() {
Serial.begin(115200);
pinMode(M_PWMA,OUTPUT);
pinMode(M_PWMB,OUTPUT);
pinMode(M_AIN1,OUTPUT);
pinMode(M_BIN1,OUTPUT);
pinMode(US_TRIG,OUTPUT);
pinMode(US_ECHO,INPUT);
pinMode(LS_RR,INPUT);
pinMode(LS_RF,INPUT);
pinMode(LS_CF,INPUT);
pinMode(LS_LF,INPUT);
pinMode(LS_LL,INPUT);
pinMode(STR_BTN,INPUT);
pinMode(STOP_BTN,INPUT);
pinMode(R_LED,OUTPUT);
pinMode(L_LED,OUTPUT);
strBtnStatus = digitalRead(STR_BTN);
strBtnPreStatus = strBtnStatus;
stopBtnStatus = digitalRead(STOP_BTN);
stopBtnPreStatus = stopBtnStatus;
display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS);
attachInterrupt(digitalPinToInterrupt(STOP_BTN),int0,RISING);
display.clearDisplay();
display.setTextSize(2); // Draw 1X-scale text
display.setTextColor(SSD1306_WHITE);
display.setCursor(10, 10);
display.println("--Hello--");
display.println("");
display.println("--My Car--");
display.display();
}
void loop() {
strBtnStatus = digitalRead(STR_BTN);
if (strBtnStatus != strBtnPreStatus){
if (strBtnStatus == 0){
strBtnCount ++;
delay(500);
if (strBtnCount >= 2) carRunFlag = true;
}
strBtnPreStatus = strBtnStatus;
}
/*stopBtnStatus = digitalRead(STOP_BTN);
if (stopBtnStatus != stopBtnPreStatus){
if (stopBtnStatus == 0){
carRunFlag = false;
}
stopBtnPreStatus = stopBtnStatus;
}*/
if (strBtnCount == 1 && irCalibrated == false) irLevelCalibration();
else if (strBtnCount >= 2) firstRun();
else if (strBtnCount >= 3) secondRun();
readAllLS();
displayLS("LOOP");
// put your main code here, to run repeatedly:
}
```
## 2021/12/18 更新
### 右手法則示範影片 1
<iframe width="560" height="315" src="https://www.youtube.com/embed/7yae5bHsinQ" title="YouTube video player" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture" allowfullscreen></iframe>
### 右手法則示範影片 2
<iframe width="560" height="315" src="https://www.youtube.com/embed/vCo0y0mas5A" title="YouTube video player" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture" allowfullscreen></iframe>
### 右手法則示範影片 3
<iframe width="560" height="315" src="https://www.youtube.com/embed/S_GcF3X9L0s" title="YouTube video player" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture" allowfullscreen></iframe>
### 程式碼
```cpp=
#include <Adafruit_SSD1306.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include "car_img.h"
#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels
#define OLED_RESET 4 // Reset pin # (or -1 if sharing Arduino reset pin)
#define SCREEN_ADDRESS 0x3C ///< See datasheet for Address; 0x3D for 128x64, 0x3C for 128x32
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
//Micro:bit I/O port function define
//TB6612 Motor controller
#define M_PWMA 8
#define M_PWMB 13
#define M_BIN1 14
#define M_AIN1 12
//HC04
#define US_TRIG 15
#define US_ECHO 16
//Line sensor
#define LS_RR 0
#define LS_RF 1
#define LS_CF 2
#define LS_LF 3
#define LS_LL 10
//Start Button
#define STR_BTN 5
#define STOP_BTN 11
//LED
#define R_LED 6
#define L_LED 7
//I2C
#define SCL 19
#define SDA 20
//SPI
#define CS 19
#define SCK 13
#define MISO 14
#define MOSI 15
//Car Status code
#define carStopCode '0'
#define carForwardCode 'S'
#define carTurnRightCode 'R'
#define carTurnLeftCode 'L'
#define carBackCode 'B'
#define CARSPEED 230
#define CARSPEEDT 25
int m1Speed = CARSPEED;
int m2Speed = CARSPEED;
//-----------直線校正參數-------------
#define SCAL 1 //直線校正的速度
#define SLCFREQUNCY 150 //直線校正的頻率
//----------------------------------
#define DEBUGDELAY 100 //除錯暫停時間
#define TRDELAY 1 //右轉前繼續前進的時間
#define TBDELAY 20 //迴轉前繼續前進的時間
//-------馬達正反轉---------
const bool m1Clockwise = 0; //馬達1正轉
const bool m1Unclockwise = 1; //馬達1反轉
const bool m2Clockwise = 1; //馬達2正轉
const bool m2Unclockwise = 0; //馬達2反轉
String routeRecord = "" ;
//Button Status
bool strBtnStatus = 0;
bool strBtnPreStatus =0;
bool stopBtnStatus = 0;
bool stopBtnPreStatus = 0;
bool carRunFlag = false;
bool carCalibration = true;
//-----------紅外線感測器感測值變數-------------
int irRR = 0; //最右邊紅外線感測器 U1
int irRF = 0; //右前方紅外線感測器 U3
int irCF = 0; //中心紅外線感測器 U4
int irLF = 0; //左前方紅外線感測器 U5
int irLL = 0; //最左邊紅外線感測器 U7
#define IRLEVELADJ 50 //判斷白線感測值的基準校正值
#define IRBLACKLEVELADJ 150 //判斷黑色區域的基準校正值
//----------輪差轉的馬達參數------------
#define TRM1 0 //右轉馬達1 轉速
#define TRM2 230 //右轉馬達2 轉速
#define TLM1 230 //左轉馬達1 轉速
#define TLM2 0 //左轉馬達2 轉速
#define TBM1 150 //迴轉馬達1 轉速
#define TBM2 150 //迴轉馬達2 轉速
//--------------白線判定值---------------------
int irRRLevel = 0; //最右邊紅外線感測器
int irRFLevel = 0; //右前方紅外線感測器
int irCFLevel = 0; //中心紅外線感測器
int irLFLevel = 0; //左前方紅外線感測器
int irLLLevel = 0; //最左邊紅外線感測器
//--------------黑色判定值---------------------
int irRRBlackLevel = 0; //最右邊紅外線感測器
int irRFBlackLevel = 0; //右前方紅外線感測器
int irCFBlackLevel = 0; //中心紅外線感測器
int irLFBlackLevel = 0; //左前方紅外線感測器
int irLLBlackLevel = 0; //最左邊紅外線感測器
bool irCalibrated = false;
byte strBtnCount = 0;
unsigned long timer1 = 0;
unsigned long startTimer = 0;
void carForward(int speed1,int speed2){
digitalWrite(M_AIN1,m1Clockwise);
digitalWrite(M_BIN1,m2Clockwise);
analogWrite(M_PWMA,speed1);
analogWrite(M_PWMB,speed2);
carCalibration = true;
}
void carStop(){
analogWrite(M_PWMA,0);
analogWrite(M_PWMB,0);
carRunFlag = false;
}
void carTurnRight(int speed1,int speed2){
digitalWrite(M_AIN1,m1Clockwise);
digitalWrite(M_BIN1,m2Clockwise);
analogWrite(M_PWMA,speed1);
analogWrite(M_PWMB,speed2);
}
void carTurnAround(int speed1,int speed2){
digitalWrite(M_AIN1,m1Unclockwise);
digitalWrite(M_BIN1,m2Clockwise);
analogWrite(M_PWMA,speed1);
analogWrite(M_PWMB,speed2);
}
void carTurnLeft(int speed1,int speed2){
digitalWrite(M_AIN1,m1Clockwise);
digitalWrite(M_BIN1,m2Clockwise);
analogWrite(M_PWMA,speed1);
analogWrite(M_PWMB,speed2);
}
void readAllLS(){
irRR = analogRead(LS_RR);
irRF = analogRead(LS_RF);
irCF = analogRead(LS_CF);
irLF = analogRead(LS_LF);
irLL = analogRead(LS_LL);
}
void displayLS(String s){
/*
Serial.print("LS_RR =");
Serial.println(irRR);
Serial.print("LS_RF =");
Serial.println(irRF);
Serial.print("LS_CF =");
Serial.println(irCF);
Serial.print("LS_LF =");
Serial.println(irLF);
Serial.print("LS_LL =");
Serial.println(irLL);
*/
display.clearDisplay();
display.setTextSize(1); // Draw 1X-scale text
display.setTextColor(SSD1306_WHITE);
display.setCursor(0, 0);
display.print("LS_RR =");
display.println(irRR);
display.print("LS_RF =");
display.println(irRF);
display.print("LS_CF =");
display.println(irCF);
display.print("LS_LF =");
display.println(irLF);
display.print("LS_LL =");
display.println(irLL);
display.println("");
display.setTextSize(2); // Draw 1X-scale text
display.println(s);
display.display();
}
/*void dispLSMaxMin(){
display.clearDisplay();
display.setTextSize(1); // Draw 1X-scale text
display.setTextColor(SSD1306_WHITE);
display.setCursor(0, 0);
display.println("Max Min Level");
display.print(irRRMax);
display.print(" ");
display.print(irRRMin);
display.print(" ");
display.println(irRRLevel);
display.print(irRFMax);
display.print(" ");
display.print(irRFMin);
display.print(" ");
display.println(irRFLevel);
display.print(irCFMax);
display.print(" ");
display.print(irCFMin);
display.print(" ");
display.println(irCFLevel);
display.print(irLFMax);
display.print(" ");
display.print(irLFMin);
display.print(" ");
display.println(irLFLevel);
display.print(irLLMax);
display.print(" ");
display.print(irLLMin);
display.print(" ");
display.println(irLLLevel);
display.display();
}
*/
//-------------紅外線感測器--黑白線感測值校正---紅外線感測器正規化-------------
void irLevelCalibration(){
int irRRMax = 0,irRFMax = 0,irCFMax = 0,irLFMax = 0,irLLMax = 0;
int irRRMin = 500,irRFMin = 500,irCFMin = 500,irLFMin = 500,irLLMin = 500;
timer1 = millis();
while ((millis() - timer1 < 5000) && (irCalibrated == false)){
carTurnAround(150,150);
readAllLS();
if (irRR > irRRMax) irRRMax = irRR;
else if (irRR < irRRMin) irRRMin = irRR;
if (irRF > irRFMax) irRFMax = irRF;
else if (irRF < irRFMin) irRFMin = irRF;
if (irCF > irCFMax) irCFMax = irCF;
else if (irCF < irCFMin) irCFMin = irCF;
if (irLF > irLFMax) irLFMax = irLF;
else if (irLF < irLFMin) irLFMin = irLF;
if (irLL > irLLMax) irLLMax = irLL;
else if (irLL < irLLMin) irLLMin = irLL;
}
irRRLevel = irRRMin + IRLEVELADJ;
irRFLevel = irRFMin + IRLEVELADJ;
irCFLevel = irCFMin + IRLEVELADJ;
irLFLevel = irLFMin + IRLEVELADJ;
irLLLevel = irLLMin + IRLEVELADJ;
irRRBlackLevel = irRRMin + IRBLACKLEVELADJ;
irRFBlackLevel = irRFMin + IRBLACKLEVELADJ;
irCFBlackLevel = irCFMin + IRBLACKLEVELADJ;
irLFBlackLevel = irLFMin + IRBLACKLEVELADJ;
irLLBlackLevel = irLLMin + IRBLACKLEVELADJ;
display.clearDisplay();
display.setTextSize(1); // Draw 1X-scale text
display.setTextColor(SSD1306_WHITE);
display.setCursor(0, 0);
display.println("Max Min Level");
display.print(irRRMax);
display.print(" ");
display.print(irRRMin);
display.print(" ");
display.println(irRRLevel);
display.print(irRFMax);
display.print(" ");
display.print(irRFMin);
display.print(" ");
display.println(irRFLevel);
display.print(irCFMax);
display.print(" ");
display.print(irCFMin);
display.print(" ");
display.println(irCFLevel);
display.print(irLFMax);
display.print(" ");
display.print(irLFMin);
display.print(" ");
display.println(irLFLevel);
display.print(irLLMax);
display.print(" ");
display.print(irLLMin);
display.print(" ");
display.println(irLLLevel);
display.display();
carStop();
irCalibrated = true;
delay(10000);
}
void firstRun(){
unsigned long checkStraightTime = 0;
while(carRunFlag){
readAllLS();
if ((micros() - checkStraightTime ) > SLCFREQUNCY){
if (((irRF - irLF) < (irRFLevel - irLFLevel + 10)) && irCF < irCFLevel){
m2Speed += SCAL; // 直線校正 往右
if (m2Speed > (CARSPEED + CARSPEEDT)) m2Speed = CARSPEED + CARSPEEDT;
digitalWrite(R_LED,HIGH);
digitalWrite(L_LED,LOW);
//displayLS("R"+String(m2Speed));
}else if (((irRF - irLF) > (irRFLevel - irLFLevel - 10)) && irCF < irCFLevel){
m2Speed -= SCAL; //直線校正 往左
if (m2Speed <= (CARSPEED - CARSPEEDT)) m2Speed = CARSPEED - CARSPEEDT ;
digitalWrite(L_LED,HIGH);
digitalWrite(R_LED,LOW);
//displayLS("L"+String(m2Speed));
}else{
m1Speed = CARSPEED;
m2Speed = CARSPEED;
digitalWrite(L_LED,LOW);
digitalWrite(R_LED,LOW);
}
checkStraightTime = micros();
}
carForward(m1Speed,m2Speed);
readAllLS();
while (irRF < irRFLevel && irCF < irCFLevel && irLF < irLFLevel){
readAllLS();
if (irRF < irRFLevel && irCF < irCFLevel && irLF < irLFLevel && irRR < irRRLevel && irLL < irLLLevel) {
goal();
return;
}
}
if (irRR < irRRLevel) {
//displayLS("turnRight");
turnRight90D();
delay(DEBUGDELAY);
}else if (irLL < irLLLevel && (irRR > irRRBlackLevel && irCF > irCFBlackLevel)) turnLeft90D();
else if (irCF > irCFBlackLevel && irRF > irRFBlackLevel && irLF > irLFBlackLevel) {
//displayLS("turnBack1");
delay(10);
readAllLS();
if ((irCF > irCFBlackLevel && irRF > irRFBlackLevel && irLF >irLFBlackLevel) && (irRR > irRRBlackLevel && irLL > irLLBlackLevel)){
turn180D();
}
}
}
}
void turnRight90D(){
if (analogRead(LS_RR) > irRRBlackLevel) return;
//delay(TRDELAY);
long elapsedTime = millis();
carTurnRight(TRM1,TRM2);
displayLS("turnRight");
while(!(analogRead(LS_LF) > irLFBlackLevel)){
//displayLS("KeepTR_1");
}
readAllLS();
while(!((irRF > (irRFLevel - IRLEVELADJ)) && irCF < irCFLevel)){
readAllLS();
//displayLS("KeepTR_2");
}
carSuspend();
elapsedTime = millis() - elapsedTime; //計算轉向的時間
showElapsedTime(elapsedTime); //秀出轉向的時間
}
void turnLeft90D(){
if (analogRead(LS_LL) > irLLLevel) return;
long elapsedTime = millis();
carTurnLeft(TLM1,TLM2);
displayLS("turnLeft");
while(!(analogRead(LS_RF) > irRFBlackLevel)){
//displayLS("KeepTR_1");
}
readAllLS();
while(!((irLF > (irLFLevel - IRLEVELADJ)) && irCF < irCFLevel)){
readAllLS();
}
carSuspend();
elapsedTime = millis() - elapsedTime;
showElapsedTime(elapsedTime);
}
void turn180D(){
displayLS("turnBack3");
delay(TBDELAY);
long elapsedTime = millis();
carTurnAround(TBM1,TBM2);
readAllLS();
while(!((irRF > (irRFLevel - IRLEVELADJ)) && irCF < irCFLevel)){
readAllLS();
}
carSuspend();
elapsedTime = millis() - elapsedTime;
showElapsedTime(elapsedTime);
}
void carSuspend(){
analogWrite(M_PWMA,0);
analogWrite(M_PWMB,0);
m1Speed = CARSPEED;
m2Speed = CARSPEED;
}
void goal(){
carStop();
displayLS(String(millis()-startTimer));
delay(10000);
}
void showElapsedTime(long elTime){
display.clearDisplay();
display.setTextSize(2); // Draw 1X-scale text
display.setTextColor(SSD1306_WHITE);
display.setCursor(10,20);
display.println(elTime);
display.display();
}
void secondRun(){
}
void int0(){
carStop();
carRunFlag = false;
strBtnCount = 0;
}
void setup() {
Serial.begin(115200);
pinMode(M_PWMA,OUTPUT);
pinMode(M_PWMB,OUTPUT);
pinMode(M_AIN1,OUTPUT);
pinMode(M_BIN1,OUTPUT);
pinMode(US_TRIG,OUTPUT);
pinMode(US_ECHO,INPUT);
pinMode(LS_RR,INPUT);
pinMode(LS_RF,INPUT);
pinMode(LS_CF,INPUT);
pinMode(LS_LF,INPUT);
pinMode(LS_LL,INPUT);
pinMode(STR_BTN,INPUT);
pinMode(STOP_BTN,INPUT);
pinMode(R_LED,OUTPUT);
pinMode(L_LED,OUTPUT);
strBtnStatus = digitalRead(STR_BTN);
strBtnPreStatus = strBtnStatus;
stopBtnStatus = digitalRead(STOP_BTN);
stopBtnPreStatus = stopBtnStatus;
display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS);
attachInterrupt(digitalPinToInterrupt(STOP_BTN),int0,RISING);
display.clearDisplay();
display.setTextSize(2); // Draw 1X-scale text
display.setTextColor(SSD1306_WHITE);
display.drawBitmap(0, 0, gImage_car_img, 128, 64, 1); //顯示"CIRCUS Pi"Logo
/* display.println("--Hello--");
display.println("");
display.println("--My Car--");*/
display.display();
delay(2000);
}
void loop() {
strBtnStatus = digitalRead(STR_BTN);
if (strBtnStatus != strBtnPreStatus){
if (strBtnStatus == 0){
strBtnCount ++;
delay(500);
if (strBtnCount >= 2) carRunFlag = true;
}
strBtnPreStatus = strBtnStatus;
}
/*stopBtnStatus = digitalRead(STOP_BTN);
if (stopBtnStatus != stopBtnPreStatus){
if (stopBtnStatus == 0){
carRunFlag = false;
}
stopBtnPreStatus = stopBtnStatus;
}*/
if (strBtnCount == 1 && irCalibrated == false) irLevelCalibration();
else if (strBtnCount >= 2)
{
startTimer = millis();
firstRun();
}
else if (strBtnCount >= 3) secondRun();
readAllLS();
displayLS("LOOP");
// put your main code here, to run repeatedly:
}
```
### 開機圖片 128x64
### 開機圖片程式碼- car_img.h
```cpp=
const unsigned char gImage_LINE_LOGO[1024] = { /* 0X00,0X01,0X80,0X00,0X40,0X00, */
0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00,
0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00,
0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X03,0X80,0X00,0X00,0X00,0X00,0X00,0X00,
0X00,0X00,0X00,0X3F,0XFF,0XFF,0X80,0X00,0X07,0XC0,0X00,0X00,0X00,0X00,0X00,0X00,
0X00,0X00,0X00,0X7F,0XFF,0XFF,0XC0,0X00,0X07,0XC0,0X00,0X00,0X00,0X00,0X00,0X00,
0X00,0X00,0X00,0X7F,0XFF,0XFF,0XC0,0X00,0X07,0XC0,0X00,0X00,0X00,0X01,0XF0,0X00,
0X00,0X00,0X00,0X7F,0XFF,0XFF,0XC0,0X00,0X07,0XC0,0X00,0X00,0X00,0X0F,0XFC,0X00,
0X00,0X00,0X00,0X3F,0XFF,0XFF,0X80,0X00,0X07,0XC0,0X00,0X00,0X00,0X1F,0XFE,0X00,
0X00,0X00,0X00,0X3E,0X00,0X00,0X00,0X00,0X07,0XC0,0X00,0X00,0X00,0X3F,0XFF,0X00,
0X00,0X00,0X00,0X3E,0X00,0X00,0X00,0X00,0X07,0XC0,0X00,0X00,0X00,0X3F,0XFF,0X80,
0X00,0X00,0X00,0X3E,0X00,0X00,0X00,0X00,0X07,0XC0,0X00,0X00,0X00,0X7F,0XFF,0X80,
0X00,0X00,0X00,0X3E,0X00,0X00,0X00,0X00,0X07,0XC0,0X00,0X00,0X00,0X7F,0XFF,0XC0,
0X00,0X00,0X00,0X3E,0X00,0X00,0X00,0X00,0X07,0XC0,0X00,0X7F,0XFF,0XFF,0XFF,0XC0,
0X00,0X00,0X00,0X3E,0X00,0X00,0X00,0X00,0X07,0XC0,0X00,0XFF,0XFF,0XFF,0XFF,0XC0,
0X00,0X00,0X00,0X3E,0X00,0X00,0X00,0X00,0X07,0XC0,0X00,0XFF,0XFF,0XFF,0XFF,0XC0,
0X00,0X00,0X00,0X3E,0X00,0X00,0X00,0X00,0X07,0XC0,0X00,0XFF,0XFF,0XFF,0XFF,0XC0,
0X00,0X00,0X00,0X3E,0X00,0X00,0X00,0X00,0X07,0XC0,0X00,0XFF,0XFF,0XFF,0XFF,0XC0,
0X00,0X00,0X00,0X3E,0X00,0X00,0X00,0X00,0X07,0XC0,0X00,0XF8,0X00,0X7F,0XFF,0XC0,
0X00,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XC0,0X00,0XF8,0X00,0X7F,0XFF,0X80,
0X01,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XC0,0X00,0XF8,0X00,0X3F,0XFF,0X80,
0X01,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XC0,0X00,0XF8,0X00,0X3F,0XFF,0X00,
0X01,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XC0,0X00,0XF8,0X00,0X1F,0XFE,0X00,
0X00,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0X80,0X00,0XF8,0X00,0X0F,0XFC,0X00,
0X00,0X00,0X00,0X00,0X00,0X03,0XE0,0X00,0X00,0X00,0X00,0XF8,0X00,0X01,0XF0,0X00,
0X00,0X00,0X00,0X00,0X00,0X03,0XE0,0X00,0X00,0X00,0X00,0XF8,0X00,0X00,0X00,0X00,
0X00,0X00,0X00,0X00,0X00,0X03,0XE0,0X00,0X00,0X00,0X00,0XF8,0X00,0X00,0X00,0X00,
0X00,0X00,0X00,0X00,0X00,0X03,0XE0,0X00,0X00,0X00,0X00,0XF8,0X00,0X00,0X00,0X00,
0X00,0X00,0X00,0X00,0X00,0X03,0XE0,0X00,0X00,0X00,0X00,0XF8,0X00,0X00,0X00,0X00,
0X00,0X00,0X00,0X00,0X00,0X03,0XE0,0X00,0X00,0X00,0X00,0XF8,0X00,0X00,0X00,0X00,
0X00,0X00,0X00,0X00,0X00,0X03,0XE0,0X00,0X00,0X00,0X00,0XF8,0X00,0X00,0X00,0X00,
0X00,0X00,0X00,0X00,0X00,0X03,0XE0,0X00,0X00,0X00,0X00,0XF8,0X00,0X00,0X00,0X00,
0X00,0X00,0X00,0X00,0X00,0X03,0XE0,0X00,0X00,0X00,0X00,0XF8,0X00,0X00,0X00,0X00,
0X00,0X00,0X00,0X00,0X00,0X03,0XE0,0X00,0X00,0X00,0X00,0XF8,0X00,0X00,0X00,0X00,
0X00,0X00,0X00,0X1F,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XFE,0X00,
0X00,0X00,0X00,0X3F,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0X00,
0X00,0X00,0X00,0X3F,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0X00,
0X00,0X00,0X00,0X3F,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0X00,
0X00,0X00,0X00,0X1F,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0XFE,0X00,
0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00,
0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X03,0X00,0X00,0X00,0X00,0X00,0X01,0XC0,0X00,
0X00,0X00,0X00,0X00,0X00,0XC0,0X00,0X01,0X80,0X00,0X01,0X80,0X00,0X00,0XC0,0X00,
0X00,0X00,0XE0,0X00,0X00,0X70,0X00,0X01,0X80,0X00,0X01,0X80,0X00,0X00,0XF8,0X00,
0X00,0X00,0X76,0X00,0X00,0XF0,0X00,0X03,0X80,0X00,0X01,0X80,0X00,0X07,0XF0,0X00,
0X00,0X00,0X77,0X00,0X00,0X0F,0X00,0X03,0X00,0X00,0X01,0X80,0X00,0X00,0XC0,0X00,
0X00,0X00,0X76,0X00,0X00,0X79,0X80,0X06,0X7E,0X00,0X01,0XF0,0X00,0X00,0XFE,0X00,
0X00,0X63,0X7C,0X00,0X19,0XC1,0X80,0X0F,0XCE,0X00,0X07,0XC0,0X00,0X03,0XC7,0X00,
0X00,0X77,0XF0,0X00,0X1F,0X03,0X80,0X0E,0X0E,0X00,0X01,0X80,0X00,0X18,0XF7,0X00,
0X00,0X66,0X3C,0X00,0X3C,0X0E,0X00,0X0C,0X0E,0X00,0X01,0XBE,0X00,0X0D,0XFE,0X00,
0X00,0X06,0XF0,0X00,0X38,0X38,0X00,0X0C,0XEE,0X00,0X01,0XF8,0X00,0X0F,0XEC,0X00,
0X00,0X03,0XF0,0X00,0X30,0XF8,0X00,0X0F,0XCE,0X00,0X07,0XE0,0X00,0X0C,0XF8,0X00,
0X00,0X03,0X70,0X00,0X07,0XB8,0X00,0X0E,0X0E,0X00,0X1C,0XC0,0X00,0X03,0XE0,0X00,
0X00,0X78,0XFC,0X00,0X07,0X70,0X00,0X0C,0X0E,0X00,0X7B,0X80,0X00,0X03,0XC0,0X00,
0X00,0XF1,0XBF,0X00,0X06,0XF0,0X00,0X0D,0XFC,0X01,0XE7,0XF8,0X00,0X00,0XFF,0X80,
0X00,0X67,0X33,0X80,0X06,0XE0,0X00,0X0F,0XCC,0X00,0X0F,0X70,0X00,0X07,0XE1,0XC0,
0X00,0X60,0X70,0X00,0X01,0X8C,0X00,0X0C,0X0C,0X00,0X1D,0XC0,0X00,0X3E,0XC0,0X00,
0X00,0X70,0XF0,0X00,0X03,0X7C,0X00,0X0C,0X0C,0X00,0X03,0X00,0X00,0XF8,0XC0,0X00,
0X00,0X30,0X00,0X00,0X07,0XDC,0X00,0X0C,0X6C,0X00,0X1F,0XC0,0X00,0X00,0XC0,0X00,
0X00,0XFF,0XE0,0X00,0X07,0X38,0X00,0X0F,0XFC,0X00,0X30,0X7F,0XF0,0X00,0XC0,0X00,
0X00,0XC0,0X7F,0XE0,0X06,0X70,0X00,0X00,0X1C,0X00,0X00,0X0F,0XC0,0X00,0XC0,0X00,
0X00,0X00,0X0F,0X80,0X00,0X70,0X00,0X00,0X0C,0X00,0X00,0X00,0X00,0X00,0XC0,0X00,
0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00,0XC0,0X00,
0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00,0XC0,0X00,
0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00,
0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00,
};
```
## 2023/05/11更新--程式碼
**馬達速度200**
```cpp=
#include <Adafruit_SSD1306.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include "car_img.h"
#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels
#define OLED_RESET 4 // Reset pin # (or -1 if sharing Arduino reset pin)
#define SCREEN_ADDRESS 0x3C ///< See datasheet for Address; 0x3D for 128x64, 0x3C for 128x32
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
//Micro:bit I/O port function define
//TB6612 Motor controller
#define M_PWMA 8
#define M_PWMB 13
#define M_BIN1 14
#define M_AIN1 12
//HC04
#define US_TRIG 15
#define US_ECHO 16
//Line sensor
#define LS_RR 0
#define LS_RF 1
#define LS_CF 2
#define LS_LF 3
#define LS_LL 10
//Start Button
#define STR_BTN 5
#define STOP_BTN 11
//LED
#define R_LED 6
#define L_LED 7
//I2C
#define SCL 19
#define SDA 20
//SPI
#define CS 19
#define SCK 13
#define MISO 14
#define MOSI 15
//Car Status code
#define carStopCode '0'
#define carForwardCode 'S'
#define carTurnRightCode 'R'
#define carTurnLeftCode 'L'
#define carBackCode 'B'
#define CARSPEED 200
#define CARSPEEDT 20
int m1Speed = CARSPEED;
int m2Speed = CARSPEED;
//-----------直線校正參數-------------
#define SCAL 1 //直線校正的速度
#define SLCFREQUNCY 50 //直線校正的頻率
//----------------------------------
#define DEBUGDELAY 1 //除錯暫停時間
#define TRDELAY 1 //右轉前繼續前進的時間
#define TBDELAY 20 //迴轉前繼續前進的時間
//-------馬達正反轉---------
const bool m1Clockwise = 0; //馬達1正轉
const bool m1Unclockwise = 1; //馬達1反轉
const bool m2Clockwise = 1; //馬達2正轉
const bool m2Unclockwise = 0; //馬達2反轉
String routeRecord = "" ;
//Button Status
bool strBtnStatus = 0;
bool strBtnPreStatus = 0;
bool stopBtnStatus = 0;
bool stopBtnPreStatus = 0;
bool carRunFlag = false;
bool carCalibration = true;
//-----------紅外線感測器感測值變數-------------
int irRR = 0; //最右邊紅外線感測器 U1
int irRF = 0; //右前方紅外線感測器 U3
int irCF = 0; //中心紅外線感測器 U4
int irLF = 0; //左前方紅外線感測器 U5
int irLL = 0; //最左邊紅外線感測器 U7
#define IRLEVELADJ 80 //判斷白線感測值的基準校正值
#define IRBLACKLEVELADJ 250 //判斷黑色區域的基準校正值
//----------輪差轉的馬達參數------------
#define TRM1 0 //右轉馬達1 轉速
#define TRM2 230 //右轉馬達2 轉速
#define TLM1 230 //左轉馬達1 轉速
#define TLM2 0 //左轉馬達2 轉速
#define TBM1 150 //迴轉馬達1 轉速
#define TBM2 150 //迴轉馬達2 轉速
//--------------白線判定值---------------------
int irRRLevel = 0; //最右邊紅外線感測器
int irRFLevel = 0; //右前方紅外線感測器
int irCFLevel = 0; //中心紅外線感測器
int irLFLevel = 0; //左前方紅外線感測器
int irLLLevel = 0; //最左邊紅外線感測器
//--------------黑色判定值---------------------
int irRRBlackLevel = 0; //最右邊紅外線感測器
int irRFBlackLevel = 0; //右前方紅外線感測器
int irCFBlackLevel = 0; //中心紅外線感測器
int irLFBlackLevel = 0; //左前方紅外線感測器
int irLLBlackLevel = 0; //最左邊紅外線感測器
bool irCalibrated = false;
byte strBtnCount = 0;
unsigned long timer1 = 0;
unsigned long startTimer = 0;
void carForward(int speed1, int speed2) {
digitalWrite(M_AIN1, m1Clockwise);
digitalWrite(M_BIN1, m2Clockwise);
analogWrite(M_PWMA, speed1);
analogWrite(M_PWMB, speed2);
carCalibration = true;
}
void carStop() {
analogWrite(M_PWMA, 0);
analogWrite(M_PWMB, 0);
carRunFlag = false;
}
void carTurnRight(int speed1, int speed2) {
digitalWrite(M_AIN1, m1Clockwise);
digitalWrite(M_BIN1, m2Clockwise);
analogWrite(M_PWMA, speed1);
analogWrite(M_PWMB, speed2);
}
void carTurnAround(int speed1, int speed2) {
digitalWrite(M_AIN1, m1Unclockwise);
digitalWrite(M_BIN1, m2Clockwise);
analogWrite(M_PWMA, speed1);
analogWrite(M_PWMB, speed2);
}
void carTurnLeft(int speed1, int speed2) {
digitalWrite(M_AIN1, m1Clockwise);
digitalWrite(M_BIN1, m2Clockwise);
analogWrite(M_PWMA, speed1);
analogWrite(M_PWMB, speed2);
}
void readAllLS() {
irRR = analogRead(LS_RR);
irRF = analogRead(LS_RF);
irCF = analogRead(LS_CF);
irLF = analogRead(LS_LF);
irLL = analogRead(LS_LL);
}
void displayLS(String s) {
/*
Serial.print("LS_RR =");
Serial.println(irRR);
Serial.print("LS_RF =");
Serial.println(irRF);
Serial.print("LS_CF =");
Serial.println(irCF);
Serial.print("LS_LF =");
Serial.println(irLF);
Serial.print("LS_LL =");
Serial.println(irLL);
*/
display.clearDisplay();
display.setTextSize(1); // Draw 1X-scale text
display.setTextColor(SSD1306_WHITE);
display.setCursor(0, 0);
display.print("LS_RR =");
display.println(irRR);
display.print("LS_RF =");
display.println(irRF);
display.print("LS_CF =");
display.println(irCF);
display.print("LS_LF =");
display.println(irLF);
display.print("LS_LL =");
display.println(irLL);
display.println("");
display.setTextSize(2); // Draw 1X-scale text
display.println(s);
display.display();
}
/*void dispLSMaxMin(){
display.clearDisplay();
display.setTextSize(1); // Draw 1X-scale text
display.setTextColor(SSD1306_WHITE);
display.setCursor(0, 0);
display.println("Max Min Level");
display.print(irRRMax);
display.print(" ");
display.print(irRRMin);
display.print(" ");
display.println(irRRLevel);
display.print(irRFMax);
display.print(" ");
display.print(irRFMin);
display.print(" ");
display.println(irRFLevel);
display.print(irCFMax);
display.print(" ");
display.print(irCFMin);
display.print(" ");
display.println(irCFLevel);
display.print(irLFMax);
display.print(" ");
display.print(irLFMin);
display.print(" ");
display.println(irLFLevel);
display.print(irLLMax);
display.print(" ");
display.print(irLLMin);
display.print(" ");
display.println(irLLLevel);
display.display();
}
*/
//-------------紅外線感測器--黑白線感測值校正---紅外線感測器正規化-------------
void irLevelCalibration() {
int irRRMax = 0, irRFMax = 0, irCFMax = 0, irLFMax = 0, irLLMax = 0;
int irRRMin = 500, irRFMin = 500, irCFMin = 500, irLFMin = 500, irLLMin = 500;
timer1 = millis();
while ((millis() - timer1 < 5000) && (irCalibrated == false)) {
carTurnAround(150, 150);
readAllLS();
if (irRR > irRRMax) irRRMax = irRR;
else if (irRR < irRRMin) irRRMin = irRR;
if (irRF > irRFMax) irRFMax = irRF;
else if (irRF < irRFMin) irRFMin = irRF;
if (irCF > irCFMax) irCFMax = irCF;
else if (irCF < irCFMin) irCFMin = irCF;
if (irLF > irLFMax) irLFMax = irLF;
else if (irLF < irLFMin) irLFMin = irLF;
if (irLL > irLLMax) irLLMax = irLL;
else if (irLL < irLLMin) irLLMin = irLL;
}
irRRLevel = irRRMin + IRLEVELADJ;
irRFLevel = irRFMin + IRLEVELADJ;
irCFLevel = irCFMin + IRLEVELADJ;
irLFLevel = irLFMin + IRLEVELADJ;
irLLLevel = irLLMin + IRLEVELADJ;
irRRBlackLevel = irRRMin + IRBLACKLEVELADJ;
irRFBlackLevel = irRFMin + IRBLACKLEVELADJ;
irCFBlackLevel = irCFMin + IRBLACKLEVELADJ;
irLFBlackLevel = irLFMin + IRBLACKLEVELADJ;
irLLBlackLevel = irLLMin + IRBLACKLEVELADJ;
display.clearDisplay();
display.setTextSize(1); // Draw 1X-scale text
display.setTextColor(SSD1306_WHITE);
display.setCursor(0, 0);
display.println("Max-----Min------Level");
display.print(irRRMax);
display.print(" ");
display.print(irRRMin);
display.print(" ");
display.println(irRRLevel);
display.print(irRFMax);
display.print(" ");
display.print(irRFMin);
display.print(" ");
display.println(irRFLevel);
display.print(irCFMax);
display.print(" ");
display.print(irCFMin);
display.print(" ");
display.println(irCFLevel);
display.print(irLFMax);
display.print(" ");
display.print(irLFMin);
display.print(" ");
display.println(irLFLevel);
display.print(irLLMax);
display.print(" ");
display.print(irLLMin);
display.print(" ");
display.println(irLLLevel);
display.display();
readAllLS();
while (!((irRF > (irRFLevel - IRLEVELADJ)) && irCF < irCFLevel)) {
readAllLS();
if (irRF < irRFLevel) carTurnAround(TBM1 / 2, TBM2 / 2);
}
carStop();
irCalibrated = true;
delay(5000);
}
void firstRun() {
unsigned long checkStraightTime = 0;
while (carRunFlag) {
readAllLS();
if ((micros() - checkStraightTime ) > SLCFREQUNCY) {
if (((irRF - irLF) < (irRFLevel - irLFLevel + 10)) && irCF < irCFLevel) {
m2Speed += SCAL; // 直線校正 往右
if (m2Speed > (CARSPEED + CARSPEEDT)) m2Speed = CARSPEED + CARSPEEDT;
digitalWrite(R_LED, HIGH);
digitalWrite(L_LED, LOW);
//displayLS("R"+String(m2Speed));
} else if (((irRF - irLF) > (irRFLevel - irLFLevel - 10)) && irCF < irCFLevel) {
m2Speed -= SCAL; //直線校正 往左
if (m2Speed <= (CARSPEED - CARSPEEDT)) m2Speed = CARSPEED - CARSPEEDT ;
digitalWrite(L_LED, HIGH);
digitalWrite(R_LED, LOW);
//displayLS("L"+String(m2Speed));
} else {
m1Speed = CARSPEED;
m2Speed = CARSPEED;
digitalWrite(L_LED, LOW);
digitalWrite(R_LED, LOW);
}
checkStraightTime = micros();
}
carForward(m1Speed, m2Speed);
readAllLS();
while (irRF < irRFLevel && irCF < irCFLevel && irLF < irLFLevel) {
readAllLS();
if (irRF < irRFLevel && irCF < irCFLevel && irLF < irLFLevel && irRR < irRRLevel && irLL < irLLLevel) {
goal();
return;
}
}
if (irRR < irRRLevel) {
//displayLS("turnRight");
turnRight90D();
delay(DEBUGDELAY);
} else if (irLL < irLLLevel && (irRR > irRRBlackLevel && irCF > irCFBlackLevel)) turnLeft90D();
else if (irCF > irCFBlackLevel && irRF > irRFBlackLevel && irLF > irLFBlackLevel) {
//displayLS("turnBack1");
delay(10);
readAllLS();
if ((irCF > irCFBlackLevel && irRF > irRFBlackLevel && irLF > irLFBlackLevel) && (irRR > irRRBlackLevel && irLL > irLLBlackLevel)) {
turn180D();
}
}
}
}
void turnRight90D() {
if (analogRead(LS_RR) > irRRBlackLevel) return;
//delay(TRDELAY);
long elapsedTime = millis();
carTurnRight(TRM1, TRM2);
displayLS("turnRight");
while (!(analogRead(LS_LF) > irLFBlackLevel)) {
//displayLS("KeepTR_1");
}
readAllLS();
while (!((irRF > irRFLevel) && irCF < irCFLevel)) {
readAllLS();
if (irCF < irCFLevel) carTurnRight(TRM1, TRM2 - 50);
//displayLS("KeepTR_2");
}
carSuspend();
elapsedTime = millis() - elapsedTime; //計算轉向的時間
showElapsedTime(elapsedTime); //秀出轉向的時間
}
void turnLeft90D() {
if (analogRead(LS_LL) > irLLLevel) return;
long elapsedTime = millis();
carTurnLeft(TLM1, TLM2);
displayLS("turnLeft");
while (!(analogRead(LS_RF) > irRFBlackLevel)) {
//displayLS("KeepTR_1");
}
readAllLS();
while (!((irLF > irLFLevel) && irCF < irCFLevel)) {
readAllLS();
if (irCF < irCFLevel) carTurnLeft(TLM1 - 50, TLM2);
}
carSuspend();
elapsedTime = millis() - elapsedTime;
showElapsedTime(elapsedTime);
}
void turn180D() {
displayLS("turnBack3");
readAllLS();
if ((irCF < irCFLevel) && (irRF < irRFLevel || irLF < irLFLevel)) return;
delay(TBDELAY);
long elapsedTime = millis();
readAllLS();
carTurnAround(TBM1, TBM2);
readAllLS();
while (!((irRF > (irRFLevel - IRLEVELADJ)) && irCF < irCFLevel)) {
readAllLS();
if (irRF < irRFLevel) carTurnAround(TBM1 / 2, TBM2 / 2);
}
carSuspend();
elapsedTime = millis() - elapsedTime;
showElapsedTime(elapsedTime);
}
void carSuspend() {
analogWrite(M_PWMA, 0);
analogWrite(M_PWMB, 0);
m1Speed = CARSPEED;
m2Speed = CARSPEED;
}
void goal() {
carStop();
displayLS(String(millis() - startTimer));
delay(10000);
}
void showElapsedTime(long elTime) {
display.clearDisplay();
display.setTextSize(2); // Draw 1X-scale text
display.setTextColor(SSD1306_WHITE);
display.setCursor(10, 20);
display.println(elTime);
display.display();
}
void secondRun() {
}
void int0() {
carStop();
carRunFlag = false;
strBtnCount = 0;
}
void setup() {
Serial.begin(115200);
pinMode(M_PWMA, OUTPUT);
pinMode(M_PWMB, OUTPUT);
pinMode(M_AIN1, OUTPUT);
pinMode(M_BIN1, OUTPUT);
pinMode(US_TRIG, OUTPUT);
pinMode(US_ECHO, INPUT);
pinMode(LS_RR, INPUT);
pinMode(LS_RF, INPUT);
pinMode(LS_CF, INPUT);
pinMode(LS_LF, INPUT);
pinMode(LS_LL, INPUT);
pinMode(STR_BTN, INPUT);
pinMode(STOP_BTN, INPUT);
pinMode(R_LED, OUTPUT);
pinMode(L_LED, OUTPUT);
strBtnStatus = digitalRead(STR_BTN);
strBtnPreStatus = strBtnStatus;
stopBtnStatus = digitalRead(STOP_BTN);
stopBtnPreStatus = stopBtnStatus;
display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS);
attachInterrupt(digitalPinToInterrupt(STOP_BTN), int0, RISING);
display.clearDisplay();
display.setTextSize(2); // Draw 1X-scale text
display.setTextColor(SSD1306_WHITE);
display.drawBitmap(0, 0, gImage_car_img, 128, 64, 1); //顯示"CIRCUS Pi"Logo
/* display.println("--Hello--");
display.println("");
display.println("--My Car--");*/
display.display();
delay(2000);
}
void loop() {
strBtnStatus = digitalRead(STR_BTN);
if (strBtnStatus != strBtnPreStatus) {
if (strBtnStatus == 0) {
strBtnCount ++;
delay(500);
if (strBtnCount >= 2) carRunFlag = true;
}
strBtnPreStatus = strBtnStatus;
}
/*stopBtnStatus = digitalRead(STOP_BTN);
if (stopBtnStatus != stopBtnPreStatus){
if (stopBtnStatus == 0){
carRunFlag = false;
}
stopBtnPreStatus = stopBtnStatus;
}*/
if (strBtnCount == 1 && irCalibrated == false) irLevelCalibration();
else if (strBtnCount >= 2)
{
startTimer = millis();
firstRun();
}
else if (strBtnCount >= 3) secondRun();
readAllLS();
displayLS("LOOP-0511a");
// put your main code here, to run repeatedly:
}
```
## 2023/05/11 加強紅外線感測板靈敏度
#### 修改自走車底板GPIO供電電壓3.3V -> 5V
#### 1.移除R46
#### 2.將SW4 Pin1-2 短路,GPIO供電電壓3.3V -> 5V
###### tags: `創客工作坊`
Google Drive
Gist
Clipboard
Sign in with Wallet
