/*เซอโวใหญ่ mg995 996 ต่อช่อง D2 เซต เริ่มต้นที่ 0 องศา
* เซอโวหนีบกระป๋อง ต่อช่อง D3 เซตเริ่มต้นที่ ตอนอ้าที่ 30 องศา หนีบ 160 องศา
* เซอโวชักธง ต่อช่อง D9 ตอนเริ่มต้นที่ 30 องศา ตอนยก 120 องศา
* p(องศา,เวลา);
*
*
*
*
*/
#include <SPI.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_HMC5883_U.h>
#include <Wire.h>
#include <Servo.h>
//#include <Adafruit_MCP3008.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
Adafruit_SSD1306 OLED(-1);
Adafruit_HMC5883_Unified mag = Adafruit_HMC5883_Unified(12345);
////////////ตั้งค่าพอร์ตมอเตอร์////////////////////
#define PWMR 6
#define DR1 8 /// กำหนดสัญญาณดิจิตอลขวาที่ 1 พอร์ต 7
#define DR2 7 /// กำหนดสัญญาณดิจิตอลขวาที่ 2 พอร์ต 8
/////////////////////////////////
#define PWML 5
#define DL1 9 // กำหนดสัญญาณดิจิตอลซ้ายที่ 1 พอร์ต 9
#define DL2 4 // กำหนดสัญญาณดิจิตอลซ้ายที่ 2 พอร์ต 4
///////////////////////////////////////////////
const int button = 2;
////////ตัวแปรเก็บค่าแสงจากเซนเซอร์
int S1 = 464;
int S2 = 454;
int S3 = 450;
int S4 = 456;
int S6 = 0;
#define Pdegree 140 //องศาที่ต้องการเดินข้ามพื้นสีเขียว
//////////ค่ากลางเซนเซอร์ เอาไว้เปนียบเทียบค่าแสง//////////
int CEN1 = 489;
int CEN2 = 488;
int CEN3 = 331;
int CEN4 = 507;
int CEN6 = 600; //เซนเซอร์เชคกระป๋อง
///////////////ตั้งพอร์ตเซอร์โว////////////////////////////////
const int servo1 = 10; // เปิดใช้งาน s1 ที่พอร์ต 10 zackrobot & 7 kruroRobot
const int servo2 = 11; // เปิดใช้งาน s1 ที่พอร์ต 11 & 8
const int servo3 = 12; // เปิดใช้งาน s1 ที่พอร์ต 12
#define SpeedOffsetMotorL 0
#define SpeedOffsetMotorR 2
Servo sv1; // ประกาศการใช้งาน sv1
Servo sv2; // ประกาศการใช้งาน sv2
Servo sv3; // ประกาศการใช้งาน sv3
void P() ///////////// ฟังก์ชันแทกเส้นเจอคู่ดำ เดินหน้านิดนึงแล้วหยุด/////////////////////////////
{ while (1) {
S1 = analogRead(0); // อ่านค่า Analog จาก MCP3008 ขาที่ 0
S2 = analogRead(1); // อ่านค่า Analog จาก MCP3008 ขาที่ 1
S3 = analogRead(2); // อ่านค่า Analog จาก MCP3008 ขาที่ 2
S4 = analogRead(3); // อ่านค่า Analog จาก MCP3008 ขาที่ 3
if ((S2 > CEN2) and (S3 > CEN3)) {
run(250, 250);
}
if (S2 < CEN2) {
run(-250, 250);
}
if (S3 < CEN3) {
run(250, -250);
}
if (((S2 < CEN2) and (S3 < CEN3)) or ((S2 < CEN2) and (S1 < CEN1)) or ((S3 < CEN3) and (S4 < CEN4)))
{
run(250, 250);
delay(550);
run(0, 0);
delay(100);
break;
}
}
}
void PP() ///////////// ฟังก์ชันแทกเส้นเจอคู่ดำหยุดเลยทันที/////////////////////////////ใช้เช๋คก่อนถึงสีเขียว
{ while (1) {
S1 = analogRead(0); // อ่านค่า Analog จาก MCP3008 ขาที่ 0
S2 = analogRead(1); // อ่านค่า Analog จาก MCP3008 ขาที่ 1
S3 = analogRead(2); // อ่านค่า Analog จาก MCP3008 ขาที่ 2
S4 = analogRead(3); // อ่านค่า Analog จาก MCP3008 ขาที่ 3
if ((S2 > CEN2) and (S3 > CEN3)) {
run(180, 190);
}
if (S2 < CEN2) {
run(-180, 180);
}
if (S3 < CEN3) {
run(180, -180);
}
if ((S1 < CEN1) and (S4 < CEN4))
{
run(0, 0);
delay(200);
break;
}
if (S1 < CEN1) {
run(-250, 180);
}
if (S4 < CEN4) {
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);}
OLED.clearDisplay();
OLED.setTextColor(WHITE,BLACK); //สีอักษรเป็นสีขาว ,พื้นหลังดำ
OLED.setCursor(0, 0); // เซตตำแหน่ง 0,0
OLED.setTextSize(2); // เซตขนาดอักษรมีขนาดเป็น 2
OLED.println("Degrees = ");
OLED.println(headingDegrees);
OLED.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);
OLED.clearDisplay();
OLED.setTextColor(WHITE,BLACK); //สีอักษรเป็นสีขาว ,พื้นหลังดำ
OLED.setCursor(0, 0); // เซตตำแหน่ง 0,0
OLED.setTextSize(2); // เซตขนาดอักษรมีขนาดเป็น 2
OLED.println("degree = ");
OLED.println(headingDegrees);
OLED.display();
delay(20);
}}
void PW() ///////////// ฟังก์ชันแทกเส้นเจอคู่ดำหยุด ใช้กรณี อ่านสีฟ้า แล้ว เจอดำ/////////////////////////////
{ while (1) {
S1 = analogRead(0); // อ่านค่า Analog จาก MCP3008 ขาที่ 0
S2 = analogRead(1); // อ่านค่า Analog จาก MCP3008 ขาที่ 1
S3 = analogRead(2); // อ่านค่า Analog จาก MCP3008 ขาที่ 2
S4 = analogRead(3); // อ่านค่า Analog จาก MCP3008 ขาที่ 3
if ((S2 < CEN2) and (S3 < CEN3)) {
run(150, 150);
}
if (S2 > CEN2) {
run(-200, 200);
}
if (S3 > CEN3) {
run(200, -200);
}
if (((S2 > CEN2) and (S3 > CEN3)) or ((S2 > CEN2) and (S1 > CEN1)))
{
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) {
S1 = analogRead(0); // อ่านค่า Analog จาก MCP3008 ขาที่ 0
S2 = analogRead(1); // อ่านค่า Analog จาก MCP3008 ขาที่ 1
S3 = analogRead(2); // อ่านค่า Analog จาก MCP3008 ขาที่ 2
S4 = analogRead(3); // อ่านค่า Analog จาก MCP3008 ขาที่ 3
run(-200, 200); if (S1 < CEN1) {
run(0, 0); //S0
break;
}
}
}
void L1() {
while (1) {
S1 = analogRead(0); // อ่านค่า Analog จาก MCP3008 ขาที่ 0
S2 = analogRead(1); // อ่านค่า Analog จาก MCP3008 ขาที่ 1
S3 = analogRead(2); // อ่านค่า Analog จาก MCP3008 ขาที่ 2
S4 = analogRead(3); // อ่านค่า Analog จาก MCP3008 ขาที่ 3
run(-200, 200); if (S2 < CEN2) {
run(0, 0); //S1
break;
}
}
}
void L2() {
while (1) {
S1 = analogRead(0); // อ่านค่า Analog จาก MCP3008 ขาที่ 0
S2 = analogRead(1); // อ่านค่า Analog จาก MCP3008 ขาที่ 1
S3 = analogRead(2); // อ่านค่า Analog จาก MCP3008 ขาที่ 2
S4 = analogRead(3); // อ่านค่า Analog จาก MCP3008 ขาที่ 3
run(-200, 200); if (S3 < CEN3) {
run(0, 0); //S2
break;
}
}
}
void L3() {
while (1) {
S1 = analogRead(0); // อ่านค่า Analog จาก MCP3008 ขาที่ 0
S2 = analogRead(1); // อ่านค่า Analog จาก MCP3008 ขาที่ 1
S3 = analogRead(2); // อ่านค่า Analog จาก MCP3008 ขาที่ 2
S4 = analogRead(3); // อ่านค่า Analog จาก MCP3008 ขาที่ 3
run(-200, 200); if (S4 < CEN4) {
run(0, 0); //S3
break;
}
}
}
//////////////////////เลี้ยวขวา//////////////////////////////////////////
void R0() {
while (1) {
S1 = analogRead(0); // อ่านค่า Analog จาก MCP3008 ขาที่ 0
S2 = analogRead(1); // อ่านค่า Analog จาก MCP3008 ขาที่ 1
S3 = analogRead(2); // อ่านค่า Analog จาก MCP3008 ขาที่ 2
S4 = analogRead(3); // อ่านค่า Analog จาก MCP3008 ขาที่ 3
run(200, -200); if (S1 < CEN1) {
run(0, 0); //S0
break;
}
}
}
void R1() {
while (1) {
S1 = analogRead(0); // อ่านค่า Analog จาก MCP3008 ขาที่ 0
S2 = analogRead(1); // อ่านค่า Analog จาก MCP3008 ขาที่ 1
S3 = analogRead(2); // อ่านค่า Analog จาก MCP3008 ขาที่ 2
S4 = analogRead(3); // อ่านค่า Analog จาก MCP3008 ขาที่ 3
run(200, -200); if (S3 < CEN3) {
run(0, 0); //S1
break;
}
}
}
void R2() {
while (1) {
S1 = analogRead(0); // อ่านค่า Analog จาก MCP3008 ขาที่ 0
S2 = analogRead(1); // อ่านค่า Analog จาก MCP3008 ขาที่ 1
S3 = analogRead(2); // อ่านค่า Analog จาก MCP3008 ขาที่ 2
S4 = analogRead(3); // อ่านค่า Analog จาก MCP3008 ขาที่ 3
run(200, -200); if (S3 < CEN3) {
run(0, 0); //S2
break;
}
}
}
void R3() {
while (1) {
S1 = analogRead(0); // อ่านค่า Analog จาก MCP3008 ขาที่ 0
S2 = analogRead(1); // อ่านค่า Analog จาก MCP3008 ขาที่ 1
S3 = analogRead(2); // อ่านค่า Analog จาก MCP3008 ขาที่ 2
S4 = analogRead(3); // อ่านค่า Analog จาก MCP3008 ขาที่ 3
run(200, -200); if (S4 < CEN4) {
run(0, 0); //S3
break;
}
}
}
///////////////////////องศา เซอร์โว เริ่มต้น///////////////////////
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; //ธง ชักครึ่ง
void setup()
{ OLED.begin(SSD1306_SWITCHCAPVCC, 0x3C); // กำหนดแอดเดรสของพอร์ตจอเป็น 0x3C (for the 128x64)
//qmc.init();
mag.begin();
pinMode(PWML, OUTPUT);
pinMode(DL1, OUTPUT);
pinMode(DL2, OUTPUT);
pinMode(PWMR, OUTPUT);
pinMode(DR1, OUTPUT);
pinMode(DR2, OUTPUT);
pinMode(button, INPUT);
// pinMode(17, OUTPUT);
///////////// ตั้งพอร์ตเซอร์โว////////////
sv1.attach(servo1); // เรียกใช้งานเตรียมตอบสนองเซอร์โว sv1
sv2.attach(servo2); // เรียกใช้งานเตรียมตอบสนองเซอร์โว sv2
sv3.attach(servo3); // เรียกใช้งานเตรียมตอบสนองเซอร์โว sv3
// adc.begin(13, 11, 12, 10);//เริ่มจากใช้ adc จาก library
servostart();
}
void set_servo() {
while (1) {
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.println(nob); // นำค่า nob มาแสดงใน oled
OLED.display(); // เปิดฟังก์ชันแสดงผล
sv1.write(nob); // สั่งเซอร์โวมอเตอร์ให้หมุนไปตามค่าองศาที่ทำการ nob ไว้
sv2.write(nob); // สั่งเซอร์โวมอเตอร์ให้หมุนไปตามค่าองศาที่ทำการ nob ไว้
sv3.write(nob); // สั่งเซอร์โวมอเตอร์ให้หมุนไปตามค่าองศาที่ทำการ nob ไว้
delay(50); // หน่วงเวลา 0.05 วินาที
}
}
void analogADC()
{
int S1 = analogRead(0);
int S2 = analogRead(1);
int S3 = analogRead(2);
int S4 = analogRead(3);
int S5 = analogRead(4);
int S6 = analogRead(5);
int S7 = analogRead(6);
int S8 = analogRead(7);
}
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){
S1 = analogRead(0); // อ่านค่า Analog จาก MCP3008 ขาที่ 0
S2 = analogRead(1); // อ่านค่า Analog จาก MCP3008 ขาที่ 1
S3 = analogRead(2); // อ่านค่า Analog จาก MCP3008 ขาที่ 2
S4 = analogRead(3); // อ่านค่า Analog จาก MCP3008 ขาที่ 3
S6 = analogRead(6); // อ่านค่า Analog จาก MCP3008 ขาที่ 2
if (S6 > CEN6){
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 sensor()
{
while (true) {
S1 = analogRead(0); // อ่านค่า Analog จาก MCP3008 ขาที่ 0
S2 = analogRead(1); // อ่านค่า Analog จาก MCP3008 ขาที่ 1
S3 = analogRead(2); // อ่านค่า Analog จาก MCP3008 ขาที่ 2
S4 = analogRead(3); // อ่านค่า Analog จาก MCP3008 ขาที่ 3
S6 = analogRead(6); // อ่านค่า Analog จาก MCP3008 ขาที่ 2
OLED.clearDisplay(); //ล้างหน้าจอ
OLED.setTextColor(WHITE, BLACK); //สีอักษรเป็นสีขาว ,พื้นหลังดำ จอที่ใช้ไม่สามารถเปลี่ยนสีได้
OLED.setCursor(0, 0); // เซตตำแหน่ง 0,0
OLED.setTextSize(1); // เซตขนาดอักษรมีขนาดเป็น 2
OLED.print(" S1 : "); // แสดงข้อความ S1 :
OLED.print(S1); // แสดงค่า S1
OLED.print(" S2 : "); // แสดงข้อความ S2 :
OLED.println(S2); // แสดงค่า S2
OLED.print(" S3 : "); // แสดงข้อความ S3 :
OLED.print(S3); // แสดงค่า S3
OLED.print(" S4 : "); // แสดงข้อความ S4 :
OLED.println(S4); // แสดงค่า S4
OLED.print(" S6 : "); // แสดงข้อความ S2 :
OLED.println(S6); // แสดงค่า S2
OLED.display(); // สั่งให้ จอแสดงผล
delay(100);
}
}
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(3); // เซตขนาดอักษรมีขนาดเป็น 2
OLED.print(" "); // วรรค
OLED.println(menu); // แสดงค่า Menu ที่ได้จากการ map nob ให้เหลือ 0-12
OLED.setTextSize(1); // เซตขนาดอักษรมีขนาดเป็น 2
OLED.println(" ");
OLED.println("2NONG ROBOT");
OLED.println(" ");
OLED.print(" "); // วรรค
OLED.print(nob); // แสดงค่าที่อ่านได้จาก nob หรือ Analog7
OLED.println(" MeNu");
OLED.display();
if ((sw == 1) and (menu == 0))
{
sensor();
}
if ((sw == 1) and (menu == 1)){set_servo();}
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();}
delay(50);
}
/* ***MENU*** */
void menu2() /// code 2 ที่นี่
{
cpp();
}
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();
}
///////////////////////////////////////////////////////////////////////
// ฟังก์ชั่นสั่งงานมอเตอร์ run(ความเร็วซ้าย,ความเร็วขวา);
//
// run(100,100); // เดินหน้าทั้งสองล้อ
// run(-100,-100); // ถอยหลังทั้งสองล้อ
// run(-100,100); // ล้อซ้ายถอย ล้อขวาเดินหน้า
// run(100,-100); // ล้อซ้ายเดินหน้า ล้อขวาถอย
// run(0,0); // หยุดทั้ง สองล้อ
//
// สามารถกำหนดความเร็วได้ -255 ถึง 255
//
///////////////////////////////////////////////////////////////////////
/////////////////////////////ฟังก์ชันมอเตอร์///////////////////////////////////
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 <Wire.h>
#include <PS2X_lib.h> //for v1.6
#include <SPI.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
Adafruit_SSD1306 display(-1);
#include <Servo.h>
int ss1 = 15; // เปิดใช้งาน s2 ที่พอร์ต 3
int ss2 = 14; // เปิดใช้งาน s3 ที่พอร์ต A0
Servo sv1; // ประกาศการใช้งาน sv1
Servo sv2; // ประกาศการใช้งาน sv2
///////////ตั้งค่าปุ่มกด///////////////////
int button = 2; /// กำหนดปุ่มกดสวิตซ์ขา 2
//////////////////////////////////////////////
//////////กำหนดตำแหน่งเซอโว/////////////
#define pos1min 0 //ค่าตำแหน่งน้อยสุดที่ยอมให้เซอโว 1 ทำงาน (ตัวยก)
#define pos1max 130 //ค่าตำแหน่งมากสุดที่ยอมให้เซอโว 1 ทำงาน (ตัวยก)
#define pos2min 0 //ค่าตำแหน่งน้อยสุดที่ยอมให้เซอโว 2 ทำงาน (ตัวหนีบ)
#define pos2max 100 //ค่าตำแหน่งมากสุดที่ยอมให้เซอโว 2 ทำงาน (ตัวหนีบ)
int pos1 = 0;
int pos2 = 0;
///////จูนค่ามอเตอร์/////////////////
#define SpeedOffsetMotorL 0
#define SpeedOffsetMotorR 8
////////////ตั้งค่าพอร์ตมอเตอร์////////////////////
#define PWMR 6
#define DR1 8 /// กำหนดสัญญาณดิจิตอลขวาที่ 1 พอร์ต 7
#define DR2 7 /// กำหนดสัญญาณดิจิตอลขวาที่ 2 พอร์ต 8
/////////////////////////////////
#define PWML 5
#define DL1 9 // กำหนดสัญญาณดิจิตอลซ้ายที่ 1 พอร์ต 9
#define DL2 4 // กำหนดสัญญาณดิจิตอลซ้ายที่ 2 พอร์ต 4
/******************************************************************
* set pins connected to PS2 controller:
* - 1e column: original
* - 2e colmun: Stef?
* replace pin numbers by the ones you use
******************************************************************/
#define PS2_SEL 10
#define PS2_CLK 13 ////sck
#define PS2_CMD 12
#define PS2_DAT 11
/******************************************************************
* select modes of PS2 controller:
* - pressures = analog reading of push-butttons
* - rumble = motor rumbling
* uncomment 1 of the lines for each mode selection
******************************************************************/
//#define pressures true
#define pressures false
//#define rumble true
#define rumble false
PS2X ps2x; // create PS2 Controller Class
//right now, the library does NOT support hot pluggable controllers, meaning
//you must always either restart your Arduino after you connect the controller,
//or call config_gamepad(pins) again after connecting the controller.
int error = 0;
byte type = 0;
byte vibrate = 0;
void setup(){
Serial.begin(57600);
delay(300); //added delay to give wireless ps2 module some time to startup, before configuring it
//CHANGES for v1.6 HERE!!! **************PAY ATTENTION*************
//setup pins and settings: GamePad(clock, command, attention, data, Pressures?, Rumble?) check for error
error = ps2x.config_gamepad(PS2_CLK, PS2_CMD, PS2_SEL, PS2_DAT, pressures, rumble);
if(error == 0){
Serial.print("Found Controller, configured successful ");
Serial.print("pressures = ");
if (pressures)
Serial.println("true ");
else
Serial.println("false");
Serial.print("rumble = ");
if (rumble)
Serial.println("true)");
else
Serial.println("false");
Serial.println("Try out all the buttons, X will vibrate the controller, faster as you press harder;");
Serial.println("holding L1 or R1 will print out the analog stick values.");
Serial.println("Note: Go to www.billporter.info for updates and to report bugs.");
}
else if(error == 1)
Serial.println("No controller found, check wiring, see readme.txt to enable debug. visit www.billporter.info for troubleshooting tips");
else if(error == 2)
Serial.println("Controller found but not accepting commands. see readme.txt to enable debug. Visit www.billporter.info for troubleshooting tips");
else if(error == 3)
Serial.println("Controller refusing to enter Pressures mode, may not support it. ");
// Serial.print(ps2x.Analog(1), HEX);
type = ps2x.readType();
switch(type) {
case 0:
Serial.print("Unknown Controller type found ");
break;
case 1:
Serial.print("DualShock Controller found ");
break;
case 2:
Serial.print("GuitarHero Controller found ");
break;
case 3:
Serial.print("Wireless Sony DualShock Controller found ");
break;
}
sv1.attach(ss1); // เรียกใช้งานเตรียมตอบสนองเซอร์โว sv1
sv2.attach(ss2); // เรียกใช้งานเตรียมตอบสนองเซอร์โว sv2
sv1.write(pos1);
sv2.write(pos2);
}
void loop() {
/* You must Read Gamepad to get new values and set vibration values
ps2x.read_gamepad(small motor on/off, larger motor strenght from 0-255)
if you don't enable the rumble, use ps2x.read_gamepad(); with no values
You should call this at least once a second
*/
if(error == 1) //skip loop if no controller found
return;
if(type == 2){ //Guitar Hero Controller
ps2x.read_gamepad(); //read controller
if(ps2x.ButtonPressed(GREEN_FRET))
Serial.println("Green Fret Pressed");
if(ps2x.ButtonPressed(RED_FRET))
Serial.println("Red Fret Pressed");
if(ps2x.ButtonPressed(YELLOW_FRET))
Serial.println("Yellow Fret Pressed");
if(ps2x.ButtonPressed(BLUE_FRET))
Serial.println("Blue Fret Pressed");
if(ps2x.ButtonPressed(ORANGE_FRET))
Serial.println("Orange Fret Pressed");
if(ps2x.ButtonPressed(STAR_POWER))
Serial.println("Star Power Command");
if(ps2x.Button(UP_STRUM)) //will be TRUE as long as button is pressed
Serial.println("Up Strum");
if(ps2x.Button(DOWN_STRUM))
Serial.println("DOWN Strum");
if(ps2x.Button(PSB_START)) //will be TRUE as long as button is pressed
Serial.println("Start is being held");
if(ps2x.Button(PSB_SELECT))
Serial.println("Select is being held");
if(ps2x.Button(ORANGE_FRET)) { // print stick value IF TRUE
Serial.print("Wammy Bar Position:");
Serial.println(ps2x.Analog(WHAMMY_BAR), DEC);
}
}
else { //DualShock Controller
ps2x.read_gamepad(false, vibrate); //read controller and set large motor to spin at 'vibrate' speed
if(ps2x.Button(PSB_START)) //will be TRUE as long as button is pressed
Serial.println("Start is being held");
if(ps2x.Button(PSB_SELECT))
Serial.println("Select is being held");
if(ps2x.Button(PSB_PAD_UP)) { //will be TRUE as long as button is pressed
Serial.print("Up held this hard: ");
Serial.println(ps2x.Analog(PSAB_PAD_UP), DEC);
Motor(120,120);
}
else if(ps2x.Button(PSB_PAD_RIGHT)){
Serial.print("Right held this hard: ");
Serial.println(ps2x.Analog(PSAB_PAD_RIGHT), DEC);
Motor(100,-100);
}
else if(ps2x.Button(PSB_PAD_LEFT)){
Serial.print("LEFT held this hard: ");
Serial.println(ps2x.Analog(PSAB_PAD_LEFT), DEC);
Motor(-100,100);
}
else if(ps2x.Button(PSB_PAD_DOWN)){
Serial.print("DOWN held this hard: ");
Serial.println(ps2x.Analog(PSAB_PAD_DOWN), DEC);
Motor(-120,-120);
}
else if(ps2x.Button(PSB_TRIANGLE)){
// if (pos3>pos3max) {sv3.write(pos3max);}
// else { pos3=pos3+4 ;
// sv3.write(pos3); }
Motor(200,200);
}
else if(ps2x.Button(PSB_CIRCLE)) { //will be TRUE if button was JUST pressed
Serial.println("Circle just pressed");
Motor(140,-140);}
else if(ps2x.Button(PSB_CROSS)) { //will be TRUE if button was JUST pressed OR released
// if (pos3<pos3min) {sv3.write(pos3min);}
// else { pos3=pos3-4 ;
// sv3.write(pos3); }
Motor(-200,-200);
}
else if(ps2x.Button(PSB_SQUARE)) { //will be TRUE if button was JUST released
Serial.println("Square just released");
Motor(-140,140); }
else if(ps2x.Button(PSB_L2)) {
if (pos1>pos1max) {sv1.write(pos1max);}
else { pos1=pos1+5 ;
sv1.write(pos1); }
}
else if(ps2x.Button(PSB_L1)) {
if (pos1<pos1min) {sv1.write(pos1min);}
else { pos1=pos1-5 ;
sv1.write(pos1); }}
else if(ps2x.Button(PSB_R2)) {
if (pos2>pos2max) {sv2.write(pos2max);}
else { pos2=pos2+4 ;
sv2.write(pos2); }
}
else if(ps2x.Button(PSB_R1)) {
if (pos2<pos2min) {sv2.write(pos2min);}
else { pos2=pos2-4 ;
sv2.write(pos2) ; }
}
else {Motor(0,0);}
}
delay(50);
}
/////////////////////////////ฟังก์ชันมอเตอร์///////////////////////////////////
void Motor(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);}
}