น่าจะเอาไว้ในฟังก์ชัน readInputs()
ไว้หลัง readUpDownX ทุกมอเตอร์
แล้วก็ point ไปที่ตำแหน่ง eeprom นั้นๆ
น่าจะเอาไว้ในฟังก์ชัน readInputs()
ไว้หลัง readUpDownX ทุกมอเตอร์
แล้วก็ point ไปที่ตำแหน่ง eeprom นั้นๆ
กำลังนำข้อมูลไปพัฒนาต่อครับใกล้ถึงเป้าหมาย
เยี่ยมไปเลยครับ รอชม…
ตอนนี้ใช้อะไรเป็นแหล่งจ่ายไฟให้กับแขนกลครับ ตอนนี้ใช้กำลังไฟเท่าไหร่ในการทำงาน เรื่องความสมูทของมอเตอร์ก็แต่เผื่อเรื่องจ่ายไฟด้วย เพราะมอเตอร์ถูดไดร์ด้วยกระแสไฟฟ้า
ยังไม่ได้มีวิธีีคำนวณค่ากระแสกะว่าจะติดตั้งแอมมิสเตอร์วัดค่าตอนทำงาน เคยลองใช้สวิชชิงเหมือนว่าจะมีสัณณานกวน ตอนนี้เลือกใช้อะแดปเตอร์ 12 VDCจ่ายกระแสสูงสุด5A ต่อผ่านตัวแปลงไฟแบบปรับโวลท์ได้ 7.2 Vเลือกจ่าย5A เพื่อใช้กับเซอร์โวทั้ง6ตัวมีสวิทช์ตัดไฟเข้าเชอร์โวเพื่อความปลอดภัยช่วงที่ทดสอบโค้ดอาจทำให้แขนเคลื่อนที่ผิดพลาดตีหัวหรือหนีบมือได้ ตอนนี้เซอร์โวตัวที่2,3เริ่มกระตุกเมื่อจ่ายไฟเข้า ยังหาสาเหตุไม่เจอ เมื่อใช้ Serial.print เยอะทำให้หน่วยความจำเต็มและทำงานช้า จะขยายอย่างไรหรืออาจต้องเปลี่ยน บอร์ดที่เร็วขึ้นดีขึ้น(พัฒนาขั้นต่อไปจะใช้DUE 32bit)
เรื่องสเปคที่เพียงพอก็สำคัญเหมือนกันนะครับ จะเห็นว่าเริ่มมีหลายประเด็นที่อาจเป็นที่มาของปัญหาที่เราต้องการแก้ไข ยังไงตามกันต่อไปครับ
สำเร็จแล้วครับ ตอนนี้ให้ไปทำหน้าที่ขายอาหารปลามีฟังก์ชั่นเพิ่มตามนี้
1.สอนให้จำตำแหน่งทำงานซ้ำๆ
2’.บันทึกตำแหน่งทำงานใน EEPROM
3.นำข้อมูลจากEEPROMมาใช้เมื่อทำงานใหม่
4.สั่งให้ทำงานแบบรอบเดียวหยุด
5.สั่งให้ทำงานซ้ำหลายรอบได้โดยไม่หยุด
จะอับโหลดโค้ดให้ครับ
เยี่ยมไปเลยครับ ขอบคุณที่แชร์สิ่งดีๆ นะครับ
เดียวว่างๆ จะไปช่วยซื้ออาหารปลานะครับ อิอิ
สำเร็จแล้วได้งาน ตอนนี้ได้งานให้ไปทำหน้าที่ขายอาหารปลา ชวนเด็กมาใให้อาหารปลา ให้เด็กๆได้ศึกกษากัน และทดสอบความทนทานของเซอร์โว
ฟังก์ชั่นเพิ่มตามนี้
1.สอนให้จำตำแหน่งทำงานซ้ำๆ
2’.บันทึกตำแหน่งทำงานใน EEPROM
3.นำข้อมูลจากEEPROMมาใช้เมื่อทำงานใหม่
4.สั่งให้ทำงานแบบรอบเดียวหยุด
5.สั่งให้ทำงานซ้ำหลายรอบได้โดยไม่หยุด
6.บันทึกทับข้อมูลเก่าใน eeprom
มีฟังก์ืชันอะไรที่น่าสนใจเพิ่มเติมแนะนำด้วย
ตัวที่2 จะใช้ DC servo moter + encoder ใช้จับแผงโซล่าเซล ให้เคลื่อนที่ไปตามดวงอาทิตย์ ดูมอเตอร์มาหลายรุ่นแต่ยังเลือกไม่ถูก ทั้ง ac และ dc
สเปคเบื้องต้น ยังเลือกไม่ถูกใจ ราคาแพง
1.ใช้ไฟ DC ปลอดภัย
2.ขนาดมอเตอร์ใช้ตัวเล็กราคาไม่แพงเดี๋ยวนี้แผงโซล่าเซลมีแบบบางน้ำหนนักเบามาก ที่่ประมาณใว้ยกของได้ 1-3 กิโล
3.ใช้บอร์ดarduino เป็น คอนโทลเลอร์
ขอข้อมูลแนะนำครับ
โค้ดล่าสุดครับ
/* 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;
const int autoBT = 11;
//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 readAuto,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 count = 1;
int eeppos = 0;
int indexend ;
int saveindex=1;
int saveeeppos=0;
int runauto = 1 ;
//Change this to fo faster!
int stepSpeed = 10; //25620910 20 to 50 ช้าลง
int stepSpeed2 = 10; //25620920
int stepSpeed3 = 30; //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(autoBT, INPUT_PULLUP);//pin11
pinMode(buzzer, OUTPUT);//pin 10
//Do a smooth movement on startup, from home potion to pot position:
indexend = EEPROM.read(0);//อ่านค่าใน eeppos มาเก็บ
// readEeprom();//เพิ่มอ่านข้อมูลจากEEPROM มาใช้งาน
readInputs();
goHome();
goPot();
Serial.begin(9600);
Serial.println(indexend);
// Serial.println("--end-setup()--");
eeppos = 0;
}
void loop() {
Serial.println("..25621023_5_39..");
//เมื่อเปิดสวิทช์
if (!started){
readInputs();
moveServos();
readEeprom();//เพิ่มอ่านข้อมูลจากEEPROM มาใช้งาน
delay(100);
//1เมื่อกดปุ่มสอนการเคลื่อนที่
if (readTeach==LOW){
savePosition();//บันทึกข้อมูลใน index[],EEPROM manual
tone(buzzer,500);
delay(500);
noTone(buzzer);
}
//2.Manualเมื่อกดปุ่มสตาร์ท
if (readStart==LOW){
tone(buzzer,700);
started=true;
delay(1000);
noTone(buzzer);
}
}
else{
goHome();
runTeach();
}
//3.Autoเมื่อกดปุ่มauto
if (readAuto==LOW){
tone(buzzer,700);
delay(500);
noTone(buzzer);
goHome();
autorun();
}
Serial.println("--end loop--");
}
void autorun(){
Serial.println("--startAutoRun--");
goHome();
runTeach();
Serial.print("runauto = ");
Serial.println(runauto);
goHome();
runTeach();
Serial.print("runauto = ");
Serial.println(runauto++);
goHome();
runTeach();
Serial.print("runauto = ");
Serial.println(runauto++);
goHome();
runTeach();
Serial.print("runauto = ");
Serial.println(runauto++);
goHome();
runTeach();
Serial.print("runauto = ");
Serial.println(runauto++);
/*
Serial.print("runauto = ");
Serial.println(runauto);
if (runauto <= 2){
runTeach();
delay(500);
runauto++;
Serial.print("runauto = ");
Serial.println(runauto);
}
*/
runauto = 1;
Serial.println("--endAutoRun--");
}
void readInputs(){
//1.เก็บข้อมูลในตัวแปร เป็นค่าเริ่มต้น
//ลำดับการทำงานของเซอร์โว3,6,2,1,4,5
//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);
//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);
readAuto = digitalRead(autoBT);
Serial.println("--readInput--");
}
void moveServos(){
//ลำดับการทำงานของเซอร์โว 3,6,2,1,4,5
up_down.write(readUpDown);
// delay(stepSpeed);
// Serial.print("moveServos_up_down = ");
// Serial.println(readUpDown);
// Serial.println(up_down);
forward_backward.write(readForwardBackward);
// delay(stepSpeed);
// Serial.print("moveServos_forward_backward = ");
// Serial.println(readForwardBackward);
// Serial.println(forward_backward);
rotate.write(readRotate);
// delay(stepSpeed);
grab.write(readGrab);
// delay(stepSpeed);
//เพิ่มเซอร์โว4,5
up_down4.write(readUpDown4);
// delay(stepSpeed);
up_down5.write(readUpDown5);
// delay(stepSpeed);
Serial.println("--moveServos--");
}
//บันทึกข้อมูลเมื่อมีการให้เคลื่อนที่ทีละตำแหน่ง เข้า INDEX[],EEPROM()
void savePosition(){
// Serial.println(eeppos);//0
// Serial.println(index);//1
//ลำดับการทำงานของเซอร์โว 3,6,2,1,4,5
//2.บันทึกข้อมูลใน index[1],EEPORM เริ่มตำแหน่งที่1
//เริ่ม index=1,eeppos=0
eeppos = eeppos+1;
teachUpDown[index] = readUpDown;//index[1]ข้อมูลมาจากreadInputs,moveServos
EEPROM.write(eeppos,readUpDown);//เก็บข้อมูลใน eeppos(1)
Serial.print("index = ");
Serial.println(index);
Serial.print("EEPROM = ");
Serial.println(eeppos);
// Serial.println(teachUpDown[index]);
eeppos = eeppos+1;
teachForwardBackward[index] = readForwardBackward;
EEPROM.write(eeppos,readForwardBackward);
Serial.print("index = ");
Serial.println(index);
Serial.print("EEPROM = ");
Serial.println(eeppos);
// Serial.println(teachForwardBackward[index]);
eeppos = eeppos+1;
teachRotate[index] = readRotate;
EEPROM.write(eeppos,readRotate);
Serial.print("index = ");
Serial.println(index);
Serial.print("EEPROM = ");
Serial.println(eeppos);
// Serial.println(teachRotate[index]);
eeppos = eeppos+1;
teachGrab[index] = readGrab;
EEPROM.write(eeppos,readGrab);
Serial.print("index = ");
Serial.println(index);
Serial.print("EEPROM = ");
Serial.println(eeppos);
// Serial.println(teachGrab[index]);
//เพิ่มเซอร์โว4,5
eeppos = eeppos+1;
teachUpDown4[index] = readUpDown4;
EEPROM.write(eeppos,readUpDown4);
Serial.print("index = ");
Serial.println(index);
Serial.print("EEPROM = ");
Serial.println(eeppos);
// Serial.println(teachUpDown4[index]);
eeppos = eeppos+1;
teachUpDown5[index] = readUpDown5;
EEPROM.write(eeppos,readUpDown5);
Serial.print("index = ");
Serial.println(index);
Serial.print("EEPROM = ");
Serial.println(eeppos);
// Serial.println(teachUpDown5[index]);
/*
//indexสุดท้ายเก็บที่ index[0],eepposสุดท้ายเก็บที่ eeppos[0]
teachUpDown[indexend] = readUpDown;
teachForwardBackward[indexend] = readForwardBackward;
teachRotate[indexend] = readRotate;
teachGrab[indexend] = readGrab;
teachUpDown4[indexend] = readUpDown4;
teachUpDown5[indexend] = readUpDown5;
*/
EEPROM.write(0,index);//บันทึกข้อมูลสุดท้าย
/*eepposend = index * 6;
Serial.println("indexend = ");
Serial.println(indexend);
*/
indexend = EEPROM.read(0);//อ่านข้อมูลสุดท้าย
Serial.println("--savePosition--");
index++;
//เก็บข้อมูลเข้าindex ,eeprom
saveindex = indexend+1;//2
saveeeppos = eeppos;//6
/*โค้ดทดลอง
saveindex = indexend+1;//2
saveeeppos = eeppos;//6
*/
}
//4.นำข้อมูลมาใช้งาน อ่านค่าจาก eeppos ไปเก็บใน index ที่ 1 ถึง indexที่ eeppos/6
//-----------------------
void readEeprom(){
//ลำดับการทำงานของเซอร์โว
// 25620930//ลำดับการทำงานของเซอร์โว3,6,2,1,4,5
// eeppos = 0;
// index = 1;
// Serial.println("indexend = ");
// Serial.println(indexend);
if (count <= indexend) {
Serial.print("count = ");
Serial.println(count);//1 ตั้งค่าเริ่มนับในsetup
Serial.print("indexend = ");
Serial.println(indexend);//29 อ่านค่าใน eeppos ที่ 0
Serial.print("index = ");
Serial.println(index);//1 ตั้งค่าเริ่มต้นในsetup
eeppos = eeppos+1;//1,7,13,19,...
readUpDown = EEPROM.read(eeppos);//1
teachUpDown[index] = readUpDown;
//ข้อมูลถูกต้อง
Serial.print("EEPROM = ");
Serial.println(eeppos);//1
// Serial.print("teachUpDown[index] = ");
// Serial.println(teachUpDown[index]);
//
eeppos = eeppos+1;//2,8,14,20,...
readForwardBackward = EEPROM.read(eeppos);
teachForwardBackward[index] = readForwardBackward;
//ข้อมูลถูกต้อง
Serial.print("EEPROM = ");
Serial.println(eeppos);//2
// Serial.print("teachForwardBackward[index] = ");//2
// Serial.println(teachForwardBackward[index]);//2
//
eeppos = eeppos+1;//3,9,15,21,...
readRotate = EEPROM.read(eeppos);
teachRotate[index] = readRotate;
Serial.print("EEPROM = ");
Serial.println(eeppos);//3
// Serial.print("readRotate = ");
// Serial.println(readRotate);
eeppos = eeppos+1;//4,10,16,22,...
readGrab = EEPROM.read(eeppos);
teachGrab[index] = readGrab;
Serial.print("EEPROM = ");
Serial.println(eeppos);//3
// Serial.print("EEPROM_readGrab = ");
// Serial.println(readGrab);
eeppos = eeppos+1;//5,11,17,23,...
readUpDown4 = EEPROM.read(eeppos);
teachUpDown4[index] = readUpDown4;
Serial.print("EEPROM = ");
Serial.println(eeppos);//4
// Serial.print("EEPROM_readUpDown4 = ");
// Serial.println(readUpDown4);
eeppos = eeppos+1;//6,12,18,24,...
readUpDown5 = EEPROM.read(eeppos);
teachUpDown5[index] = readUpDown5;
Serial.print("EEPROM = ");
Serial.println(eeppos);//6
// Serial.print("EEPROM_readUpDown5 = ");
// Serial.println(readUpDown5);
Serial.println("--endreadEeprom()--");
index++;
count++;
}
//เมื่อสอนการเคลื่อนที่ใหม่ให้เริ่มต้นที่index=1,eeppos=0 เพื่อทับข้อมูลเก่า
if (count > indexend ){
index = saveindex;//1
eeppos = saveeeppos;//0
Serial.print("index = ");
Serial.println(saveindex);//1
Serial.print("eeppos = ");
Serial.println(saveeeppos);//0
}
}
// ดึงข้อมูลจาก index[] มาใช้
void runTeach(){
//ลำดับการทำงานของเซอร์โวเปลี่ยนลำดับการทำงานเซอรืโว25620919//3,6,2,1,4,5
Serial.print("indexend = ");//เป็นindexสุดท้ายที่บันทึกใน eeppos(0) = index
Serial.println(indexend);//จำนวนชุดตำแหน่งเซอร์โวที่เก็บใน eeppos(0) = index
index = indexend;
Serial.println(index);//จำนวนชุดตำแหน่งของเซอร์โว6ตัว
for (int i=0; i<index-1; i++){
//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("teachUpDown3 = ");
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);
}
//servo6 ForwardBackward
if (teachForwardBackward[i] < teachForwardBackward[i+1]){
for (int j = teachForwardBackward[i]; j<= teachForwardBackward[i+1]; j++){
forward_backward.write(j);
delay(stepSpeed);
Serial.print("teachForward6 = ");
Serial.println(j);
}
}
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]);
}
//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("teachRotate2=");
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);
}
//servo1 Grab
if (teachGrab[i] < teachGrab[i+1]){
for (int j = teachGrab[i]; j<= teachGrab[i+1]; j++){
delay(stepSpeed3);
grab.write(j);
delay(stepSpeed3);
Serial.print("teachGrab1 = ");
Serial.println(j);
}
}
else if (teachGrab[i] > teachGrab[i+1]){
for (int j = teachGrab[i]; j>= teachGrab[i+1]; j--){
delay(stepSpeed3);
grab.write(j);
delay(stepSpeed3);
}
}
else{
delay(stepSpeed3);
grab.write(teachGrab[i]);
delay(stepSpeed3);
}
//servo4 UpDown4
if (teachUpDown4[i] < teachUpDown4[i+1]){
for (int j = teachUpDown4[i]; j<= teachUpDown4[i+1]; j++){
up_down4.write(j);
delay(stepSpeed3);
Serial.print("teachUpDown4 = ");
Serial.println(j);
}
}
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);
Serial.print("teachUpDown5 = ");
Serial.println(j);
}
}
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);
}
}
started=false;
Serial.println("--endrunTeach--");
}
//Change values if it's necessary...
void goHome(){
//ลำดับการทำงานของเซอร์โว
//servo3 UpDown 5-100
//servo6 ForwardBackward 160-80
//servo2 Rotate 0-100
//servo1 Grab 180-126
//servo4 UpDown4 5-100
//servo5 UpDown5 5-100
//25621008
//ลำดับการทำงานของเซอร์โว3,6,2,1,4,5
//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);
}
//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);
}
//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 < 150){
for (int j = readGrab; j<=150; j++){
delay(stepSpeed3);
grab.write(j);
delay(stepSpeed2);
}
}
else if (readGrab > 150){
for (int j = readGrab; j>=150; j--){
delay(stepSpeed3);
grab.write(j);
delay(stepSpeed3);
}
}
else{
delay(stepSpeed3);
grab.write(150);
delay(stepSpeed3);
}
//servo4 UpDown4 5-100
//32
//50
//32
if (readUpDown4 < 60){
for (int j = readUpDown4; j<=60; j++){
delay(stepSpeed3);
up_down4.write(j);
delay(stepSpeed3);
}
}
else if (readUpDown4 > 60){
for (int j = readUpDown4; j>=60; j--){
delay(stepSpeed3);
up_down4.write(j);
delay(stepSpeed3);
}
}
else{
up_down4.write(60);
delay(stepSpeed3);
delay(stepSpeed3);
}
//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);
}
//Always start from home position จุดเริ่มต้นไปตำแหน่งใหม่
teachForwardBackward[0]= 80;
//delay(stepSpeed);
teachUpDown[0]=32;
//delay(stepSpeed);
teachRotate[0]=0;
delay(stepSpeed);
teachGrab[0]=150;
//teachGrab[0]=148;
delay(stepSpeed);
//เพิ่มเซอร์โว4,5
//teachUpDown4[0]=32;
teachUpDown4[0]=60;
delay(stepSpeed);
teachUpDown5[0]=32;
delay(stepSpeed);
Serial.println("--end goHome--");
}
//ไปยังตำแหน่งที่ต้องการ
void goPot(){
//ลำดับการทำงานของเซอร์โว3,6,2,1,4,5
Serial.println("--goPot--");
//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);
}
//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);
}
//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 > 180){
for (int j = 180; j<=readGrab; j++){
grab.write(j);
delay(stepSpeed);
}
}
else if (readGrab < 180){
for (int j = 180; j>=readGrab; j--){
grab.write(j);
delay(stepSpeed);
}
}
else{
grab.write(readGrab);
}
//เพิ่มเซอร์โว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);
}
Serial.println("--end goPot--");
}type or paste code here
ขอบคุณที่แชร์ครับ มีเวลาเอารูปใช้งานจริงมาชมกันหน่อยนะครับ
การทดสอบใช้งานจริง เชื่อฟังดี ไม่มีปัญหา เชื่อง รอดูความเสถียรระยะยาว ครับ
กำลังหาเซ็นเซอร์ใส่เพื่อให้ตามจับสิ่งของที่มือและส่งไปวางที่กำหนดใว้
และกำหนดแรงบีบจับสิ่งของ จะทำให้หุ่นยนต์มีชีวิตชีวากับคนเล่น
ใช้เซ็นเซอร์อะไรดีครับ
เห็นแล้วน่าสนุกนะครับ เอาไปประยุกค์ได้มากมายเลยทีเดียว
เอาข้อมูลไปตั้งกระทู้ใหม่เลยครับ ถ่ายภาพมาดูด้วย จะได้มองภาพกันออกครับ
น่าจะแบบนี้รึป่าว
แปลงเอาก็น่าจะได้นะครับ
โค้ดตัวไหนครับเดียวลองเอาไปแปลงดูครับ
โค้ดด้านบนใช้เซอร์โว6ตัวถ้าใช้เซอร์น้อยกว่าตัดโค้ดออกได้ครับหรือเสียบสายเซอโวช่องที่ทำงาน