//////// ผนวกไลบรารี่ ////////////
#include <ESP32Servo.h>
#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
//#include <Adafruit_Sensor.h>
#include <Adafruit_HMC5883_U.h>
#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels
#define OLED_RESET -1 // 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);
Adafruit_HMC5883_Unified mag = Adafruit_HMC5883_Unified(12345);
///////////ตั้งค่าปุ่มกด///////////////////
int button = 34; /// กำหนดปุ่มกดสวิตซ์ขา 27
int snd = 12;
int ss1 = 23; // เปิดใช้งาน s1 ที่พอร์ต 19
int ss2 = 19; // เปิดใช้งาน s1 ที่พอร์ต 23
int ss3 = 18; // เปิดใช้งาน s1 ที่พอร์ต 18
Servo sv1; // ประกาศการใช้งาน sv1
Servo sv2; // ประกาศการใช้งาน sv2
Servo sv3; // ประกาศการใช้งาน sv3
#define Pdegree 140 //องศาที่ต้องการเดินข้ามพื้นสีเขียว
///////////////////////องศา เซอร์โว///////////////////////
///////////////////////องศา เซอร์โว เริ่มต้น///////////////////////
const int v1_set0 = 180; //ตัวยก ตำแหน่งรอรับกระป๋อง
const int v2_set0 = 30; //ตัวหนีบอ้า
const int v3_set0 = 30; //ธง
/////เซอโวตอนหนีบ/////
const int v2_set1 = 100; //ตัวหนีบหนีบกระป๋อง
/////เซอโวตอนยกขึ้นก่อนวาง/////
const int v1_set1 = 20;
/////เซอโวตอนขึ้นธง ////
const int v3_set1 = 120; //ธง ชักสูงสุด
const int v3_set2 = 90; //ธง ชักครึ่ง
////////////ตั้งค่าพอร์ตมอเตอร์////////////////////
#define DL1 2 /// กำหนดสัญญาณดิจิตอลซ้ายที่ 1 พอร์ต 15
#define DL2 15 /// กำหนดสัญญาณดิจิตอลซ้ายที่ 2 พอร์ต 2
#define PWML 13 /// กำหนดสัญญาณ PWM ซ้ายพอร์ต 13
/////////////////////////////////
#define DR1 17 // กำหนดสัญญาณดิจิตอลขวาที่ 1 พอร์ต 16
#define DR2 16 // กำหนดสัญญาณดิจิตอลขวาที่ 2 พอร์ต 17
#define PWMR 4 /// กำหนดสัญญาณ PWM ขวาพอร์ต 4
/////////////////ตั้งค่าพอร์ต Analog//////////////
#define A0 2000 // ค่ากลางเซนเซอร์ A0 (ซ้ายสุด)
#define A1 2000 // ค่ากลางเซนเซอร์ A1 (ซ้ายกลาง)
#define A2 2000 // ค่ากลางเซนเซอร์ A2 (กลาง)
#define A3 2000 // ค่ากลางเซนเซอร์ A3 (ขวากลาง)
#define A6 2000 // ค่ากลางเซนเซอร์ A6 (หลัง)
//////ตั้งค่าการเดินให้ตรงลดกำลังมอเตอร์////////
#define SpeedOffsetMotorL 0
#define SpeedOffsetMotorR 2
///////////////////////////////////////////////
int s0,s1,s2,s3,s6 ;
void analogs() /////จะให้ดี ใช้ 39 36
{
s0 = analogRead(14);
s1 = analogRead(25);
s2 = analogRead(26);
s3 = analogRead(27);
s6 = analogRead(39);
}
//////////////////////////////////////////////
void setup() {
display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS); // กำหนดแอดเดรสของพอร์ตจอเป็น 0x3C (for the 128x64)
pinMode(34, INPUT); // ตั้งค่าขา 34 เป็น INPUT
pinMode(s0, INPUT); // ตั้งค่าขา s0 เป็น INPUT
pinMode(s1, INPUT); // ตั้งค่าขา s1 เป็น INPUT
pinMode(s2, INPUT); // ตั้งค่าขา s2 เป็น INPUT
pinMode(s3, INPUT); // ตั้งค่าขา s3 เป็น INPUT
pinMode(s6, INPUT); // ตั้งค่าขา s6 เป็น INPUT
////////กำหนดสัญญาณทุกขาเป็น OUTPUT /////////
pinMode(12, OUTPUT);
pinMode(DL1, OUTPUT);
pinMode(DL2, OUTPUT);
pinMode(PWML, OUTPUT);
pinMode(DR1, OUTPUT);
pinMode(DR2, OUTPUT);
pinMode(PWMR, OUTPUT);
pinMode(ss1, OUTPUT);
pinMode(ss2, OUTPUT);
pinMode(ss3, OUTPUT);
sv1.attach(ss1); // เรียกใช้งานเตรียมตอบสนองเซอร์โว sv1
sv2.attach(ss2); // เรียกใช้งานเตรียมตอบสนองเซอร์โว sv2
sv3.attach(ss3); // เรียกใช้งานเตรียมตอบสนองเซอร์โว sv3
}
void servostart(){
sv1.write(v1_set0);
sv2.write(v2_set0);
sv3.write(v3_set0);
}
void servorun()
{
sv1.write(v1_set0);
sv2.write(v2_set1);
sv3.write(v3_set0);
}
void servoup()
{
sv1.write(v1_set1);
delay(2000);
sv2.write(v2_set0);
}
void servokeep()
{
sv1.write(v1_set0);
sv2.write(v2_set1);
sv3.write(v3_set0);
delay(300);
}
void servowait() ///รอรับกระป๋อง
{
while(1){
analogs();
if (s6 > A6){
delay(3000);servokeep();break;
}
}
}
void servoflag() ///สะบัดธงไปมา 5 วินาที
{
sv3.write(v3_set1);
delay(500);
sv3.write(v3_set2);
delay(500);
sv3.write(v3_set1);
delay(500);
sv3.write(v3_set2);
delay(500);
sv3.write(v3_set1);
delay(500);
sv3.write(v3_set2);
delay(500);
sv3.write(v3_set1);
delay(500);
sv3.write(v3_set2);
delay(500);
sv3.write(v3_set1);
delay(500);
sv3.write(v3_set2);
delay(500);
sv3.write(v3_set1);
delay(500);
}
void P() ///////////// ฟังก์ชันแทกเส้นเจอคู่ดำ เดินหน้านิดนึงแล้วหยุด/////////////////////////////
{ while (1) {
analogs();
if ((s1 > A1) and (s2 > A2)) {
run(250, 250);
}
if (s1 < A1) {
run(-250, 250);
}
if (s2 < A2) {
run(250, -250);
}
if (((s1 < A1) and (s2 < A2)) or ((s1 < A1) and (s0 < A0)) or ((s2 < A2) and (s3 < A3)))
{
run(250, 250);
delay(550);
run(0, 0);
delay(100);
break;
}
}
}
void PP() ///////////// ฟังก์ชันแทกเส้นเจอคู่ดำหยุดเลยทันที/////////////////////////////ใช้เช๋คก่อนถึงสีเขียว
{ while (1) {
analogs();
if ((s1 > A1) and (s2 > A2)) {
run(180, 190);
}
if (s1 < A1) {
run(-180, 180);
}
if (s2 < A2) {
run(180, -180);
}
if ((s0 < A0) and (s3 < A3))
{
run(0, 0);
delay(200);
break;
}
if (s0 < A0) {
run(-250, 180);
}
if (s3 < A3) {
run(180, -250);
}
}
}
/////////////////////ฟังก์ชั่นเดินตามทิศทาง/////////ใช้ตอนอยู่โซนสีเขียว
void p(int cp,int t) {
// Serial.print("Heading (degrees): "); Serial.println(headingDegrees);
int t1 = millis();
while (1) {
sensors_event_t event;
mag.getEvent(&event);
float heading = atan2(event.magnetic.y, event.magnetic.x);
float declinationAngle = 0.22;
heading += declinationAngle;
// Correct for when signs are reversed.
if(heading < 0)
heading += 2*PI;
// Check for wrap due to addition of declination.
if(heading > 2*PI)
heading -= 2*PI;
// Convert radians to degrees for readability.
float headingDegrees = heading * 180/M_PI;
int t2 = millis();
if ((t2-t1)>t){run(0,0);break;}
if (headingDegrees > (cp + 3)) {run(80, 180);}
else if (headingDegrees < (cp - 3)) {run(180, 80);}
else{run(180,185);}
display.clearDisplay();
display.setTextColor(WHITE,BLACK); //สีอักษรเป็นสีขาว ,พื้นหลังดำ
display.setCursor(0, 0); // เซตตำแหน่ง 0,0
display.setTextSize(2); // เซตขนาดอักษรมีขนาดเป็น 2
display.println("Degrees = ");
display.println(headingDegrees);
display.display();
delay(20);
}}
///////////วัดค่ามุม
void cpp(){
// int x, y, z;int azimuth;
while (1) {
// qmc.read(&x, &y, &z, &azimuth);
sensors_event_t event;
mag.getEvent(&event);
float heading = atan2(event.magnetic.y, event.magnetic.x);
float declinationAngle = 0.22;
heading += declinationAngle;
// Correct for when signs are reversed.
if(heading < 0)
heading += 2*PI;
// Check for wrap due to addition of declination.
if(heading > 2*PI)
heading -= 2*PI;
// Convert radians to degrees for readability.
float headingDegrees = heading * 180/M_PI;
// Serial.print("Heading (degrees): "); Serial.println(headingDegrees);
display.clearDisplay();
display.setTextColor(WHITE,BLACK); //สีอักษรเป็นสีขาว ,พื้นหลังดำ
display.setCursor(0, 0); // เซตตำแหน่ง 0,0
display.setTextSize(2); // เซตขนาดอักษรมีขนาดเป็น 2
display.println("degree = ");
display.println(headingDegrees);
display.display();
delay(20);
}}
void PW() ///////////// ฟังก์ชันแทกเส้นเจอคู่ดำหยุด ใช้กรณี อ่านสีฟ้า แล้ว เจอดำ/////////////////////////////
{ while (1) {
analogs();
if ((s1 < A1) and (s2 < A2)) {
run(150, 150);
}
if (s1 > A1) {
run(-200, 200);
}
if (s2 > A2) {
run(200, -200);
}
if (((s1 > A1) and (s2 > A2)) or ((s1 > A1) and (s0 > A0)))
{
run(200, 200);
delay(500);
run(0, 0);
delay(100);
break;
}
}
}
void L() {
run(-250, 250); /////// เลี้ยวซ้าย
delay(450 );
L1();
delay(100);
}
void R() {
run(250, -250); ///////// เลี้ยวขวา
delay(650);
R1();
delay(100);
}
void L0() {
while (1) {
analogs();
run(-200, 200); if (s0 < A0) {
run(0, 0); //S0
break;
}
}
}
void L1() {
while (1) {
analogs();
run(-200, 200); if (s1 < A1) {
run(0, 0); //s1
break;
}
}
}
void L2() {
while (1) {
analogs();
run(-200, 200); if (s2 < A2) {
run(0, 0); //s2
break;
}
}
}
void L3() {
while (1) {
analogs();
run(-200, 200); if (s3 < A3) {
run(0, 0); //s3
break;
}
}
}
//////////////////////เลี้ยวขวา//////////////////////////////////////////
void R0() {
while (1) {
analogs();
run(200, -200); if (s0 < A0) {
run(0, 0); //S0
break;
}
}
}
void R1() {
while (1) {
analogs();
run(200, -200); if (s1 < A1) {
run(0, 0); //s1
break;
}
}
}
void R2() {
while (1) {
analogs();
run(200, -200); if (s2 < A2) {
run(0, 0); //s2
break;
}
}
}
void R3() {
while (1) {
analogs();
run(200, -200); if (s3 < A3) {
run(0, 0); //s3
break;
}
}
}
void loop() {
int sw = digitalRead(button); // ให้ sw อ่านค่า digital จากพอร์ต 2(button)
int nob = analogRead(35); // ให้ nob เทียบเท่าค่า 35
int menu = map(nob, 0, 4094, 0, 9); // เทียบอัตราส่วนของพอร์ต 35 จาก 0-4095 เพื่อทำเป็นเมนู 0-9
display.clearDisplay(); // ล้างค่าหน้าจอ
display.setTextColor(WHITE, BLACK); //สีอักษรเป็นสีขาว ,พื้นหลังดำ
display.setCursor(0, 0); // เซตตำแหน่ง 0,0
display.setTextSize(2); // เซตขนาดอักษรมีขนาดเป็น 2
display.print(" "); // วรรค
display.println(menu); // แสดงค่า Menu ที่ได้จากการ map nob ให้เหลือ 0-9
display.setTextSize(1); // เซตขนาดอักษรมีขนาดเป็น 2
display.println(" KruRo"); // พิมพ์คำว่า kruro
display.print(" "); // วรรค
display.print(nob); // แสดงค่าที่อ่านได้จาก nob หรือ 35
display.println(" Robot"); // พิมพ์คำว่า Robot
display.display();
//////////////////////////////////////////////////////
if ((sw == 1) and (menu == 0)) {sensor();}
if ((sw == 1) and (menu == 1)){set_servo();}
if ((sw == 1) and (menu == 2)){cpp();}
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(100);
}
////////////////////////////////////////////////
void sensor()
{
while (true) {
analogs();
display.clearDisplay();
display.setTextColor(WHITE, BLACK); //สีอักษรเป็นสีขาว ,พื้นหลังดำ
display.setCursor(0, 0); // เซตตำแหน่ง 0,0
display.setTextSize(1); // เซตขนาดอักษรมีขนาดเป็น 1
display.print(" S0 = "); display.println(s0); // แสดงค่าเซนเซอร์ S0
display.print(" s1 = "); display.println(s1); // แสดงค่าเซนเซอร์ s1
display.print(" s2 = "); display.println(s2); // แสดงค่าเซนเซอร์ s2
display.print(" s3 = "); display.println(s3); // แสดงค่าเซนเซอร์ s3
display.display();
delay(50);
}
}
/////////////////////////////ฟังก์ชันเซอร์โว////////////////////////////////////////
void set_servo() {
while(true){
int vr = analogRead(35); // กำหนดตัวแปรจำนวนเต็มอ่านค่าอนาล็อกที่พอร์ต 35
int nob = map(vr, 0, 4096, 0, 180); // ทำการ map อัตราส่วนจากสัญญาณ analog 0-1023 เป็น 0-180
display.clearDisplay(); // เคลียร์หน้าจอ oled
display.setTextColor(WHITE, BLACK); //สีอักษรเป็นสีขาว ,พื้นหลังดำ
display.setCursor(0, 0); // เซตตำแหน่ง 0,0
display.setTextSize(1); // เซตขนาดอักษรมีขนาดเป็น 2
display.print("Servo1 = "); // พิมพ์คำว่า Servo1 =
display.print("Servo2 = "); // พิมพ์คำว่า Servo2 =
display.print("Servo3 = "); // พิมพ์คำว่า Servo3 =
display.println(nob); // นำค่า nob มาแสดงใน oled
display.display(); // เปิดฟังก์ชันแสดงผล
sv1.write(nob); // สั่งเซอร์โวมอเตอร์ให้หมุนไปตามค่าองศาที่ทำการ nob ไว้
sv2.write(nob); // สั่งเซอร์โวมอเตอร์ให้หมุนไปตามค่าองศาที่ทำการ nob ไว้
sv3.write(nob); // สั่งเซอร์โวมอเตอร์ให้หมุนไปตามค่าองศาที่ทำการ nob ไว้
delay(50); // หน่วงเวลา 0.05 วินาที
}
}
/////////////////////////////////////////////////////
///////////////////////////////// เรียกใช้ code menu ////////////
void menu3() /// code 3 ที่นี่
{
p(140,12000);run(0,0);delay(500);
// servokeep(); delay(1000);servoup();
//run(200,200);delay(3000);run(0,0);
}
void menu4() /// code 4 ที่นี่
{
servostart();delay(1000);servowait(); P();run(140,130);delay(700);run(0,0);delay(100);servoup();delay(100); servoflag();
}
void menu5() /// code 5 ที่นี่
{
//servostart();delay(1000);servowait();servoup();delay(5000);servoflag();
servostart();delay(1000);servowait();delay(3000);run(150,140);delay(1300);P();L();P();L();P();R();PP(); p(Pdegree,8000);run(0,0);delay(500);
P();R(); P();R();P();P();run(140,130);delay(700);run(0,0);delay(100);servoup();delay(100); servoflag();
}
void menu6() /// code 6 ที่นี่
{
servokeep();delay(1000);run(150,140);delay(1300);P();L();P();L();P();R();PP(); p(Pdegree,8000);run(0,0);delay(500);
P();R(); P();R();P();P();run(140,130);delay(700);run(0,0);delay(100);servoup();delay(100); servoflag();}
void menu7() /// code 7 ที่นี่
{
//run(235,250);delay(9000);run(0,0);delay(500);
//servostart();delay(1000);servowait();
servokeep();delay(1000);run(150,140);delay(1300);
P();R();P();P();run(140,130);delay(700);run(0,0);delay(100);servoup();delay(100); servoflag();
}
void menu8() /// code 7 ที่นี่
{servostart();delay(1000);servokeep();delay(1000);run(150,140);delay(1300);P();L();P();L();P();R();PP();run(190,190);delay(5000);
PP();P();R();PP();P();R();P();P();run(140,130);delay(700);run(0,0);delay(100);servoup();delay(100); servoflag();
}
void menu9() /// code 9 ที่นี่
{
}
/////////////////////////////ฟังก์ชันมอเตอร์///////////////////////////////////
void run(int spl, int spr) // ประกาศฟังก์ชัน run(กำลังมอเตอร์ซ้าาย,กำลังมอเตอร์ขวา);
{ if (spl==0 and spr==0){
digitalWrite(DL1, HIGH);
digitalWrite(DL2, HIGH);
digitalWrite(DR1, HIGH);
digitalWrite(DR2, HIGH);
}
else if (spl>0 and spr>0) ///เดินหน้า
{ spl=spl+SpeedOffsetMotorL;
spr=spr+SpeedOffsetMotorR;
digitalWrite(DL1, LOW);
digitalWrite(DL2, HIGH);
digitalWrite(DR1, HIGH);
digitalWrite(DR2, LOW);
analogWrite(PWMR, spr);
analogWrite(PWML, spl);
}
else if (spl<0 and spr<0) //ถอยหลัง
{ spl=spl-SpeedOffsetMotorL;
spr=spr-SpeedOffsetMotorR;
digitalWrite(DR1, LOW);
digitalWrite(DR2, HIGH);
analogWrite(PWMR, -spr);
digitalWrite(DL1, HIGH);
digitalWrite(DL2, LOW);
analogWrite(PWML, -spl);
}
else if (spl<0 and spr>0) //เลี้ยวซ้ายสปิน
{ spl=spl-SpeedOffsetMotorL;
spr=spr+SpeedOffsetMotorR;
digitalWrite(DL1, HIGH);
digitalWrite(DL2, LOW);
digitalWrite(DR1, HIGH);
digitalWrite(DR2, LOW);
analogWrite(PWMR, spr);
analogWrite(PWML, -spl);}
else if (spl<0 and spr==0) //เลี้ยวซ้ายเทิร์น
{ spl=spl-SpeedOffsetMotorL;
//spr=spr-SpeedOffsetMotorR;
digitalWrite(DL1, HIGH);
digitalWrite(DL2, LOW);
digitalWrite(DR1, HIGH);
digitalWrite(DR2, HIGH);
analogWrite(PWMR, spr);
analogWrite(PWML, -spl);}
else if (spl>0 and spr<0) //เลี้ยวขวาสปิน
{ spl=spl+SpeedOffsetMotorL;
spr=spr-SpeedOffsetMotorR;
digitalWrite(DL1, LOW);
digitalWrite(DL2, HIGH);
digitalWrite(DR1, LOW);
digitalWrite(DR2, HIGH);
analogWrite(PWMR, -spr);
analogWrite(PWML, spl);}
else if (spl>0 and spr==0)//เลี้ยวขวาเทิร์น
{ spl=spl+SpeedOffsetMotorL;
//spr=spr-SpeedOffsetMotorR;
digitalWrite(DL1, LOW);
digitalWrite(DL2, HIGH);
digitalWrite(DR1, HIGH);
digitalWrite(DR2, HIGH);
analogWrite(PWMR, spr);
analogWrite(PWML, spl);}
else if (spl==0 and spr>0)//เลี้ยวขวาเทิร์น
{ //spl=spl+SpeedOffsetMotorL;
spr=spr+SpeedOffsetMotorR;
digitalWrite(DL1, HIGH);
digitalWrite(DL2, HIGH);
digitalWrite(DR1, HIGH);
digitalWrite(DR2, LOW);
analogWrite(PWMR, spr);
analogWrite(PWML, spl);}
else if (spl==0 and spr<0)//เลี้ยวขวาเทิร์น
{ //spl=spl+SpeedOffsetMotorL;
spr=spr-SpeedOffsetMotorR;
digitalWrite(DL1, HIGH);
digitalWrite(DL2, HIGH);
digitalWrite(DR1, LOW);
digitalWrite(DR2, HIGH);
analogWrite(PWMR, -spr);
analogWrite(PWML, spl);}
}
//////////////////////////////////////
#include <Ps3Controller.h>
#include <ESP32Servo.h>
#define DR1 16 //2
#define DR2 17 //15
#define PWMR 4 //13
/////////////////////////////////
#define DL1 2 //16
#define DL2 15 //17
#define PWML 13 //4
int ss1 = 5; // เปิดใช้งาน s2 ที่พอร์ต 3
int ss2 = 18; // เปิดใช้งาน s3 ที่พอร์ต
Servo sv1; // ประกาศการใช้งาน sv1
Servo sv2;
#define pos1min 0 //ค่าตำแหน่งน้อยสุดที่ยอมให้เซอโว 1 ทำงาน (ตัวยก)
#define pos1max 130 //ค่าตำแหน่งมากสุดที่ยอมให้เซอโว 1 ทำงาน (ตัวยก)
#define pos2min 0 //ค่าตำแหน่งน้อยสุดที่ยอมให้เซอโว 2 ทำงาน (ตัวหนีบ)
#define pos2max 100 //ค่าตำแหน่งมากสุดที่ยอมให้เซอโว 2 ทำงาน (ตัวหนีบ)
int pos1 = 0;
int pos2 = 0;
void onConnect() {
Serial.println("Connected!.");
}
void onDisConnect() {
run(0, 0);
}
void setup() {
pinMode(DL1, OUTPUT);
pinMode(DL2, OUTPUT);
pinMode(PWML, OUTPUT);
pinMode(DR1, OUTPUT);
pinMode(DR2, OUTPUT);
pinMode(PWMR, OUTPUT);
Serial.begin(115200);
Ps3.attachOnConnect(onConnect);
Ps3.attachOnDisconnect(onDisConnect);
Ps3.begin("08:3a:f2:ac:52:da"); //ใส่ MacAddress
Serial.println("Ready.");
sv1.attach(ss1); // เรียกใช้งานเตรียมตอบสนองเซอร์โว sv1
sv2.attach(ss2); // เรียกใช้งานเตรียมตอบสนองเซอร์โว sv2
sv1.write(pos1);
sv2.write(pos2);
}
void loop() {
int yAxisValue = (Ps3.data.analog.stick.ly); //Left stick - y axis - forward/backward car movement
int xAxisValue = (Ps3.data.analog.stick.rx); //Right stick - x axis - left/right car movement
if (yAxisValue <= -50) //ถอยหลัง
{
run(-200, -200);
}
else if (yAxisValue >= 50)
{
run(200, 200);
}
else if (xAxisValue >= 50)
{
run(200, -200);
}
else if (xAxisValue <= -50)
{
run(-200, 200);
}
else if (Ps3.data.button.left)
{
run(-230, 230);
}
else if (Ps3.data.button.right)
{
run(230, -230);
}
else if (Ps3.data.button.up) //เดินหน้า
{
run(235, 230);
}
else if (Ps3.data.button.down) //ถอยหลัง
{
run(-230, -230);
}
else if (Ps3.data.button.triangle) {
run(250, 250);
}
else if (Ps3.data.button.cross) {
run(-250, -250);
}
else if (Ps3.data.button.square) {
run(-250, 250);
}
else if (Ps3.data.button.circle)
{
run(250, -250);
}
else if (Ps3.data.button.l2) {
if (pos1 > pos1max) {
sv1.write(pos1max);
} else {
pos1 = pos1 + 5;
sv1.write(pos1);
}
} else if (Ps3.data.button.l1) {
if (pos1 < pos1min) {
sv1.write(pos1min);
} else {
pos1 = pos1 - 5;
sv1.write(pos1);
}
} else if (Ps3.data.button.r1) {
if (pos2 > pos2max) {
sv2.write(pos2max);
} else {
pos2 = pos2 + 4;
sv2.write(pos2);
}
} else if (Ps3.data.button.r2) {
if (pos2 < pos2min) {
sv2.write(pos2min);
} else {
pos2 = pos2 - 4;
sv2.write(pos2);
}
} else {
run(0, 0);
}
delay(200);
}
void run(int spl, int spr) // ประกาศฟังก์ชัน run(กำลังมอเตอร์ซ้าาย,กำลังมอเตอร์ขวา);
{
if (spl > 0) {
digitalWrite(DL1, LOW);
digitalWrite(DL2, HIGH);
analogWrite(PWML, spl);
} else if (spl < 0) {
digitalWrite(DL1, HIGH);
digitalWrite(DL2, LOW);
analogWrite(PWML, -spl);
} else {
digitalWrite(DL1, HIGH);
digitalWrite(DL2, HIGH);
}
//////////////////////////////////////
if (spr > 0) {
digitalWrite(DR1, LOW);
digitalWrite(DR2, HIGH);
analogWrite(PWMR, spr);
} else if (spr < 0) {
digitalWrite(DR1, HIGH);
digitalWrite(DR2, LOW);
analogWrite(PWMR, -spr);
} else {
digitalWrite(DR1, HIGH);
digitalWrite(DR2, HIGH);
}
}