#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 = 0;
int S2 = 0;
int S3 = 0;
int S4 = 0;
int S6 = 0;
#define Pdegree 129 //องศาที่ต้องการเดินข้ามพื้นสีเขียว
//////////ค่ากลางเซนเซอร์ เอาไว้เปนียบเทียบค่าแสง//////////
int CEN1 = 489;
int CEN2 = 488;
int CEN3 = 440;
int CEN4 = 440;
int CEN6 = 120; //เซนเซอร์เชคกระป๋อง
///////////////ตั้งพอร์ตเซอร์โว////////////////////////////////
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 0
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(240, 200);
}
if (S2 < CEN2) {
run(-250, 250);
}
if (S3 < CEN3) {
run(250, -250);
}
if (S1 < CEN1) {
run(-1, 250);
}
if (S4 < CEN4) {
run(250, -1);
}
if (((S2 < CEN2) and (S3 < CEN3)) or ((S2 < CEN2) and (S1 < CEN1)) or ((S3 < CEN3) and (S4 < CEN4)))
{
run(240, 200);
delay(400);
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(180, 0);
}
if ((S1 < CEN1) and (S4 > CEN4))
{
run(0, 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, 200);
}
else if (headingDegrees < (cp - 3)) {
run(200, 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 R90() ///////////// ฟังก์ชันแทกเส้นเจอคู่ดำ เดินหน้านิดนึงแล้วหยุด/////////////////////////////
{
run(200, -210); delay(400);
run(0, 0);
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, -210);
if (S3 < CEN3) {
run(0, 0); //S0
break;
}
run(200, -210); delay(140);
run(0, 0);
}
}
void L90() ///////////// ฟังก์ชันแทกเส้นเจอคู่ดำ เดินหน้านิดนึงแล้วหยุด/////////////////////////////
{
run(-240, 170); delay(400);
run(0, 0);
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(-240, 170);
if (S2 < CEN2) {
run(0, 0); //S0
break;
}
run(-240, 170); delay(130);
run(0, 0);
}
}
void L() {
run(-240, 200); /////// เลี้ยวซ้าย
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 = 0; //ตัวหนีบอ้า
const int v3_set0 = 100; //ธง
/////เซอโวตอนหนีบ/////
const int v2_set1 = 49; //ตัวหนีบหนีบกระป๋อง
/////เซอโวตอนยกขึ้นก่อนวาง/////
const int v1_set1 = 22;
/////เซอโวตอนขึ้นธง ////
const int v3_set1 = 100; //ธง ชักสูงสุด
const int v3_set2 = 0; //ธง ชักครึ่ง
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) {
go();
delay(1000);
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 go()
{
while (true) {
OLED.clearDisplay(); //ล้างหน้าจอ
OLED.setTextColor(WHITE, BLACK); //สีอักษรเป็นสีขาว ,พื้นหลังดำ จอที่ใช้ไม่สามารถเปลี่ยนสีได้
OLED.setCursor(25, 3); // เซตตำแหน่ง 0,0
OLED.setTextSize(3); // เซตขนาดอักษรมีขนาดเป็น 2
OLED.print(" go "); // แสดงข้อความ S1 :
OLED.display(); // สั่งให้ จอแสดงผล
delay(100);
break;
}
}
void ST()
{
while (true) {
OLED.clearDisplay(); //ล้างหน้าจอ
OLED.setTextColor(WHITE, BLACK); //สีอักษรเป็นสีขาว ,พื้นหลังดำ จอที่ใช้ไม่สามารถเปลี่ยนสีได้
OLED.setCursor(25, 3); // เซตตำแหน่ง 0,0
OLED.setTextSize(3); // เซตขนาดอักษรมีขนาดเป็น 2
OLED.print(" Redy "); // แสดงข้อความ S1 :
OLED.display(); // สั่งให้ จอแสดงผล
delay(100);
break;
}
}
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)) {ST();menu2(); }
if ((sw == 1) and (menu == 3)) {ST(); menu3(); }
if ((sw == 1) and (menu == 4)) {ST(); menu4(); }
if ((sw == 1) and (menu == 5)) {ST(); menu5(); }
if ((sw == 1) and (menu == 6)) {ST(); menu6(); }
if ((sw == 1) and (menu == 7)) {ST(); menu7(); }
if ((sw == 1) and (menu == 8)) {ST(); 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 ที่นี่
{
P();L90();
}
void menu5() /// code 5 ที่นี่
{
//servostart();delay(1000);servowait();servoup();delay(5000);servoflag();
servostart();
delay(1000);
servowait();
delay(1000);
P();
L90();
P();
L90();
P();
R90();
PP();
run(200, 200);
delay(200);
p(90, 8500);
run(0, 0);
delay(500);
P();
R90();
P();
R90();
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);
}
}
พัชรพล
พัชรพล พันธะไชย
void LR_GYRO(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(-240, 200);
}
else if (headingDegrees < (cp - 3)) {
run(240, -200);
}
else {
run(0, 0);
}
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);
}
}
//////// ผนวกไลบรารี่ ////////////
#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 = 3; // เปิดใช้งาน 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 15
#define PS2_CLK 16
#define PS2_CMD 17
#define PS2_DAT 12
/******************************************************************
* 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);}
}
//////////////////////////////////////