โค้ดล่าสุดครับ
/* Arduino Robotic Arm with Grab
* More info: http://www.ardumotive.com/
* arduino pro mini 328 broad
* Dev: Michalis Vasilakis Data: 12/12/2016 Ver: 1.1 */
#include <EEPROM.h>
#include <Servo.h>
//Create servo objects to control servo motors
Servo up_down; //servo3
Servo forward_backward;//servo6
Servo rotate;//servo2
Servo grab;//servo1
//เพิ่มเซอร์โว4,5
Servo up_down4;//servo4
Servo up_down5;//servo5
//Constants
const int startBT = 7;
const int teachBT = 8;
//2562-09-06 const int buzzer = 9;
const int buzzer = 10;
const int potForwardBackward = A3;//servo6
const int potUpDown = A2;//servo3
const int potRotate = A1;//server2
const int potGrab = A0;//เซอร์โว1
//เพิ่มเซอร์โว2ตัว
const int potUpDown4 = A4;//เซอร์โว4
const int potUpDown5 = A5;//เซอร์โว5
//Max and Min values for servos ! Change them to meet your setup !
const int minGrab=180;
const int maxGrab=126;
const int minRotate=0;
const int maxRotate=100;
const int minUpDown=5;
const int maxUpDown=100;
const int minForwardBackward=160;
//const int maxForwardBackward=80;
const int maxForwardBackward=20;//25620911ทำให้องศาการหมุนเซอร์โวเพิ่มจาก90เป็น180องศาโดยประมาณ
//เพิ่มเซอร์โว4,5
const int minUpDown4=5;
const int maxUpDown4=100;
const int minUpDown5=5;
const int maxUpDown5=100;
//-------------------------------------//
//Variables ประกาศตัวแปร
int readUpDown,readForwardBackward,readRotate,readGrab,readTeach,readStart,readUpDown4,readUpDown5;
int teachUpDown[100],teachForwardBackward[100],teachRotate[100],teachGrab[100],teachUpDown4[100],teachUpDown5[100];
boolean started =false;
int index = 1;
int eeppos = 0;
//int stepSpeed = 50; //Change this to fo faster!
int stepSpeed = 50; //25620910 20 to 50 ช้าลง
int stepSpeed2 = 50; //25620920
int stepSpeed3 = 50; //25620920
void setup() {
//Attach Servo motors
forward_backward.attach(3);
up_down.attach(4);
rotate.attach(5); //portokali xontro
grab.attach(6);
//เพิ่มเซอร์โว4,5
up_down4.attach(2);//servo4
up_down5.attach(9);//servo5
//Inputs-Outputs
pinMode(teachBT, INPUT_PULLUP);//pin8
pinMode(startBT, INPUT_PULLUP);//pin7
pinMode(buzzer, OUTPUT);//pin 10
//Do a smooth movement on startup, from home potion to pot position:
readInputs();
// readEeprom();
goHome();
goPot();
Serial.begin(9600);
}
void loop() {
//เมื่อเปิดสวิทช์
if (!started){
readInputs();
moveServos();
//เมื่อกดปุ่มสอนการเคลื่อนที่
if (readTeach==LOW){
savePosition();//บันทึกข้อมูลใน index[],EEPROM
tone(buzzer,500);
delay(500);
noTone(buzzer);
}
//เมื่อกดปุ่มสตาร์ท เลือกข้อมูลจาก EEPROM
if (readStart==LOW){
tone(buzzer,700);
started=true;
delay(1000);
noTone(buzzer);
// readEeprom();
// runTeach();
}
}
else
{
goHome();
readEeprom();//อ่านข้อมูลมาใช้ เมื่อกดสตาร์ทครั้งแรก
runTeach();
}
}
void readInputs(){
//1.เก็บข้อมูลในตัวแปร
//ลำดับการทำงานของเซอร์โว
//servo3 UpDown
//servo6 ForwardBackward
//servo2 Rotate
//servo1 Grab
//servo4 UpDown4
//servo5 UpDown5
//Read potentiometers of servo1-servo6 อ่านค่าปัจจุบันของตัวต้านทานปรับค่าได้
//servo3
readUpDown = analogRead(potUpDown);
//delay(50);ใส่ดีเลย์ทำให้กระตุก ทำงานช้า
readUpDown = map(readUpDown,0,1023,minUpDown,maxUpDown);
Serial.print("readInput_readUpDown=");
Serial.println(readUpDown);
//teachUpDown[index] = readUpDown;
// Serial.print(index);
// Serial.println(teachUpDown[index]);
//servo6
readForwardBackward = analogRead(potForwardBackward);
readForwardBackward = map(readForwardBackward,0,1023,minForwardBackward,maxForwardBackward);
Serial.print("readInput_ForwardBackward = ");
Serial.println(readForwardBackward);
//servo2
readRotate = analogRead(potRotate);
readRotate = map(readRotate,0,1023,minRotate,maxRotate);
Serial.print("readInput_readRotate = ");
Serial.println(readRotate);
//servo1
readGrab = analogRead(potGrab);
readGrab = map(readGrab,0,1023,minGrab,maxGrab);
Serial.print("readInput_readGrab = ");
Serial.println(readGrab);
//เพิ่มเซอร์โว4,5
//servo4
readUpDown4 = analogRead(potUpDown4);
readUpDown4 = map(readUpDown4,0,1023,minUpDown4,maxUpDown4);
Serial.print("readInput_readUpDown4 = ");
Serial.println(readUpDown4);
//servo5
readUpDown5 = analogRead(potUpDown5);
readUpDown5 = map(readUpDown5,0,1023,minUpDown5,maxUpDown5);
Serial.print("readInput_readUpDown5 = ");
Serial.println(readUpDown5);
//Read buttons
readTeach = digitalRead(teachBT);
readStart = digitalRead(startBT);
}
void moveServos(){
//ลำดับการทำงานของเซอร์โว
//servo3 UpDown
//servo6 ForwardBackward
//servo2 Rotate
//servo1 Grab
//servo4 UpDown4
//servo5 UpDown5
up_down.write(readUpDown);
Serial.print("moveServos_up_down = ");
Serial.println(readUpDown);
// Serial.println(up_down);
forward_backward.write(readForwardBackward);
Serial.print("moveServos_forward_backward = ");
Serial.println(readForwardBackward);
// Serial.println(forward_backward);
rotate.write(readRotate);
grab.write(readGrab);
//เพิ่มเซอร์โว4,5
up_down4.write(readUpDown4);
up_down5.write(readUpDown5);
}
//บันทึกข้อมูลเมื่อมีการให้เคลื่อนที่ทีละตำแหน่ง เข้า INDEX[],EEPROM()
void savePosition(){
//ลำดับการทำงานของเซอร์โว
//servo3 UpDown
//servo6 ForwardBackward
//servo2 Rotate
//servo1 Grab
//servo4 UpDown4
//servo5 UpDown5
//2.บันทึกข้อมูลใน index[1],EEPORM เริ่มตำแหน่งที่1
eeppos = eeppos+1;
teachUpDown[index] = readUpDown;
EEPROM.write(eeppos,readUpDown);
Serial.print("savePosition_index=");
Serial.println(index);
Serial.print("EEPROM = ");
Serial.println(eeppos);
Serial.println(readUpDown);
eeppos = eeppos+1;
teachForwardBackward[index] = readForwardBackward;
EEPROM.write(eeppos,readForwardBackward);
eeppos = eeppos+1;
teachRotate[index] = readRotate;
EEPROM.write(eeppos,readRotate);
eeppos = eeppos+1;
teachGrab[index] = readGrab;
EEPROM.write(eeppos,readGrab);
//เพิ่มเซอร์โว4,5
eeppos = eeppos+1;
teachUpDown4[index] = readUpDown4;
EEPROM.write(eeppos,readUpDown4);
eeppos = eeppos+1;
teachUpDown5[index] = readUpDown5;
EEPROM.write(eeppos,readUpDown5);
index++;
//eeppos++;
}
/*
3.อ่านข้อมูลเก็บในตัวแปร
data1 = EEPROM.read(0);
data2 = EEPROM.read(1);
readUpDown = EEPROM.read(2);
readForwardBackward = EEPROM.read(3);
//4.นำข้อมูลมาใช้งาน แสดงข้อมูล Serial Moniter
Serial.println(data1);.
Serial.println(data2);
Serial.println(readUpDown);
Serial.println(readForwardBackward);
*/
//4.นำข้อมูลมาใช้งาน อ่านค่าจาก eeppos ไปเก็บใน index ที่ 1 ถึง indexที่ eeppos/6
// ดึงข้อมูลจาก EEPROM มาใช้
void readEeprom(){
//ลำดับการทำงานของเซอร์โว
//25620919
//servo6 ForwardBackward
//servo1 Grab
//servo4 UpDown4
//servo5 UpDown5
//servo3 UpDown
//servo2 Rotate
readForwardBackward = EEPROM.read(eeppos);
readGrab = EEPROM.read(eeppos);
readUpDown4 = EEPROM.read(eeppos);
readUpDown5 = EEPROM.read(eeppos);
readUpDown = EEPROM.read(eeppos);
readRotate = EEPROM.read(eeppos);
Serial.print("EEPROM_readUpDown = ");
Serial.println(readUpDown);
Serial.println(readForwardBackward);
Serial.println(readRotate);
Serial.println(readGrab);
Serial.print("EEPROM_readUpDown4 = ");
Serial.println(readUpDown4);
Serial.print("EEPROM_readUpDown4 = ");
Serial.println(readUpDown4);
eeppos++;
}
// ดึงข้อมูลจาก index[] มาใช้
void runTeach()
{
//ลำดับการทำงานของเซอร์โวเปลี่ยนลำดับการทำงานเซอรืโว25620918
//25620919
//servo6 ForwardBackward
//servo1 Grab
//servo4 UpDown4
//servo5 UpDown5
//servo3 UpDown
//servo2 Rotate
for (int i=0; i<index-1; i++)
{
//servo6 ForwardBackward
if (teachForwardBackward[i] < teachForwardBackward[i+1]){
for (int j = teachForwardBackward[i]; j<= teachForwardBackward[i+1]; j++){
forward_backward.write(j);
delay(stepSpeed);
}
}
else if (teachForwardBackward[i] > teachForwardBackward[i+1]){
for (int j = teachForwardBackward[i]; j>= teachForwardBackward[i+1]; j--){
forward_backward.write(j);
delay(stepSpeed);
}
}
else{
forward_backward.write(teachForwardBackward[i]);
}
//servo1 Grab
if (teachGrab[i] < teachGrab[i+1]){
for (int j = teachGrab[i]; j<= teachGrab[i+1]; j++){
delay(stepSpeed2);
grab.write(j);
delay(100);
}
}
else if (teachGrab[i] > teachGrab[i+1]){
for (int j = teachGrab[i]; j>= teachGrab[i+1]; j--){
delay(stepSpeed2);
grab.write(j);
//delay(stepSpeed2);
delay(stepSpeed2);
}
}
else{
delay(stepSpeed2);
grab.write(teachGrab[i]);
delay(stepSpeed2);
}
//servo4 UpDown4
if (teachUpDown4[i] < teachUpDown4[i+1]){
for (int j = teachUpDown4[i]; j<= teachUpDown4[i+1]; j++){
up_down4.write(j);
delay(stepSpeed3);
}
}
else if (teachUpDown4[i] > teachUpDown4[i+1]){
for (int j = teachUpDown4[i]; j>= teachUpDown4[i+1]; j--){
up_down4.write(j);
delay(stepSpeed);
}
}
else{
up_down4.write(teachUpDown4[i]);
delay(stepSpeed);
}
//servo5 UpDown5
if (teachUpDown5[i] < teachUpDown5[i+1]){
for (int j = teachUpDown5[i]; j<= teachUpDown5[i+1]; j++){
up_down5.write(j);
delay(stepSpeed3);
}
}
else if (teachUpDown5[i] > teachUpDown5[i+1]){
for (int j = teachUpDown5[i]; j>= teachUpDown5[i+1]; j--){
up_down5.write(j);
delay(stepSpeed3);
}
}
else{
up_down5.write(teachUpDown5[i]);
delay(stepSpeed3);
}
//servo3 UpDown
if (teachUpDown[i] < teachUpDown[i+1]){
for (int j = teachUpDown[i]; j<= teachUpDown[i+1]; j++){
up_down.write(j);
delay(stepSpeed3);
Serial.print("runteach_teachUpDown = ");
Serial.println(j);
}
}
else if (teachUpDown[i] > teachUpDown[i+1]){
for (int j = teachUpDown[i]; j>= teachUpDown[i+1]; j--){
up_down.write(j);
delay(stepSpeed3);
}
}
else{
up_down.write(teachUpDown[i]);
delay(stepSpeed3);
}
//servo2 Rotate
if (teachRotate[i] < teachRotate[i+1])
{
for (int j = teachRotate[i]; j<= teachRotate[i+1]; j++)
{
rotate.write(j);
delay(stepSpeed);
Serial.print("runteach_teachRotate=");
Serial.println(j);
}
}
else if (teachRotate[i] > teachRotate[i+1]){
for (int j = teachRotate[i]; j>= teachRotate[i+1]; j--){
rotate.write(j);
delay(stepSpeed);
}
}
else{
rotate.write(teachRotate[i]);
delay(stepSpeed);
}
}
started=false;
}
//Change values if it's necessary...
void goHome()
//ลำดับการทำงานของเซอร์โว
//servo6 ForwardBackward 160-80
//servo3 UpDown 5-100
//servo2 Rotate 0-100
//servo1 Grab 180-126
//servo4 UpDown4 5-100
//servo5 UpDown5 5-100
//25620923
//ลำดับการทำงานของเซอร์โว
//servo6 ForwardBackward 160-80
//servo3 UpDown 5-100
//servo2 Rotate 0-100
//servo1 Grab 180-126
//servo5 UpDown5 5-100
//servo4 UpDown4 5-100
{
//servo6 ForwardBackward 160-80
if (readForwardBackward < 80){
for (int j = readForwardBackward; j<=80; j++){
delay(stepSpeed);
forward_backward.write(j);
delay(stepSpeed);
Serial.print("goHome_forward_backward=");
Serial.println(j);
}
}
else if (readForwardBackward > 80){
for (int j = readForwardBackward; j>=80; j--){
delay(stepSpeed);
forward_backward.write(j);
delay(stepSpeed);
}
}
else{
delay(stepSpeed);
forward_backward.write(80);
}
//servo3 UpDown 5-100
if (readUpDown < 32){
for (int j = readUpDown; j<=32; j++){
//up_down.write(j);
delay(stepSpeed3);
up_down.write(j);
delay(stepSpeed3);
}
}
else if (readUpDown > 32){
for (int j = readUpDown; j>=32; j--){
//up_down.write(j);
delay(stepSpeed3);
up_down.write(j);
delay(stepSpeed3);
}
}
else{
// up_down.write(32);
delay(stepSpeed3);
up_down.write(32);
delay(stepSpeed3);
}
//servo2 Rotate 0-100
//0
//50
if (readRotate < 50){
for (int j = readRotate; j<=50; j++){
delay(stepSpeed);
rotate.write(j);
delay(stepSpeed);
}
}
else if (readRotate > 50){
for (int j = readRotate; j>=50; j--){
delay(stepSpeed);
rotate.write(j);
delay(stepSpeed);
}
}
else{
delay(stepSpeed);
rotate.write(50);
}
//servo1 Grab 180-126
if (readGrab < 148){
for (int j = readGrab; j<=148; j++){
delay(stepSpeed2);
grab.write(j);
delay(stepSpeed2);
}
}
else if (readGrab > 148){
for (int j = readGrab; j>=148; j--){
delay(stepSpeed2);
grab.write(j);
delay(stepSpeed2);
}
}
else{
delay(stepSpeed2);
grab.write(148);
delay(stepSpeed2);
}
//servo5 UpDown5 5-100
if (readUpDown5 < 32){
for (int j = readUpDown5; j<=32; j++){
delay(stepSpeed3);
up_down5.write(j);
delay(stepSpeed3);
}
}
else if (readUpDown5 > 32){
for (int j = readUpDown5; j>=32; j--){
delay(stepSpeed3);
up_down5.write(j);
delay(stepSpeed3);
}
}
//25620911 เพิ่ม else
else{
//up_down5.write(32);
delay(stepSpeed3);
up_down5.write(32);//25620911
delay(stepSpeed3);
}
//servo4 UpDown4 5-100
//32
//50
//32
if (readUpDown4 < 32){
for (int j = readUpDown4; j<=32; j++){
delay(stepSpeed3);
up_down4.write(j);
delay(stepSpeed3);
}
}
else if (readUpDown4 > 32){
for (int j = readUpDown4; j>=32; j--){
delay(stepSpeed3);
up_down4.write(j);
delay(stepSpeed3);
}
}
else{
up_down4.write(32);
delay(stepSpeed3);
delay(stepSpeed3);
}
//Always start from home position จุดเริ่มต้นไปตำแหน่งใหม่
teachForwardBackward[0]= 80;
//delay(stepSpeed);
teachUpDown[0]=32;
//delay(stepSpeed);
teachRotate[0]=0;
//delay(stepSpeed);
teachGrab[0]=148;
delay(stepSpeed);
//เพิ่มเซอร์โว4,5
teachUpDown4[0]=32;
delay(stepSpeed);
teachUpDown5[0]=32;
delay(stepSpeed);
}
//ไปยังตำแหน่งที่ต้องการ
void goPot()
//ลำดับการทำงานของเซอร์โว
//servo2 Rotate
//servo1 Grab
//servo6 ForwardBackward
//servo3 UpDown
//servo4 UpDown4
//servo5 UpDown5
{
//servo2 Rotate
if (0 > readRotate){
for (int j = 0; j>=readRotate; j--){
rotate.write(j);
delay(stepSpeed);
Serial.print("goPot_readRotate=");
Serial.println(j);
}
}
else if (readRotate > 0){
for (int j = 0; j<=readRotate; j++){
rotate.write(j);
delay(stepSpeed);
}
}
else{
rotate.write(readRotate);
}
//servo1 Grab
if (readGrab > 148){
for (int j = 148; j<=readGrab; j++){
grab.write(j);
delay(stepSpeed);
}
}
else if (readGrab < 148){
for (int j = 148; j>=readGrab; j--){
grab.write(j);
delay(stepSpeed);
}
}
else{
grab.write(readGrab);
}
//servo6 ForwardBackward
if (80 > readForwardBackward){
for (int j = 80; j>=readForwardBackward; j--){
forward_backward.write(j);
delay(stepSpeed);
}
}
else if (80 < readForwardBackward){
for (int j = 80; j<=readForwardBackward; j++){
forward_backward.write(j);
delay(stepSpeed);
}
}
else{
forward_backward.write(readForwardBackward);
delay(stepSpeed);
}
//servo3 UpDown
if (32 > readUpDown){
for (int j = 32; j>=readUpDown; j--){
up_down.write(j);
delay(stepSpeed);
}
}
else if (readUpDown > 32){
for (int j = 32; j<=readUpDown; j++){
up_down.write(j);
delay(stepSpeed);
}
}
else{
up_down.write(readUpDown);
delay(stepSpeed);
}
//เพิ่มเซอร์โว4,5
//servo4 UpDown4
if (32 > readUpDown4){
for (int j = 32; j>=readUpDown4; j--){
up_down4.write(j);
delay(stepSpeed);
}
}
else if (readUpDown4 > 32){
for (int j = 32; j<=readUpDown4; j++){
up_down4.write(j);
delay(stepSpeed);
//25620910เพิ่มหน่วยเวลา
delay(stepSpeed);
}
}
else{
up_down4.write(readUpDown4);
//25620910เพิ่มหน่วยเวลา
delay(stepSpeed);
delay(stepSpeed);
}
//servo5 UpDown5
if (32 > readUpDown5){
for (int j = 32; j>=readUpDown5; j--){
up_down5.write(j);
//25620910เพิ่มหน่วยเวลา
delay(stepSpeed);
delay(stepSpeed);
}
}
else if (readUpDown5 > 32){
for (int j = 32; j<=readUpDown5; j++){
up_down5.write(j);
//25620910เพิ่มหน่วยเวลา
delay(stepSpeed);
}
}
else{
up_down5.write(readUpDown5);
//25620910เพิ่มหน่วยเวลา
delay(stepSpeed);
}
}