DIY DIY Robot Cleaner Vacuum pada Arduino DIY

/*
Program untuk mengawal robot dengan dua motor.
Robot berubah ketika motor mengubah kelajuan dan arah mereka.
Bumper depan di sisi kiri dan kanan mengesan rintangan.
Sonar ultrasonik boleh disambungkan ke input analog (diuji pada LV-MaxSonar-EZ1):
- letakkan pin dalam array sonarPins dalam susunan berikut: kiri, kanan, depan, yang lain ..
Contoh:
1. hanya anak panah kiri dan kanan yang disambungkan ke pin 2 dan 3: sonarPins [] = {2,3}
2. Anak panah kiri, kanan dan depan yang disambungkan kepada pin 2, 3 dan 5: sonarPins [] = {2,3,5}
3. hanya sonar depan yang disambungkan ke pin 5: sonarPins [] = {-1, -1.5}
4. hanya meninggal sonar disambungkan ke pin 2: sonarPins [] = {2}
5. hanya sonar kanan disambungkan ke pin 3: sonarPins [] = {-1,3}
6.5 sonar yang terhubung dengan pin 1,2,3,4,5: sonarPins [] = {1,2,3,4,5}
Perisai motor digunakan untuk menjalankan motor.
*/
int int Baud = 9600; / // kelajuan port UART

/ // Hartanah Sonar
int sonarPins [] = {1, 2}; // Analog Pin Nums untuk sensor sonar Pin AN
const panjang MinLeftDistance = 20; // Minimal jarak dibenarkan minimum
const panjang MinRightDistance = 20; // Jarak minimum dibenarkan minimum
const panjang MinFrontDistance = 15; // Jarak hadapan minimum yang dibenarkan
int int SamplesAmount = 15; // lebih banyak sampel - pengukuran yang lebih lancar dan ketinggalan yang lebih besar
const int SonarDisplayFrequency = 10; // hanya memaparkan salah satu baris ini - tidak semua
int sonarDisplayFrequencyCount = 0;
faktor Faktor Panjang = 2.54 / 2;
sampel yang panjang [sizeof (sonarPins)] [SamplesAmount];
int sampleIndex [sizeof (sonarPins)];

// sebelah kanan
const int pinRightMotorDirection = 4; // ini boleh ditandakan pada perisai motor sebagai "DIR A"
const int pinRightMotorSpeed ​​= 3; // ini boleh ditandakan pada perisai motor sebagai "PWM A"
int int pinRightBumper = 2; // tempat bumper yang betul disambungkan

/ / kiri
const int pinLeftMotorDirection = 7; // ini boleh ditandakan pada perisai motor sebagai "DIR B"
const int pinLeftMotorSpeed ​​= 6; // ini boleh ditandakan pada perisai motor sebagai "PWM B"
const int pinLeftBumper = 8; // tempat bumper yang betul disambungkan

/ / Uncomment 2 baris seterusnya jika Motor Shield telah pecah
// const int pinRightMotorBreak = PUT_BREAK_PIN_HERE; // ini boleh ditandakan pada perisai motor sebagai "BREAKE A"
// const int pinLeftMotorBreak = PUT_BREAK_PIN_HERE; // ini boleh ditandakan pada perisai motor sebagai "BREAKE B"

// bidang
int int turnRightTimeout = 100;
int int turnLeftTimeout = 150;
// set di kaunter berapa lama motor berjalan kembali: N / 10 (dalam milidetik)
int countDownWhileMovingToRight;
int countDownWhileMovingToLeft;

// Inisialisasi
void setup () {
Serial.begin (Baud);
initPins ();

/ / Uncomment next 4 lines if Motor Shield breaks
// pinMode (pinLeftMotorBreak, OUTPUT);
// pinMode (pinRightMotorBreak, OUTPUT);
// digitalWrite (pinLeftMotorBreak, LOW); // matikan rehat
/ // digitalWrite (pinRightMotorBreak, LOW); // matikan rehat

runRightMotorForward ();
runLeftMotorForward ();
startMotors ();
}

// Gelung utama
kekosongan gelung () {

mengesahkanAndSetRightSide ();
sahkanAndSetLeftSide ();

prosesRightSide ();
prosesLeftSide ();

kelewatan (10); // mengulang setiap 10 milisaat
}

//---------------------------------------------------
void initPins () {
pinMode (pinRightMotorDirection, OUTPUT);
pinMode (pinRightMotorSpeed, OUTPUT);
pinMode (pinRightBumper, INPUT);
pinMode (pinLeftMotorDirection, OUTPUT);
pinMode (pinLeftMotorSpeed, OUTPUT);
pinMode (pinLeftBumper, INPUT);
untuk (int i = 0; i pinMode (sonarPins [i], INPUT);
}

tidak sah startMotors () {
setMotorSpeed ​​(pinRightMotorSpeed, 255);
setMotorSpeed ​​(pinLeftMotorSpeed, 255);
}

void waitWhileAnyBumperIsPressed () {
sementara (checkBumperIsNotPressed (pinRightBumper)
&& checkBumperIsNotPressed (pinLeftBumper)) {
kelewatan (20); // semak setiap 20 milisaat
}
}

void processRightSide () {
jika (CountDownWhileMovingToRight MinFrontDistance) // semak sekiranya jarak hadapan minimum yang dibenarkan tidak tercapai
kembali
jika (checkCounterIsNotSet (countDownWhileMovingToLeft)) // jika kaunter belum mengira
runLeftMotorBackward (); // jalankan motor kanan ke belakang
countDownWhileMovingToLeft = turnLeftTimeout; // set counter untuk nilai maksimum untuk memulakannya menghitung ke bawah
}

bool checkCounterIsNotSet (int counter) {
balik kaunter = SamplesAmount)
sampleIndex [pinIndex] = 0;
sampel [pinIndex] [sampleIndex [pinIndex]] = nilai;
kembali benar;
}

calculateAvarageDistance panjang (int pinIndex) {
purata panjang = 0;
untuk (int i = 0; i purata + = sampel [pinIndex] [i];
purata pulangan / SamplesAmount;
}