เดินหน้า PF();
ถอยหลัง PB();
เลี้ยวซ้าย L();
เลี้ยวขวา R();
ยก up();
วาง down();
หนีบ (keep);
ปล่อย(pause);
/////// ผนวกไลบรารี่ ////////////
#include <Wire.h>
#include <SPI.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Servo.h>
Adafruit_SSD1306 OLED(-1);
#include <Adafruit_MCP3008.h>
Adafruit_MCP3008 adc, adc2;
int ss1 = 15; // เปิดใช้งาน s2 ที่พอร์ต 3
int ss2 = 16; // เปิดใช้งาน s3 ที่พอร์ต A0
Servo sv1; // ประกาศการใช้งาน sv1
Servo sv2; // ประกาศการใช้งาน sv2
///////////ตั้งค่าปุ่มกด///////////////////
int button = 2; /// กำหนดปุ่มกดสวิตซ์ขา 2
//////////กำหนดตำแหน่งเซอโว/////////////
void up() {
sv1.write(60);
delay(500);
} //////ยก
void down() {
sv1.write(90);
delay(500);
} //////วาง
void keep() {
sv2.write(50);
delay(500);
} //////หนีบ
void pasue() {
sv2.write(120);
delay(500);
} //////ปล่อย
///////จูนค่ามอเตอร์/////////////////
#define SpeedOffsetMotorL 0
#define SpeedOffsetMotorR 0
////////////ตั้งค่าพอร์ตมอเตอร์////////////////////
/////// motor R ////////
#define MLD1 4 //DIR1A
#define MLD2 9 //DIR2A
#define MLEN 5 //PWMA
/////// motor L ////////
#define MRD1 7 //DIR1B
#define MRD2 8 //DIR2B
#define MREN 6 //PWMB
#define pressures false
#define rumble false
int errorJ = 0;
byte type = 0;
byte vibrate = 0;
//////////////////////////////////
float error = 0;
float pTerm = 0;
float integrated_error = 0;
float iTerm = 0;
float dTerm = 0;
float last_error = 0;
int STD_LOOP_TIME = 10;
int lastLoopTime = STD_LOOP_TIME;
int lastLoopUsefulTime = STD_LOOP_TIME;
unsigned long loopStartTime = 0;
int power = 0;
int adcValue_1, adcValue_2, adcValue_3, adcValue_4, adcValue_5, adcValue_6, adcValue_7, adcValue_8, s3, s6;
int adcValue_1b, adcValue_2b, adcValue_3b, adcValue_4b, adcValue_5b, adcValue_6b, adcValue_7b, adcValue_8b;
bool adcflaq_1, adcflaq_2, adcflaq_3, adcflaq_4, adcflaq_5, adcflaq_6, adcflaq_7, adcflaq_8;
bool adcflaq_1b, adcflaq_2b, adcflaq_3b, adcflaq_4b, adcflaq_5b, adcflaq_6b, adcflaq_7b, adcflaq_8b;
/////////////////หน้า//////////////////////////////
int cenValue_1 = 650;
int cenValue_2 = 650;
int cenValue_3 = 650;
int cenValue_4 = 650;
int cenValue_5 = 650;
int cenValue_6 = 650;
int cenValue_7 = 650;
int cenValue_8 = 650;
///////////////// หลัง//////////////////////////////
int cenValue_1b = 650;
int cenValue_2b = 650;
int cenValue_3b = 650;
int cenValue_4b = 650;
int cenValue_5b = 650;
int cenValue_6b = 650;
int cenValue_7b = 650;
int cenValue_8b = 650;
#define A3 650
#define A6 650
//////////////////////// pid //////////////////////////
float Kp = 0.2; ///2.2///2.4///
float Ki = 0;
float Kd = Kp + 0.7; //KP+5///KP+6///
int sp = 100; ///เปรม 230////250///
//////////////////////////////// setting control /////////////////////////////////////////
void run(int sl, int sr) {
if (sr > 255)
sr = 255;
else if (sr < -255)
sr = -255;
if (sl > 255)
sl = 255;
else if (sl < -255)
sl = -255;
if (sr > 0) {
digitalWrite(MRD1, LOW);
digitalWrite(MRD2, HIGH);
analogWrite(MREN, sr);
} else if (sr < 0) {
digitalWrite(MRD1, HIGH);
digitalWrite(MRD2, LOW);
analogWrite(MREN, -sr);
} else {
digitalWrite(MRD1, HIGH);
digitalWrite(MRD2, HIGH);
analogWrite(MREN, 255);
}
if (sl > 0) {
digitalWrite(MLD1, LOW);
digitalWrite(MLD2, HIGH);
analogWrite(MLEN, sl);
} else if (sl < 0) {
digitalWrite(MLD1, HIGH);
digitalWrite(MLD2, LOW);
analogWrite(MLEN, -sl);
} else {
digitalWrite(MLD1, HIGH);
digitalWrite(MLD2, HIGH);
analogWrite(MLEN, 255);
}
}
void move_robot(int spp) {
if (spp >= 0) {
if (spp > sp) {
run(sp, -(spp - sp));
} else {
run(sp, sp - spp);
}
} else if (spp < 0) {
if (spp < -sp) {
run(spp + sp, sp);
} else {
run(sp + spp, sp);
}
}
}
int updatePid(int currentPosition) {
currentPosition = currentPosition / 20;
int error = 0 - currentPosition;
pTerm = Kp * error;
integrated_error += error;
iTerm = Ki * constrain(integrated_error, -sp, sp);
dTerm = Kd * (error - last_error);
last_error = error;
return constrain((pTerm + iTerm + dTerm), -(sp * 2), sp * 2);
}
void readADC() {
////////// หน้า /////////////
adcValue_1 = adc.readADC(0);
adcValue_2 = adc.readADC(1);
adcValue_3 = adc.readADC(2);
adcValue_4 = adc.readADC(3);
adcValue_5 = adc.readADC(4);
adcValue_6 = adc.readADC(5);
adcValue_7 = adc.readADC(6);
adcValue_8 = adc.readADC(7);
////////// หลัง /////////////
adcValue_1b = adc2.readADC(0);
adcValue_2b = adc2.readADC(1);
adcValue_3b = adc2.readADC(2);
adcValue_4b = adc2.readADC(3);
adcValue_5b = adc2.readADC(4);
adcValue_6b = adc2.readADC(5);
adcValue_7b = adc2.readADC(6);
adcValue_8b = adc2.readADC(7);
s3 = analogRead(A3);
s6 = analogRead(A6);
#define midpoint < //line black < ,line white >
if (adcValue_1 midpoint cenValue_1) {
adcflaq_1 = 1;
} else {
adcflaq_1 = 0;
}
if (adcValue_2 midpoint cenValue_2) {
adcflaq_2 = 1;
} else {
adcflaq_2 = 0;
}
if (adcValue_3 midpoint cenValue_3) {
adcflaq_3 = 1;
} else {
adcflaq_3 = 0;
}
if (adcValue_4 midpoint cenValue_4) {
adcflaq_4 = 1;
} else {
adcflaq_4 = 0;
}
if (adcValue_5 midpoint cenValue_5) {
adcflaq_5 = 1;
} else {
adcflaq_5 = 0;
}
if (adcValue_6 midpoint cenValue_6) {
adcflaq_6 = 1;
} else {
adcflaq_6 = 0;
}
if (adcValue_7 midpoint cenValue_7) {
adcflaq_7 = 1;
} else {
adcflaq_7 = 0;
}
if (adcValue_8 midpoint cenValue_8) {
adcflaq_8 = 1;
} else {
adcflaq_8 = 0;
}
if (adcValue_1b midpoint cenValue_1b) {
adcflaq_1b = 1;
} else {
adcflaq_1b = 0;
}
if (adcValue_2b midpoint cenValue_2b) {
adcflaq_2b = 1;
} else {
adcflaq_2b = 0;
}
if (adcValue_3b midpoint cenValue_3b) {
adcflaq_3b = 1;
} else {
adcflaq_3b = 0;
}
if (adcValue_4b midpoint cenValue_4b) {
adcflaq_4b = 1;
} else {
adcflaq_4b = 0;
}
if (adcValue_5b midpoint cenValue_5b) {
adcflaq_5b = 1;
} else {
adcflaq_5b = 0;
}
if (adcValue_6b midpoint cenValue_6b) {
adcflaq_6b = 1;
} else {
adcflaq_6b = 0;
}
if (adcValue_7b midpoint cenValue_7b) {
adcflaq_7b = 1;
} else {
adcflaq_7b = 0;
}
if (adcValue_8b midpoint cenValue_8b) {
adcflaq_8b = 1;
} else {
adcflaq_8b = 0;
}
}
//////////////////////////////// setting general ////////////////////////////////////////
void sensor() {
while (true) {
int vr = analogRead(A7); // กำหนดตัวแปรจำนวนเต็มอ่านค่าอนาล็อกที่พอร์ต 7
int nob = map(vr, 0, 1023, 0, -127); // ทำการ map อัตราส่วนจากสัญญาณ analog 0-1023 เป็น 0-180
readADC();
OLED.clearDisplay();
OLED.setTextColor(WHITE, BLACK); //สีอักษรเป็นสีขาว ,พื้นหลังดำ
OLED.setCursor(0, nob); // เซตตำแหน่ง 0,0
OLED.setTextSize(1); // เซตขนาดอักษรมีขนาดเป็น 1
OLED.print("s1 = ");
OLED.println(adcValue_1); // แสดงค่าเซนเซอร์ S1
OLED.print("S2 = ");
OLED.println(adcValue_2); // แสดงค่าเซนเซอร์ S2
OLED.print("S3 = ");
OLED.println(adcValue_3); // แสดงค่าเซนเซอร์ S3
OLED.print("S4 = ");
OLED.println(adcValue_4); // แสดงค่าเซนเซอร์ S4
OLED.print("S5 = ");
OLED.println(adcValue_5); // แสดงค่าเซนเซอร์ S5
OLED.print("s6 = ");
OLED.println(adcValue_6); // แสดงค่าเซนเซอร์ S6
OLED.print("S7 = ");
OLED.println(adcValue_7); // แสดงค่าเซนเซอร์ S7
OLED.print("S8 = ");
OLED.println(adcValue_8); // แสดงค่าเซนเซอร์ S8
OLED.print("s1b = ");
OLED.println(adcValue_1b); // แสดงค่าเซนเซอร์ S1
OLED.print("S2b = ");
OLED.println(adcValue_2b); // แสดงค่าเซนเซอร์ S2
OLED.print("S3b = ");
OLED.println(adcValue_3b); // แสดงค่าเซนเซอร์ S3
OLED.print("S4b = ");
OLED.println(adcValue_4b); // แสดงค่าเซนเซอร์ S4
OLED.print("S5b = ");
OLED.println(adcValue_5b); // แสดงค่าเซนเซอร์ S5
OLED.print("s6b = ");
OLED.println(adcValue_6b); // แสดงค่าเซนเซอร์ S6
OLED.print("S7b = ");
OLED.println(adcValue_7b); // แสดงค่าเซนเซอร์ S7
OLED.print("S8b = ");
OLED.println(adcValue_8b); // แสดงค่าเซนเซอร์ S8
OLED.print("A3 = ");
OLED.println(s3); // แสดงค่าเซนเซอร์ S1
OLED.print("A6 = ");
OLED.println(s6); // แสดงค่าเซนเซอร์ S2
OLED.display();
delay(50);
}
}
void sv_knob() {
while (true) {
int vr = analogRead(A7); // กำหนดตัวแปรจำนวนเต็มอ่านค่าอนาล็อกที่พอร์ต 7
int nob = map(vr, 0, 1023, 0, 180); // ทำการ map อัตราส่วนจากสัญญาณ analog 0-1023 เป็น 0-180
OLED.clearDisplay(); // เคลียร์หน้าจอ oled
OLED.setTextColor(WHITE, BLACK); //สีอักษรเป็นสีขาว ,พื้นหลังดำ
OLED.setCursor(0, 0); // เซตตำแหน่ง 0,0
OLED.setTextSize(2); // เซตขนาดอักษรมีขนาดเป็น 2
OLED.print("Servo angle = "); // พิมพ์คำว่า Servo angle
OLED.println(nob); // นำค่า nob มาแสดงใน oled
OLED.display(); // เปิดฟังก์ชันแสดงผล
sv1.write(nob); // สั่งเซอร์โวมอเตอร์ให้หมุนไปตามค่าองศาที่ทำการ nob ไว้
sv2.write(nob); // สั่งเซอร์โวมอเตอร์ให้หมุนไปตามค่าองศาที่ทำการ nob ไว้
delay(50); // หน่วงเวลา 0.05 วินาที
}
}
//////////////////////////////// setting line track /////////////////////////////////////
void linetrack_no_loop() {
while (1) {
readADC();
if ((adcflaq_1 + adcflaq_2 + adcflaq_3 + adcflaq_4 + adcflaq_5 + adcflaq_6 + adcflaq_7 + adcflaq_8) > 0) {
error = ((adcflaq_1 * 3) + (adcflaq_2 * 2) + (adcflaq_3 * 0.5)) - ((adcflaq_6 * 0.5) + (adcflaq_7 * 2) + (adcflaq_8 * 3));
power = updatePid(error * 500);
move_robot(power);
} else {
power = updatePid(error * 500);
move_robot(power);
}
}
}
void PF() { //เดินหน้า
while (1) {
if (adcflaq_3 == 1) {
run(-180, 180);
delay(10);
}
if (adcflaq_4 == 1) {
run(120, 180);
delay(10);
}
if (adcflaq_5 == 1) {
run(180, 120);
delay(10);
}
if (adcflaq_6 == 1) {
run(180, -180);
delay(10);
}
if (s3 < A3 or s6 < A6) {
run(0, 0);
delay(100);
break;
} else {
run(100, 100);
}
}
}
void PB() { //ถอยหลัง
while (1) {
if (adcflaq_3b == 1) {
run(120, -120);
delay(10);
}
if (adcflaq_4b == 1) {
run(-180, -100);
delay(10);
}
if (adcflaq_5b == 1) {
run(-180, -100);
delay(10);
}
if (adcflaq_6b == 1) {
run(-180, 180);
delay(10);
}
if (s3 < A3 or s6 < A6) {
run(0, 0);
delay(100);
break;
} else {
run(-100, -100);
}
}
}
/////////////////////////เลี้ยวซ้าย///////////////////////////////////////////
void L() {
L0();
L1();
L2();
delay(100);
} /////// เลี้ยวซ้าย
void L0() {
while (1) {
readADC();
run(-150, 150);
if (adcflaq_1 == 1) {
run(0, 0);
break;
}
}
} //S0
void L1() {
while (1) {
readADC();
run(-150, 150);
if (adcflaq_2 == 1) {
run(0, 0);
break;
}
}
} //S1
void L2() {
while (1) {
readADC();
run(-150, 150);
if (adcflaq_4 == 1) {
run(0, 0);
break;
}
}
} //S2
//////////////////////เลี้ยวขวา//////////////////////////////////////////
void R() {
R0();
R1();
R2();
delay(100);
} ///////// เลี้ยว
void R0() {
while (1) {
readADC();
run(150, -150);
if (adcflaq_8 == 1) {
run(0, 0);
break;
}
}
} //S0
void R1() {
while (1) {
readADC();
run(150, -150);
if (adcflaq_7 == 1) {
run(0, 0);
break;
}
}
} //S1
void R2() {
while (1) {
readADC();
run(150, -150);
if (adcflaq_5 == 1) {
run(0, 0);
break;
}
}
} //S2
//////////////////////////////// setting servo ///////////////////////////////////////////
void setup() {
OLED.begin(SSD1306_SWITCHCAPVCC, 0x3C);
Serial.begin(9600);
adc.begin(10);
adc2.begin(14);
delay(300); //added delay to give wireless ps2 module some time to startup, before configuring it
sv1.attach(ss1); // เรียกใช้งานเตรียมตอบสนองเซอร์โว sv1
sv2.attach(ss2); // เรียกใช้งานเตรียมตอบสนองเซอร์โว sv2
}
void loop() {
int sw = digitalRead(button); // ให้ sw อ่านค่า digital จากพอร์ต 2(button)
int nob = analogRead(7); // ให้ nob เทียบเท่าค่า A7
int menu = map(nob, 0, 1023, 0, 9); // เทียบอัตราส่วนของพอร์ต A7 จาก 0-1023 เพื่อทำเป็นเมนู 0-12
OLED.clearDisplay(); // ล้างค่าหน้าจอ
OLED.setTextColor(WHITE, BLACK); //สีอักษรเป็นสีขาว ,พื้นหลังดำ
OLED.setCursor(0, 0); // เซตตำแหน่ง 0,0
OLED.setTextSize(2); // เซตขนาดอักษรมีขนาดเป็น 2
OLED.print(" "); // วรรค
OLED.println(menu); // แสดงค่า Menu ที่ได้จากการ map nob ให้เหลือ 0-12
OLED.setTextSize(1); // เซตขนาดอักษรมีขนาดเป็น 2
OLED.println(" Logi"); // พิมพ์คำว่า kruro
OLED.print(" "); // วรรค
OLED.println(" Robot"); // พิมพ์คำว่า Robot
OLED.display();
if ((sw == 1) and (menu == 0)) { sensor(); }
if ((sw == 1) and (menu == 1)) { sv_knob(); }
if ((sw == 1) and (menu == 2)) { menu2(); }
if ((sw == 1) and (menu == 3)) { menu3(); }
if ((sw == 1) and (menu == 4)) { menu4(); }
if ((sw == 1) and (menu == 5)) { menu5(); }
if ((sw == 1) and (menu == 6)) { menu6(); }
if ((sw == 1) and (menu == 7)) { menu7(); }
if ((sw == 1) and (menu == 8)) { menu8(); }
if ((sw == 1) and (menu == 9)) { menu9(); }
delay(50);
}
void menu2() {
run(255, 255);
delay(1000);
run(0, 0);
delay(1000);
run(-255, 255);
delay(1000);
run(0, 0);
delay(1000);
run(255, -255);
delay(1000);
run(0, 0);
delay(1000);
run(-255, -255);
delay(1000);
run(0, 0);
delay(1000);
}
void menu3() {
PF();
PF();
L();
up();
down();
}
void menu4() {
up();
down();
}
void menu5() {
keep();
pasue();
}
void menu6() {
}
void menu7() {
}
void menu8() {
}
void menu9() {
}