ดาวน์โหลดและติตตั้งโปรแกรม sixaxispairtoolsetup-0.3.1
ค้นหา ชื่อบลูทูธ และ MacAddress
#include <Wire.h>
#include <Ps3Controller.h>
#include <ESP32Servo.h>
////////////ตั้งค่าพอร์ตมอเตอร์////////////////////
/////////////M1/////////////////
#define DL1 16
#define DL2 17
#define PWML 4
/////// M2 //////////////////////////////////
#define DR1 2
#define DR2 15
#define PWMR 13
////////M3////////////////////////////////////
#define DR1b 27
#define DR2b 26
#define PWMRb 25
////////M4////////////////////////////////////
#define DL1b 18
#define DL2b 19
#define PWMLb 5
int ss1 = 20; // เปิดใช้งาน s2 ที่พอร์ต 3
int ss2 = 21; // เปิดใช้งาน 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() {
Serial.begin(115200);
/////////////////////////////////////////////////
pinMode(DL1, OUTPUT);
pinMode(DL2, OUTPUT);
pinMode(PWML, OUTPUT);
pinMode(DR1, OUTPUT);
pinMode(DR2, OUTPUT);
pinMode(PWMR, OUTPUT);
////////////////////////////
pinMode(DL1b, OUTPUT);
pinMode(DL2b, OUTPUT);
pinMode(PWMLb, OUTPUT);
pinMode(DR1b, OUTPUT);
pinMode(DR2b, OUTPUT);
pinMode(PWMRb, OUTPUT);
Ps3.attachOnConnect(onConnect);
Ps3.attachOnDisconnect(onDisConnect);
Ps3.begin("7c:87:ce:cb:47:7e");//ใส่ MacAddress
Serial.println("Ready.");
sv1.attach(ss1); // เรียกใช้งานเตรียมตอบสนองเซอร์โว sv1
sv2.attach(ss2); // เรียกใช้งานเตรียมตอบสนองเซอร์โว sv2
////////////////////////////
}
void loop()
{
int lyAxisValue = (Ps3.data.analog.stick.ly); //Left stick - y axis - forward/backward car movement
int lxAxisValue = (Ps3.data.analog.stick.lx); //Right stick - x axis - left/right car movement
int ryAxisValue = (Ps3.data.analog.stick.ry); //Left stick - y axis - forward/backward car movement
int rxAxisValue = (Ps3.data.analog.stick.rx); //Right stick - x axis - left/right car movement
// Serial.println(" ");
int lthrottle = map( lyAxisValue, 127, -127, -255, 255);
int lsteering = map( lxAxisValue, -127, 127, -255, 255);
int rthrottle = map( ryAxisValue, 127, -127, -255, 255);
int rsteering = map( rxAxisValue, -127, 127, -255, 255);
///////////////////////////// analog ซ้าย /////////////////////////////////
if (lyAxisValue <= -50) //ถอยหลัง
{
moveBackward(200);
}
else if (lyAxisValue >= 50) //เดินหน้า
{
moveForward(200);
}
else if (lxAxisValue >= 50) //เลี้ยวขวา
{
turnRight(200);
}
else if (lxAxisValue <= -50) //เลี้ยวซ้าย
{
turnLeft(200);
}
///////////////////////////// analog ขวา /////////////////////////////////
if (ryAxisValue <= -50) //ถอยหลัง
{
moveBackward(200);
}
else if (ryAxisValue >= 50) //เดินหน้า
{
moveForward(200);
}
else if (rxAxisValue >= 50) //เลี้ยวขวา
{
turnRight(200);
}
else if (rxAxisValue <= -50) //เลี้ยวซ้าย
{
turnLeft(200);
}
///////////////////////////////////////////////////////////////////////////
else if ( Ps3.data.button.left ) //เลี้ยวซ้าย
{
turnLeft(250);Serial.println("Left");
}
else if ( Ps3.data.button.right )//เลี้ยวขวา
{
turnRight(250); Serial.println("right");
}
else if ( Ps3.data.button.up ) //เดินหน้า
{
moveForward(250); Serial.println("moveForward");
}
else if ( Ps3.data.button.down ) //ถอยหลัง
{
moveBackward(250); Serial.println("moveBackward");
}
else if ( Ps3.data.button.triangle )
{
moveForward(120); Serial.println("triangle");
}
else if ( Ps3.data.button.cross )
{
moveBackward(120); Serial.println("cross");
}
else if ( Ps3.data.button.square )
{
slideLeft(200); Serial.println("square");
}
else if ( Ps3.data.button.circle ) ///triangle
{
slideRight(200); Serial.println("circle");
}
else if(Ps3.data.button.l2) {
if (pos1>pos1max) {sv1.write(pos1max);}
else {
pos1=pos1+5 ;
sv1.write(pos1); Serial.println("pos1+5");
}
}
else if(Ps3.data.button.l1) {
if (pos1<pos1min) {sv1.write(pos1min);}
else {
pos1=pos1-5 ;
sv1.write(pos1); Serial.println("pos1-5");
}
}
else if(Ps3.data.button.r1) {
if (pos2>pos2max) {sv2.write(pos2max);}
else {
pos2=pos2+4 ;
sv2.write(pos2); Serial.println("pos2+5");
}
}
else if(Ps3.data.button.r2) {
if (pos2<pos2min) {sv2.write(pos2min);}
else { pos2=pos2-4 ;
sv2.write(pos2) ; Serial.println("pos1-5");
}
}
else
{
stop();
}
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);
}
}
/////////////////////////////////////////////////////////
void runb(int splb, int sprb) // มอเตอร์คู่หน้า run(กำลังมอเตอร์ซ้าาย,กำลังมอเตอร์ขวา);
{
if (splb > 0)
{
digitalWrite(DL1b, LOW);
digitalWrite(DL2b, HIGH);
analogWrite(PWMLb, splb);
}
else if (splb < 0)
{
digitalWrite(DL1b, HIGH);
digitalWrite(DL2b, LOW);
analogWrite(PWMLb, -splb);
}
else
{
digitalWrite(DL1b, HIGH);
digitalWrite(DL2b, HIGH);
}
//////////////////////////////////////
if (sprb > 0)
{
digitalWrite(DR1b, LOW);
digitalWrite(DR2b, HIGH);
analogWrite(PWMRb, sprb);
}
else if (sprb < 0)
{
digitalWrite(DR1b, HIGH);
digitalWrite(DR2b, LOW);
analogWrite(PWMRb, -sprb);
}
else
{
digitalWrite(DR1b, HIGH);
digitalWrite(DR2b, HIGH);
}
}
// เดินหน้า
void moveForward(int speed) {
run(speed, speed); // Motor A (left) และ Motor B (right) หมุนไปข้างหน้า
runb(speed, speed);
}
// ถอยหลัง
void moveBackward(int speed) {
run(-speed, -speed); // Motor A (left) และ Motor B (right) หมุนถอยหลัง
runb(-speed, -speed);
}
// เลี้ยวซ้าย
void turnLeft(int speed) {
run(-speed, speed); // Motor A (left) หมุนถอยหลัง และ Motor B (right) หมุนไปข้างหน้า
runb(-speed, speed);
}
// เลี้ยวขวา
void turnRight(int speed) {
run(speed, -speed); // Motor A (left) หมุนไปข้างหน้า และ Motor B (right) หมุนถอยหลัง
runb(speed, -speed);
}
// สไลด์ซ้าย
void slideLeft(int speed) {
run(-speed, speed); // ล้อหน้าและล้อหลังทางซ้ายหมุนไปข้างหน้า
runb(speed, -speed); // ล้อหน้าและล้อหลังทางขวาหมุนถอยหลัง
}
// สไลด์ขวา
void slideRight(int speed) {
run(speed, -speed); // ล้อหน้าและล้อหลังทางขวาหมุนไปข้างหน้า
runb(-speed, speed); // ล้อหน้าและล้อหลังทางซ้ายหมุนถอยหลัง
}
void stop() {
// หยุดมอเตอร์ A (ซ้าย)
digitalWrite(DL1, HIGH);
digitalWrite(DL2, HIGH);
analogWrite(PWML, 0); // หยุดการหมุนของมอเตอร์ซ้าย
// หยุดมอเตอร์ B (ขวา)
digitalWrite(DR1, HIGH);
digitalWrite(DR2, HIGH);
analogWrite(PWMR, 0); // หยุดการหมุนของมอเตอร์ขวา
// หยุดมอเตอร์ A (ซ้าย) บนตัวที่ 2
digitalWrite(DL1b, HIGH);
digitalWrite(DL2b, HIGH);
analogWrite(PWMLb, 0); // หยุดการหมุนของมอเตอร์ซ้าย
// หยุดมอเตอร์ B (ขวา) บนตัวที่ 2
digitalWrite(DR1b, HIGH);
digitalWrite(DR2b, HIGH);
analogWrite(PWMRb, 0); // หยุดการหมุนของมอเตอร์ขวา
}
/////////////////////////////////////////
// _________________________
// M1 M2
// M3 M4
#include <Wire.h>
#include <Ps3Controller.h>
#include <ESP32Servo.h>
////////////ตั้งค่าพอร์ตมอเตอร์////////////////////
/////////////M1/////////////////
#define DL1 16
#define DL2 17
#define PWML 4
/////// M2 //////////////////////////////////
#define DR1 2
#define DR2 15
#define PWMR 13
////////M3////////////////////////////////////
#define DR1b 27
#define DR2b 26
#define PWMRb 25
////////M4////////////////////////////////////
#define DL1b 18
#define DL2b 19
#define PWMLb 5
//////
//////
int ss1 = 20; // เปิดใช้งาน s2 ที่พอร์ต 3
int ss2 = 21; // เปิดใช้งาน s3 ที่พอร์ต
Servo sv1; // ประกาศการใช้งาน sv1
Servo sv2;
void onConnect()
{ Serial.println("Connected!.");
}
void onDisConnect()
{
run(0, 0);
}
void setup() {
Serial.begin(115200);
//////////////////////////////////////////////////
pinMode(DL1, OUTPUT);
pinMode(DL2, OUTPUT);
pinMode(PWML, OUTPUT);
pinMode(DR1, OUTPUT);
pinMode(DR2, OUTPUT);
pinMode(PWMR, OUTPUT);
////////////////////////////
pinMode(DL1b, OUTPUT);
pinMode(DL2b, OUTPUT);
pinMode(PWMLb, OUTPUT);
pinMode(DR1b, OUTPUT);
pinMode(DR2b, OUTPUT);
pinMode(PWMRb, OUTPUT);
Ps3.attachOnConnect(onConnect);
Ps3.attachOnDisconnect(onDisConnect);
Ps3.begin("7c:87:ce:cb:47:7e");//ใส่ MacAddress
Serial.println("Ready.");
sv1.attach(ss1); // เรียกใช้งานเตรียมตอบสนองเซอร์โว sv1
sv2.attach(ss2); // เรียกใช้งานเตรียมตอบสนองเซอร์โว sv2
////////////////////////////
}
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
// Serial.println(" ");
if (yAxisValue <= -50) //ถอยหลัง
{ moveBackward(200); }
else if (yAxisValue >= 50) //เดินหน้า
{ moveForward(200); }
else if (xAxisValue >= 50) //เลี้ยวขวา
{ turnRight(200); }
else if (xAxisValue <= -50) //เลี้ยวซ้าย
{ turnLeft(200); }
else if ( Ps3.data.button.left ) //เลี้ยวซ้าย
{ turnLeft(250);Serial.println("Left"); }
else if ( Ps3.data.button.right )//เลี้ยวขวา
{ turnRight(250); Serial.println("right"); }
else if ( Ps3.data.button.up ) //เดินหน้า
{ moveForward(250); Serial.println("moveForward"); }
else if ( Ps3.data.button.down ) //ถอยหลัง
{ moveBackward(250); Serial.println("moveBackward"); }
else if ( Ps3.data.button.triangle )
{ moveForward(120); Serial.println("triangle"); }
else if ( Ps3.data.button.cross )
{ moveBackward(120); Serial.println("cross"); }
else if ( Ps3.data.button.square )
{ slideLeft(200); Serial.println("square"); }
else if ( Ps3.data.button.circle ) ///triangle
{ slideRight(200); Serial.println("circle"); }
else if(Ps3.data.button.l2) {
updown(100);
}
else if(Ps3.data.button.l1) {
updown(-100);}
else if(Ps3.data.button.r1) {
keep(100);}
else if(Ps3.data.button.r2) {
keep(-100); }
else
{
stop();
}
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);
}}
/////////////////////////////////////////////////////////
void runb(int splb, int sprb) // มอเตอร์คู่หน้า run(กำลังมอเตอร์ซ้าาย,กำลังมอเตอร์ขวา);
{
if (splb > 0)
{
digitalWrite(DL1b, LOW);
digitalWrite(DL2b, HIGH);
analogWrite(PWMLb, splb);
}
else if (splb < 0)
{
digitalWrite(DL1b, HIGH);
digitalWrite(DL2b, LOW);
analogWrite(PWMLb, -splb);
}
else
{
digitalWrite(DL1b, HIGH);
digitalWrite(DL2b, HIGH);
}
//////////////////////////////////////
if (sprb > 0)
{
digitalWrite(DR1b, LOW);
digitalWrite(DR2b, HIGH);
analogWrite(PWMRb, sprb);
}
else if (sprb < 0)
{
digitalWrite(DR1b, HIGH);
digitalWrite(DR2b, LOW);
analogWrite(PWMRb, -sprb);
}
else
{
digitalWrite(DR1b, HIGH);
digitalWrite(DR2b, HIGH);
}}
void updown(int splb) // มอเตอร์คู่หน้า run(กำลังมอเตอร์ซ้าาย,กำลังมอเตอร์ขวา);
{
if (splb > 0)
{
digitalWrite(DL1b, LOW);
digitalWrite(DL2b, HIGH);
analogWrite(PWMLb, splb);
}
else if (splb < 0)
{
digitalWrite(DL1b, HIGH);
digitalWrite(DL2b, LOW);
analogWrite(PWMLb, -splb);
}
else
{
digitalWrite(DL1b, HIGH);
digitalWrite(DL2b, HIGH);
}
}
void keep( int sprb) // มอเตอร์คู่หน้า run(กำลังมอเตอร์ซ้าาย,กำลังมอเตอร์ขวา);
{
//////////////////////////////////////
if (sprb > 0)
{
digitalWrite(DR1b, LOW);
digitalWrite(DR2b, HIGH);
analogWrite(PWMRb, sprb);
}
else if (sprb < 0)
{
digitalWrite(DR1b, HIGH);
digitalWrite(DR2b, LOW);
analogWrite(PWMRb, -sprb);
}
else
{
digitalWrite(DR1b, HIGH);
digitalWrite(DR2b, HIGH);
}}
// เดินหน้า
void moveForward(int speed) {
run(speed, speed); // Motor A (left) และ Motor B (right) หมุนไปข้างหน้า
runb(speed, speed);
}
// ถอยหลัง
void moveBackward(int speed) {
run(-speed, -speed); // Motor A (left) และ Motor B (right) หมุนถอยหลัง
runb(-speed, -speed);
}
// เลี้ยวซ้าย
void turnLeft(int speed) {
run(-speed, speed); // Motor A (left) หมุนถอยหลัง และ Motor B (right) หมุนไปข้างหน้า
runb(-speed, speed);
}
// เลี้ยวขวา
void turnRight(int speed) {
run(speed, -speed); // Motor A (left) หมุนไปข้างหน้า และ Motor B (right) หมุนถอยหลัง
runb(speed, -speed);
}
// สไลด์ซ้าย
void slideLeft(int speed) {
run(-speed, speed); // ล้อหน้าและล้อหลังทางซ้ายหมุนไปข้างหน้า
runb(speed, -speed); // ล้อหน้าและล้อหลังทางขวาหมุนถอยหลัง
}
// สไลด์ขวา
void slideRight(int speed) {
run(speed, -speed); // ล้อหน้าและล้อหลังทางขวาหมุนไปข้างหน้า
runb(-speed, speed); // ล้อหน้าและล้อหลังทางซ้ายหมุนถอยหลัง
}
void stop() {
// หยุดมอเตอร์ A (ซ้าย)
digitalWrite(DL1, HIGH);
digitalWrite(DL2, HIGH);
analogWrite(PWML, 0); // หยุดการหมุนของมอเตอร์ซ้าย
// หยุดมอเตอร์ B (ขวา)
digitalWrite(DR1, HIGH);
digitalWrite(DR2, HIGH);
analogWrite(PWMR, 0); // หยุดการหมุนของมอเตอร์ขวา
// หยุดมอเตอร์ A (ซ้าย) บนตัวที่ 2
digitalWrite(DL1b, HIGH);
digitalWrite(DL2b, HIGH);
analogWrite(PWMLb, 0); // หยุดการหมุนของมอเตอร์ซ้าย
// หยุดมอเตอร์ B (ขวา) บนตัวที่ 2
digitalWrite(DR1b, HIGH);
digitalWrite(DR2b, HIGH);
analogWrite(PWMRb, 0); // หยุดการหมุนของมอเตอร์ขวา
}
/////////////////////////////////////////
// _________________________
// M1 M2
/// M3 M4