คำอธิบาย
Biped Robot เรียนรู้การเขียนโปรแกรมควบคุมหุ่นยนต์ เดิน 2 ขา ที่มีอินพุทเป็น อัลตราโซนิคเซนเซอร์ ใช้ Micro servo motors (SG90 9G) 4ชิ้น 4DOF บอร์ดควบคุมเป็น Biped Robot Arduino Nano
ฟังก์ชั่นพื้นฐาน: การเดิน 2ขา, การเดินหลบหลีกสิ่งกีดขวาง, การควบคุมไร้สายด้วยไออาร์ รีโมท IR Remote
รองรับการเขียนโปรแกรมหลากหลายรูปแบบ
- Arduino IDE, Graphical programming แบบลากและวาง ตามแพลทฟอร์ม Blockly แพลทฟอร์ม Scratch เช่น Mixly เป็นต้น
- เหมาะสำหรับ อายุ 8 ปีขึ้นไป เด็กๆ จะได้เรียนรู้ หุ่นยนต์ ในรูปแบบของ STEM Education ด้วยไปพร้อมๆกัน
รายละเอียด:
- Microcontroller: Arduino Nano
- Power: AA ฺBattery 4 ก้อน
- Micro servo motors (SG90 9G) 4ชิ้น 4 degree of freedom (DOF)
มีอะไรเตรียมไว้ให้ในชุด
- Biped Robot หุ่นยนต์เดิน 2 ขา ประกอบแล้ว (ไม่รวมแบตเตอรี่)
- Arduino Nano Compatible
- Robot Shield
- Sensor อัลตร้าโซนิค
- IR รีโมทคอนโทรล และ ตัวรับ IR
- สายโปรแกรม USB
ตัวอย่าง การเขียนโปรแกรม Arduino ควบคุมหุ่นยนต์ สามารถใช้โปรแกรมอาดูโน่ได้ด้วย
โดยโปรแกรมตัวอย่างประกอบด้วย
4 DOF Servo
- RU upper-right servo to pin 3
- RL lower-right servo to pin 5
- LU upper-left servo to pin 6
- LL lower-left servo to pin 9
IR remote อินฟราเรด
- ir_run = 0x00FF629D; / ปุ่ม CH
- ir_back = 0x00FFA857; / ปุ่ม +
- ir_left = 0x00FF22DD; / ปุ่ม <<
- ir_right = 0x00FFC23D; // ปุ่ม> ||
- ir_stop = 0x00FF02FD; / ปุ่มปุ่ม >> |
- ir_left_turn = 0x00ffE01F; / ปุ่ม –
- ir_right_turn = 0x00FF906F; / ปุ่ม EQ
- โปรแกรมการเซ็ตซีโร่เซอร์โว
- โปรแกรมตัวอย่างเดินหน้าถอยหลังเลี้ยวซ้ายเลี้ยว
- โปรแกรมหุ่นยนต์แดนซ์
- โปรแกรมหุ่นยนต์เดินหลบหลีกสิ่งกีดขวางโดยใช้เซ็นเซอร์อัลตร้าโซนิค
- โปรแกรมควบคุมหุ่นยนต์เดินโดยใช้รีโมทคอนโทรล
- โปรแกรมควบคุมหุ่นยนต์เดินโดยใช้คอนเดนเซอร์ไมโครโฟน
- โปรแกรมควบคุมหุ่นยนต์เดินโดยใช้ Sensor LDR
ทดสอบ ไออาร์รีโมทก่อนเริ่มควบคุมหุ่นยนต์
/*
* IRremote: IRrecvDemo - demonstrates receiving IR codes with IRrecv
* An IR detector/demodulator must be connected to the input RECV_PIN.
* Version 0.1 July, 2009
* Copyright 2009 Ken Shirriff
* http://arcfn.com
*/
#include <IRremote.h>
int RECV_PIN = A4;
IRrecv irrecv(RECV_PIN);
decode_results results;
void setup()
{
Serial.begin(9600);
// In case the interrupt driver crashes on setup, give a clue
// to the user what's going on.
Serial.println("Enabling IRin");
irrecv.enableIRIn(); // Start the receiver
Serial.println("Enabled IRin");
}
void loop() {
if (irrecv.decode(&results)) {
Serial.println(results.value, HEX);
irrecv.resume(); // Receive the next value
}
delay(100);
}
ค่า Data ของข้อมูลปุ่มกดที่ได้จาก ไออาร์รีโมท (รีโมทแต่ละรุ่น Data จะไม่เหมือนกัน)
| KEY | DATA |
| CH+ | E318261B |
| CH | 511DBB |
| CH- | EE886D7F |
| |<< | 52A3D41F |
| >>| | D7E84B1B |
| >|| | 20FE4DBB |
| – | F076C13B |
| + | A3C8EDDB |
| EQ | E5CFBD7F |
| 0 | C101E57B |
| 100 | 97483BFB |
| 200 | F0C41643 |
| 1 | 9716BE3F |
| 2 | 3D9AE3F7 |
| 3 | 6182021B |
| 4 | 8C22657B |
| 5 | 488F3CBB |
| 6 | 449E79F |
| 7 | 32C6FDF7 |
| 8 | 1BC0157B |
| 9 | 3EC3FC1B |
ตัวอย่าง การเขียนโปรแกรม Arduino ควบคุมหุ่นยนต์ แบบใช้ IR Remote
// IR Remote
#include <IRremote.h>//Contains infrared library key points
#include "VarSpeedServo.h" // VarSpeedServo library
int RECV_PIN = A4;//Port declaration
IRrecv irrecv(RECV_PIN);
decode_results results;//Structure statement
int on = 0;//Flag bit
unsigned long last = millis();
long ir_run_car = 0x511DBB;//button CH
long ir_back_car = 0xA3C8EDDB;//button +
long ir_left_car = 0x52A3D41F;//button <<
long ir_right_car = 0x20FE4DBB;//button >||
long ir_stop_car = 0xD7E84B1B;//button >>|
long ir_left_turn = 0xF076C13B;//button -
long ir_right_turn = 0xE5CFBD7F;//button EQ
//==============================
VarSpeedServo RU; //Right Upper
VarSpeedServo RL; //Right Lower
VarSpeedServo LU; //Left Upper
VarSpeedServo LL; //Left Lower
int beep=A3;//Define the buzzer Digital A3 interface
int Echo = A1; // Echo (P2.0)
int Trig = A0; // Trig (P2.1)
int Distance = 0;
//vel(min), delay_Forward(max) = (5, 2000)
const int vel = 50, vel_Back = 15, vel_turn= 15; //vel(mid), delay_Forward(mid) = (20, 750)
const int delay_Forward = 700, delay_Back = 750, delay_turn = 500; //vel(max), delay_Forward(min)= (256, 50)
//wonderful ---> (10, 700) (50, 500) (100, 100) (100, 300) (100, 500)
const int array_cal[4] = {90,90,90,90}; // Define the angular adjustment of servo (RU, RL, LU, LL )
const int delay_tim = 300; //Delay 750ms
int RU_Degree = 0, LU_Degree = array_cal[2] + 5;
const int num1 = 6;
const int array_forward[num1][4] =
{
{0,-40,0,-20},
{30,-40,40,-20},
{30,0,40,0},
{0,20,0,40},
{-30,20,-30,40},
{-30,0,-30,0},
};
const int num2 = 6;
const int array_back[num2][4] ={
{-40,0,-20,0},
{-40,30,-20,30},
{0,30,0,30},
{40,0,50,0},
{40,-30,50,-40},
{0,-30,0,-40},
};
const int num3 = 5; // Define the array number 5
/* When I encountered obstacles, turned around. Each set requires 5 sets of data. Angle change of each group of 4 servo systems*/
const int array_left[num3][4] =
{
{-40,0,-20,0}, // The left foot turns left at 20 degrees and the right foot turns left at 40 degrees.
{-40,30,-20,30}, // The left foot raises and the right foot supports the ground to keep balance
{0,30,0,30},
{30,0,30,0},
{0,0,0,0},
}; //Define 5 arrays, each containing 4 elements to store the current angular value of the servo。
const int num4 = 5;
const int array_right[num4][4] =
{
{40,0,50,0},
{40,-30,50,-40},
{0,-30,0,-40},
{-30,0,-40,0},
{0,0,0,0},
};
//#define INSTALL
//#define CALIBRATION
#define RUN
void Servo_Init()
{
RU.attach(3); // Connect the signal wire of the upper-right servo to pin 9
RL.attach(5); // Connect the signal wire of the lower-right servo to pin 10
LU.attach(6); // Connect the signal wire of the upper-left servo to pin 11
LL.attach(9); // Connect the signal wire of the lower-left servo to pin 12
}
void Adjust() // Avoid the servo's fast spinning in initialization
{ // RU,LU goes from array_cal[0] - 5 ,array_cal[2] + 5 degrees to array_cal[0],array_cal[2] degrees
for(RU_Degree = array_cal[0] - 5; RU_Degree <= array_cal[0]; RU_Degree += 1) {
RU.write(RU_Degree); // in steps of 1 degree
LU.write(LU_Degree--); // tell servo to go to RU_Degreeition, LU_Degreeition in variable 'RU_Degree', 'LU_Degree'
delay(15); // waits 15ms for the servo to reach the RU_Degreeition
}
}
void TooClose()
{
digitalWrite(Trig, LOW); // Give the trigger pin low 2μs
delayMicroseconds(2);
digitalWrite(Trig, HIGH); // Give the trigger pin a high level of 10μs, here at least 10μs
delayMicroseconds(10);
digitalWrite(Trig, LOW); // Continue to low power to the trigger pin
float Fdistance = pulseIn(Echo, HIGH); // Read high time (unit: microsecond)
Fdistance= Fdistance/58; // divide by 58 equals centimeter, Y m = (X seconds * 344)/2
// X seconds = ( 2 * Y meters) / 344 = = "X seconds = 0.0058 * Y meters = = "cm = microseconds / 58
Serial.print("Distance:"); //Output distance (unit: cm)
Serial.println(Fdistance); //Display distance
Distance = Fdistance;
}
void Forward(unsigned int n_delay)
{
for(int x=0; x<num1; x++) {
RU.slowmove (array_cal[0] + array_forward[x][0] , vel);
RL.slowmove (array_cal[1] + array_forward[x][1] , vel);
LU.slowmove (array_cal[2] + array_forward[x][2] , vel);
LL.slowmove (array_cal[3] + array_forward[x][3] , vel);
delay(n_delay); //delay_Forward :700
}
}
void stop()
{
RU.slowmove (array_cal[0] , vel);
RL.slowmove (array_cal[1] , vel);
LU.slowmove (array_cal[2] , vel);
LL.slowmove (array_cal[3] , vel);
delay(delay_tim);
}
void Backward(unsigned int n_delay)
{
for(int y=0; y<num2; y++) {
RU.slowmove (array_cal[0] + array_back[y][0] , vel_Back);
RL.slowmove (array_cal[1] + array_back[y][1] , vel_Back);
LU.slowmove (array_cal[2] + array_back[y][2] , vel_Back);
LL.slowmove (array_cal[3] + array_back[y][3] , vel_Back);
delay(n_delay); //delay_Forward:700
}
}
void left_turn(unsigned int n_delay)
{
for(int z=0; z<2; z++)
{
for(int y=0; y<num3; y++)
{
RU.slowmove (array_cal[0] + array_left[y][0] , vel_Back);
RL.slowmove (array_cal[1] + array_left[y][1] , vel_Back);
LU.slowmove (array_cal[2] + array_left[y][2] , vel_Back);
LL.slowmove (array_cal[3] + array_left[y][3] , vel_Back);
delay(n_delay); //delay_Back:750
}
}
}
void right_turn(unsigned int n_delay)
{
for(int z=0; z<2; z++)
{
for(int y=0; y<num4; y++)
{
RU.slowmove (array_cal[0] + array_right[y][0] , vel_Back);
RL.slowmove (array_cal[1] + array_right[y][1] , vel_Back);
LU.slowmove (array_cal[2] + array_right[y][2] , vel_Back);
LL.slowmove (array_cal[3] + array_right[y][3] , vel_Back);
delay(n_delay); //delay_Back:750
}
}
}
void n_Forward(unsigned char n_step,unsigned int n_delay)
{
unsigned char z;
for(z = 0;z < n_step; z++)
{
Forward(n_delay);
}
}
void n_Backward(unsigned char n_step,unsigned int n_delay)
{
unsigned char z;
for(z = 0;z < n_step; z++)
{
Backward(n_delay);
}
}
void n_left_turn(unsigned char n_step,unsigned int n_delay)
{
unsigned char z;
for(z = 0;z < n_step; z++)
{
left_turn(n_delay);
}
}
void n_right_turn(unsigned char n_step,unsigned int n_delay)
{
unsigned char z;
for(z = 0;z < n_step; z++)
{
right_turn(n_delay);
}
}
void setup()
{
#ifdef INSTALL
Servo_Init();
RU.slowmove (90 , vel);
RL.slowmove (90 , vel);
LU.slowmove (90 , vel);
LL.slowmove (90 , vel);
while(1);
#endif
#ifdef CALIBRATION
Servo_Init();
RU.slowmove (array_cal[0] , vel); // Define the angle and speed of the upper-right servo.
RL.slowmove (array_cal[1] , vel);
LU.slowmove (array_cal[2] , vel);
LL.slowmove (array_cal[3] , vel);
delay(2000);
while(1);
#endif
#ifdef RUN
Servo_Init();
RU.slowmove (array_cal[0] , vel);
RL.slowmove (array_cal[1] , vel);
LU.slowmove (array_cal[2] , vel);
LL.slowmove (array_cal[3] , vel);
delay(2000);
#endif
irrecv.enableIRIn(); // tart the infrared remote control
pinMode(Echo, INPUT); // Define the ultrasonic input pin
pinMode(Trig, OUTPUT); // Define the ultrasonic output pin
pinMode(beep,OUTPUT); // buzzer
Serial.begin(9600);
}
void dump(decode_results *results)
{
int count = results->rawlen;
if (results->decode_type == UNKNOWN)
{
//Serial.println("Could not decode message");
// brake();
}
}
void loop()
{
if (irrecv.decode(&results)) //Call library function: decode
{
// If it's been at least 1/4 second since the last
// IR received, toggle the relay
if (millis() - last > 250) //Determine the signal received
{
on = !on;//Mark position
digitalWrite(13, on ? HIGH : LOW);//The signal received on the board flashesled
dump(&results);//Decoding infrared signal
}
if (results.value == ir_run_car )//buttonCH
{
n_Forward(4,250); //go ahead
}
if (results.value == ir_back_car )//button+
{
n_Backward(2,650);//Retreat
}
if (results.value == ir_left_car )//button<<
{
n_left_turn(1,650);//Turn left
}
if (results.value == ir_right_car )//button>||
{
n_right_turn(1,650);//Turn right
}
if (results.value == ir_stop_car )//button>>|
{
stop();//stop
}
last = millis();
irrecv.resume(); // Receive the next value
}
}
ตัวอย่าง Upload โปรแกรม Arduino ให้หุ่นเต้น (Dance)
//dance
#include "VarSpeedServo.h"
VarSpeedServo RU;
VarSpeedServo RL;
VarSpeedServo LU;
VarSpeedServo LL;
const int vel = 50, vel_Back = 15, vel_turn= 15;
const int delay_Forward = 700, delay_Back = 750, delay_turn = 500;
int vel_Dance1 = 30,vel_Dance2 = 25, vel_Dance3 = 40;
int delay_Dance1 = 300,delay_Dance2 = 750, delay_Dance3 = 200;
int vel_Dance4 = 20, delay_Dance4 = 400;
const int array_cal[4] = {90,90,90,90};
int RU_Degree = 0, LU_Degree = array_cal[2] + 5;
const int num1 = 6;
const int array_forward[num1][4] =
{
{0,-40,0,-20},
{30,-40,30,-20},
{30,0,30,0},
{0,20,0,40},
{-30,20,-30,40},
{-30,0,-30,0},
};
const int num3 = 5;
const int array_left[num3][4] =
{
{-40,0,-20,0},
{-40,30,-20,30},
{0,30,0,30},
{30,0,30,0},
{0,0,0,0},
};
const int num4 = 5;
const int array_right[num4][4] = {
{40,0,20,0},
{40,-30,20,-20},
{0,-30,0,-20},
{-30,0,-30,0},
{0,0,0,0},
};
const int num_dance1 = 10;
const int array_dance1[num_dance1][4] ={
{0,-20,0,0},
{0,-40,0,20},
{0,-20,0,40},
{0,0,0,20},
{0,0,0,0},
{0,0,0,20},
{0,-20,0,40},
{0,-40,0,20},
{0,-20,0,0},
{0,0,0,0},
};
const int num_dance2 = 32;
const int array_dance2[num_dance2][4] = {
{20,0,40,0},
{20,-30,10,-30},
{20,-30,10,-30},
{20,-30,40,-30},
{20,-30,10,-30},
{20,-30,40,-30},
{20,0,40,-30},
{20,80,40,-30},
{20,0,40,-30},
{20,-80,40,-30},
{20,0,40,-30},
{20,80,40,-30},
{20,0,40,-30},
{20,-30,40,-30},
{20,0,40,0},
{0,0,0,0},
{-40,0,-20,0},
{-40,40,-20,30},
{-20,40,-20,30},
{-40,40,-20,30},
{-20,40,-20,30},
{-40,40,-20,30},
{-40,40,-20,0},
{-40,40,-20,-80},
{-40,40,-20,0},
{-40,40,-20,80},
{-40,40,-20,0},
{-40,40,-20,-80},
{-40,40,-20,0},
{-40,40,-20,30},
{-40,0,-20,0},
{0,0,0,0},
};
const int num_dance4 = 20;
const int array_dance4[num_dance4][4] = {
{0,-20,0,0},
{0,-40,0,20},
{0,0,0,0},
{0,-20,0,20},
{0,0,0,0},
{0,-20,0,20},
{0,0,0,0},
{0,-20,0,20},
{0,0,0,0},
{0,-30,0,30},
{0,0,0,0},
{0,-30,0,30},
{0,0,0,0},
{0,-30,0,30},
{0,0,0,0},
{0,-40,0,40},
{0,-40,0,40},
{0,-50,0,50},
{0,0,0,0,},
};
const int num_dance3 = 8;
const int array_dance3[num_dance3][4] ={
{0,-40,0,0},
{20,-30,20,20},
{40,0,40,30},
{0,0,0,40},
{-20,-20,-20,30},
{-40,-30,-40,0},
{0,-40,0,0},
{0,0,0,0},
};
const int num2 = 6;
const int array_back[num2][4] ={
{-40,0,-20,0},
{-40,30,-20,30},
{0,30,0,30},
{40,0,20,0},
{40,-30,20,-30},{0,-30,0,-30},
};
#define RUN
void Servo_Init()
{
RU.attach(3);
RL.attach(5);
LU.attach(6);
LL.attach(9);
}
void Adjust()
{
for(RU_Degree = array_cal[0] - 5; RU_Degree <= array_cal[0]; RU_Degree += 1) {
RU.write(RU_Degree);
LU.write(LU_Degree--);
delay(15);
}
}
//bool TooClose()
//{
//int tooclose = 0;
//for(int a=0; a<5; a++) {
// delay(50);
//int din = sonar.ping_in();
//if (din < 7 && din > 0) tooclose++;
// }
//if (tooclose < 5) return 1;
//return 0;
//}
void Forward()
{
for(int x=0; x<num1; x++) {
RU.slowmove (array_cal[0] + array_forward[x][0] , vel);
RL.slowmove (array_cal[1] + array_forward[x][1] , vel);
LU.slowmove (array_cal[2] + array_forward[x][2] , vel);
LL.slowmove (array_cal[3] + array_forward[x][3] , vel);
delay(delay_Forward);
}
}
void Turnleft()
{
for(int z=0; z<2; z++) {
for(int y=0; y<num3; y++) {
RU.slowmove (array_cal[0] + array_left[y][0] , vel_Back);
RL.slowmove (array_cal[1] + array_left[y][1] , vel_Back);
LU.slowmove (array_cal[2] + array_left[y][2] , vel_Back);
LL.slowmove (array_cal[3] + array_left[y][3] , vel_Back);
delay(delay_Back);
}
}
}
void Turnright(){
for(int z=0; z<2; z++) {
for(int y=0; y<num4; y++) {
RU.slowmove (array_cal[0] + array_right[y][0] , vel_Back);
RL.slowmove (array_cal[1] + array_right[y][1] , vel_Back);
LU.slowmove (array_cal[2] + array_right[y][2] , vel_Back);
LL.slowmove (array_cal[3] + array_right[y][3] , vel_Back);
delay(delay_Back);
}
}
}
void Slide_2_Left(int times)
{
for(int time1 = 0; time1 < times; time1++)
{
for(int z=0; z<5; z++)
{
vel_Dance2 = 50;
RU.slowmove (array_cal[0] + array_dance1[z][0] , vel_Dance1);
LU.slowmove (array_cal[2] + array_dance1[z][2] , vel_Dance1);
LL.slowmove (array_cal[3] + array_dance1[z][3] , vel_Dance1);
delay(delay_Dance1);}
}
}
void Left_Foot_Support(){
for(int z=0; z<16; z++) {
if ( z > 5 && z < 14)
{
vel_Dance2 = 50;delay_Dance2 = 200;
}
else {
vel_Dance2 = 25;
delay_Dance2 = 750;
}
RU.slowmove (array_cal[0] + array_dance2[z][0] , vel_Dance2);
RL.slowmove (array_cal[1] + array_dance2[z][1] , vel_Dance2);
LU.slowmove (array_cal[2] + array_dance2[z][2] , vel_Dance2);
LL.slowmove (array_cal[3] + array_dance2[z][3] , vel_Dance2);
delay(delay_Dance2);
}
}
void Dancing1_2()
{
Slide_2_Left(2);
Left_Foot_Support();
}
void Dancing4()
{
for(int z=0; z<num_dance4; z++)
{
if ( z > 17) {
vel_Dance4 = 10;
delay_Dance4 = 1500;}
else
{
vel_Dance4 = 40;
delay_Dance4 = 400;}
RU.slowmove (array_cal[0] + array_dance4[z][0] , vel_Dance4);
RL.slowmove (array_cal[1] + array_dance4[z][1] , vel_Dance4);
LU.slowmove (array_cal[2] + array_dance4[z][2] , vel_Dance4);
LL.slowmove (array_cal[3] + array_dance4[z][3] , vel_Dance4);
delay(delay_Dance4);
}
}
void Dancing3(int Times = 1, int Vel = 40, int Delay = 250, int low = 0, int high = 0)
{
for(int time3=0; time3<Times; time3++)
{
for(int z=0; z<6; z++) { if ( time3 > 1 && time3 < 4)
{
vel_Dance3 = Vel;
delay_Dance3 = Delay;
}
else
{
vel_Dance3 = 40;
delay_Dance3 = 200;
}
RU.slowmove (array_cal[0] + array_dance3[z][0] , vel_Dance3);
RL.slowmove (array_cal[1] + array_dance3[z][1] , vel_Dance3);
LU.slowmove (array_cal[2] + array_dance3[z][2] , vel_Dance3);
LL.slowmove (array_cal[3] + array_dance3[z][3] , vel_Dance3);
delay(delay_Dance3);
}
}
for(int z=6; z<8; z++) {
RU.slowmove (array_cal[0] + array_dance3[z][0] , vel_Dance3);
RL.slowmove (array_cal[1] + array_dance3[z][1] , vel_Dance3);
LU.slowmove (array_cal[2] + array_dance3[z][2] , vel_Dance3);
LL.slowmove (array_cal[3] + array_dance3[z][3] , vel_Dance3);
delay(delay_Dance3);
}
}
void Backward(){
for(int y=0; y<num2; y++)
{
RU.slowmove (array_cal[0] + array_back[y][0] , vel_Back);
RL.slowmove (array_cal[1] + array_back[y][1] , vel_Back);
LU.slowmove (array_cal[2] + array_back[y][2] , vel_Back);
LL.slowmove (array_cal[3] + array_back[y][3] , vel_Back);
delay(delay_Forward);
}
}
void setup()
{
#ifdef RUN
Servo_Init();
Adjust();
RL.slowmove (array_cal[1] , vel);
LL.slowmove (array_cal[3] , vel);
delay(2000);
#endif
}
void loop()
{
Dancing3(5,20,400);
}
โปรแกรม Arduino เซตซีโร่ เซอโวร์
// 1. Biped robot servo zero adjustment program
#include "VarSpeedServo.h" //contain VarSpeedServo library
VarSpeedServo RU; //Right Upper
VarSpeedServo RL; //Right Lower
VarSpeedServo LU; //Left Upper
VarSpeedServo LL; //Left Lower
int beep=A3;//Define the buzzer Digital A3 interface
int Echo = A1; // Echo pin(P2.0)
int Trig = A0; // Trig pin(P2.1)
int Distance = 0;
//vel(min), delay_Forward(max) = (5, 2000)
const int vel = 20, vel_Back = 10; //vel(mid), delay_Forward(mid) = (20, 750)
const int delay_Forward = 750, delay_Back = 1000; //vel(max), delay_Forward(min)= (256, 50)
//wonderful ---> (10, 700) (50, 500) (100, 100) (100, 300) (100, 500)
const int array_cal[4] = {90,80,100,90}; // Define the angular adjustment of servo (RU, RL, LU, LL )
const int delay_tim = 300; //Delay 750ms
int RU_Degree = 0, LU_Degree = array_cal[2] + 5;
const int num1 = 6;
const int array_forward[num1][4] =
{
{0,-40,0,-20},
{30,-40,30,-20},
{30,0,30,0},
{0,20,0,40},
{-30,20,-30,40},
{-30,0,-30,0},
};
const int num2 = 6;
const int array_back[num2][4] ={
{-40,0,-20,0},
{-40,30,-20,30},
{0,30,0,30},
{40,0,20,0},
{40,-30,20,-30},
{0,-30,0,-30},
};
const int num3 = 5; // Define the array number to be 5.
/* Turned when you encountered an obstacle. Each set requires 5 sets of data. Angle change of 4 servo systems per group*/
const int array_left[num3][4] =
{
{-40,0,-20,0}, // The left foot turns left at 20 degrees and the right foot turns left at 40 degrees.
{-40,30,-20,30}, // The left foot raises and the right foot supports the ground to keep balance
{0,30,0,30},
{30,0,30,0},
{0,0,0,0},
}; // Define 5 arrays, each containing 4 elements to store the current angle value of the servo.)
const int num4 = 5;
const int array_right[num4][4] =
{
{40,0,20,0},
{40,-30,20,-20},
{0,-30,0,-20},
{-30,0,-30,0},
{0,0,0,0},
};
//#define INSTALL
#define CALIBRATION
//#define RUN
void Servo_Init()
{
RU.attach(3); // Connect the signal wire of the upper-right servo to pin 3
RL.attach(5); // Connect the signal wire of the lower-right servo to pin 5
LU.attach(6); // Connect the signal wire of the upper-left servo to pin 6
LL.attach(9); // Connect the signal wire of the lower-left servo to pin 9
}
void Adjust() // Avoid the servo's fast spinning in initialization
{ // RU,LU goes from array_cal[0] - 5 ,array_cal[2] + 5 degrees to array_cal[0],array_cal[2] degrees
for(RU_Degree = array_cal[0] - 5; RU_Degree <= array_cal[0]; RU_Degree += 1) {
RU.write(RU_Degree); // in steps of 1 degree
LU.write(LU_Degree--); // tell servo to go to RU_Degreeition, LU_Degreeition in variable 'RU_Degree', 'LU_Degree'
delay(15); // waits 15ms for the servo to reach the RU_Degreeition
}
}
void TooClose()
{
digitalWrite(Trig, LOW); // Give the trigger pin low level 2μs
delayMicroseconds(2);
digitalWrite(Trig, HIGH); // Give the trigger pin a high level of 10μs, here at least 10μs
delayMicroseconds(10);
digitalWrite(Trig, LOW); // Continue to low power to the trigger pin
float Fdistance = pulseIn(Echo, HIGH); // Read high time (unit: microsecond)
Fdistance= Fdistance/58; //Why divide by 58 equals centimeter, Y m = (X seconds * 344)/2
// X seconds = ( 2 * Y meters) / 344 = = "X seconds = 0.0058 * Y meters = = "cm = microseconds / 58
Serial.print("Distance:"); // Output distance (unit: cm))
Serial.println(Fdistance); //Display distance
Distance = Fdistance;
}
void Forward()
{
for(int x=0; x<num1; x++) {
RU.slowmove (array_cal[0] + array_forward[x][0] , vel);
RL.slowmove (array_cal[1] + array_forward[x][1] , vel);
LU.slowmove (array_cal[2] + array_forward[x][2] , vel);
LL.slowmove (array_cal[3] + array_forward[x][3] , vel);
delay(delay_Forward);
}
}
void Backward()
{
for(int y=0; y<num2; y++) {
RU.slowmove (array_cal[0] + array_back[y][0] , vel_Back);
RL.slowmove (array_cal[1] + array_back[y][1] , vel_Back);
LU.slowmove (array_cal[2] + array_back[y][2] , vel_Back);
LL.slowmove (array_cal[3] + array_back[y][3] , vel_Back);
delay(delay_Back);
}
}
void left_turn()
{
for(int y=0; y<num3; y++) // When turning, the angle and speed of the 4 servos with every step.
{
RU.slowmove (array_cal[0] + array_left[y][0] , vel); // Turn to go, define the calibrated angle of the upper-right servo and the angle as well as the speed when going ahead.
RL.slowmove (array_cal[1] + array_left[y][1] , vel);
LU.slowmove (array_cal[2] + array_left[y][2] , vel);
LL.slowmove (array_cal[3] + array_left[y][3] , vel);
delay(delay_tim); // Delay delay_tim
}
}
void right_turn()
{
for(int y=0; y<num4; y++) // When turning, the angle and speed of the 4 servos with every step.
{
RU.slowmove (array_cal[0] + array_right[y][0] , vel); // Turn to go, define the calibrated angle of the upper-right servo and the angle as well as the speed when going ahead.
RL.slowmove (array_cal[1] + array_right[y][1] , vel);
LU.slowmove (array_cal[2] + array_right[y][2] , vel);
LL.slowmove (array_cal[3] + array_right[y][3] , vel);
delay(delay_tim); // Delay delay_tim
}
}
void setup()
{
#ifdef INSTALL
Servo_Init();
RU.slowmove (90 , vel);
RL.slowmove (90 , vel);
LU.slowmove (90 , vel);
LL.slowmove (90 , vel);
while(1);
#endif
#ifdef CALIBRATION
Servo_Init();
RU.slowmove (array_cal[0] , vel); // Define the angle and speed of the upper-right servo.
RL.slowmove (array_cal[1] , vel);
LU.slowmove (array_cal[2] , vel);
LL.slowmove (array_cal[3] , vel);
delay(2000);
while(1);
#endif
#ifdef RUN
Servo_Init();
RU.slowmove (array_cal[0] , vel);
RL.slowmove (array_cal[1] , vel);
LU.slowmove (array_cal[2] , vel);
LL.slowmove (array_cal[3] , vel);
delay(2000);
#endif
pinMode(Echo, INPUT); // Define ultrasonic input feet
pinMode(Trig, OUTPUT); // Define the ultrasonic output pin
pinMode(beep,OUTPUT); //buzzer
}
void loop()
{
TooClose();
if(Distance>30)
{
Forward();
}
else
{
digitalWrite(beep,HIGH); //Buzzer
for(int z=0; z<4; z++) //Turn right to turn 4, and adjust your steering value even more.
{
right_turn();
digitalWrite(beep,LOW);
}
}
}
โปรแกรม Arduino ควบคุมหุ่นยนต์เดินด้วยเสียง โดยใช้คอนเดนเซอร์ไมโครโฟน
// Sound
#include "VarSpeedServo.h" // VarSpeedServo library
//==============================
VarSpeedServo RU; //Right Upper
VarSpeedServo RL; //Right Lower
VarSpeedServo LU; //Left Upper
VarSpeedServo LL; //Left Lower
int beep=A3;//Define the buzzer Digital A3 interface
int Echo = A1; // Echo (P2.0)
int Trig = A0; // Trig (P2.1)
int Distance = 0;
int sensorValue = 0;
//vel(min), delay_Forward(max) = (5, 2000)
const int vel = 50, vel_Back = 15, vel_turn= 15; //vel(mid), delay_Forward(mid) = (20, 750)
const int delay_Forward = 700, delay_Back = 750, delay_turn = 500; //vel(max), delay_Forward(min)= (256, 50)
//wonderful ---> (10, 700) (50, 500) (100, 100) (100, 300) (100, 500)
const int array_cal[4] = {90,90,90,90}; // Define the angular adjustment of servo (RU, RL, LU, LL )
const int delay_tim = 300; //Delay 750ms
int RU_Degree = 0, LU_Degree = array_cal[2] + 5;
const int num1 = 6;
const int array_forward[num1][4] =
{
{0,-40,0,-20},
{30,-40,40,-20},
{30,0,40,0},
{0,20,0,40},
{-30,20,-30,40},
{-30,0,-30,0},
};
const int num2 = 6;
const int array_back[num2][4] ={
{-40,0,-20,0},
{-40,30,-20,30},
{0,30,0,30},
{40,0,50,0},
{40,-30,50,-40},
{0,-30,0,-40},
};
const int num3 = 5; // Define the array number 5
/* When I encountered obstacles, turned around. Each set requires 5 sets of data. Angle change of each group of 4 servo systems*/
const int array_left[num3][4] =
{
{-40,0,-20,0}, // The left foot turns left at 20 degrees and the right foot turns left at 40 degrees.
{-40,30,-20,30}, // The left foot raises and the right foot supports the ground to keep balance
{0,30,0,30},
{30,0,30,0},
{0,0,0,0},
}; //Define 5 arrays, each containing 4 elements to store the current angular value of the servo。
const int num4 = 5;
const int array_right[num4][4] =
{
{40,0,50,0},
{40,-30,50,-40},
{0,-30,0,-40},
{-30,0,-40,0},
{0,0,0,0},
};
#define RUN
void Servo_Init()
{
RU.attach(3); // Connect the signal wire of the upper-right servo to pin 9
RL.attach(5); // Connect the signal wire of the lower-right servo to pin 10
LU.attach(6); // Connect the signal wire of the upper-left servo to pin 11
LL.attach(9); // Connect the signal wire of the lower-left servo to pin 12
}
void Adjust() // Avoid the servo's fast spinning in initialization
{ // RU,LU goes from array_cal[0] - 5 ,array_cal[2] + 5 degrees to array_cal[0],array_cal[2] degrees
for(RU_Degree = array_cal[0] - 5; RU_Degree <= array_cal[0]; RU_Degree += 1) {
RU.write(RU_Degree); // in steps of 1 degree
LU.write(LU_Degree--); // tell servo to go to RU_Degreeition, LU_Degreeition in variable 'RU_Degree', 'LU_Degree'
delay(15); // waits 15ms for the servo to reach the RU_Degreeition
}
}
void Forward(unsigned int n_delay)
{
for(int x=0; x<num1; x++) {
RU.slowmove (array_cal[0] + array_forward[x][0] , vel);
RL.slowmove (array_cal[1] + array_forward[x][1] , vel);
LU.slowmove (array_cal[2] + array_forward[x][2] , vel);
LL.slowmove (array_cal[3] + array_forward[x][3] , vel);
delay(n_delay); //delay_Forward :700
}
}
void stop()
{
RU.slowmove (array_cal[0] , vel);
RL.slowmove (array_cal[1] , vel);
LU.slowmove (array_cal[2] , vel);
LL.slowmove (array_cal[3] , vel);
delay(delay_tim);
}
void Backward(unsigned int n_delay)
{
for(int y=0; y<num2; y++) {
RU.slowmove (array_cal[0] + array_back[y][0] , vel_Back);
RL.slowmove (array_cal[1] + array_back[y][1] , vel_Back);
LU.slowmove (array_cal[2] + array_back[y][2] , vel_Back);
LL.slowmove (array_cal[3] + array_back[y][3] , vel_Back);
delay(n_delay); //delay_Forward:700
}
}
void left_turn(unsigned int n_delay)
{
for(int z=0; z<2; z++)
{
for(int y=0; y<num3; y++)
{
RU.slowmove (array_cal[0] + array_left[y][0] , vel_Back);
RL.slowmove (array_cal[1] + array_left[y][1] , vel_Back);
LU.slowmove (array_cal[2] + array_left[y][2] , vel_Back);
LL.slowmove (array_cal[3] + array_left[y][3] , vel_Back);
delay(n_delay); //delay_Back:750
}
}
}
void right_turn(unsigned int n_delay)
{
for(int z=0; z<2; z++)
{
for(int y=0; y<num4; y++)
{
RU.slowmove (array_cal[0] + array_right[y][0] , vel_Back);
RL.slowmove (array_cal[1] + array_right[y][1] , vel_Back);
LU.slowmove (array_cal[2] + array_right[y][2] , vel_Back);
LL.slowmove (array_cal[3] + array_right[y][3] , vel_Back);
delay(n_delay); //delay_Back:750
}
}
}
void n_Forward(unsigned char n_step,unsigned int n_delay)
{
unsigned char z;
for(z = 0;z < n_step; z++)
{
Forward(n_delay);
}
}
void n_Backward(unsigned char n_step,unsigned int n_delay)
{
unsigned char z;
for(z = 0;z < n_step; z++)
{
Backward(n_delay);
}
}
void n_left_turn(unsigned char n_step,unsigned int n_delay)
{
unsigned char z;
for(z = 0;z < n_step; z++)
{
left_turn(n_delay);
}
}
void n_right_turn(unsigned char n_step,unsigned int n_delay)
{
unsigned char z;
for(z = 0;z < n_step; z++)
{
right_turn(n_delay);
}
}
void setup()
{
Servo_Init();
RU.slowmove (array_cal[0] , vel);
RL.slowmove (array_cal[1] , vel);
LU.slowmove (array_cal[2] , vel);
LL.slowmove (array_cal[3] , vel);
delay(2000);
Serial.begin(9600);
}
void loop()
{
sensorValue = analogRead(A6);
if (sensorValue > 20 )//buttonCH
{
n_Forward(4,250); //go ahead
delay(2000);
}
}
โปรแกรม Arduino ควบคุมหุ่นยนต์เดินด้วยแสง โดยใช้เซนเซอร์ แอลดีอาร์
// light
#include "VarSpeedServo.h" // VarSpeedServo library
//==============================
VarSpeedServo RU; //Right Upper
VarSpeedServo RL; //Right Lower
VarSpeedServo LU; //Left Upper
VarSpeedServo LL; //Left Lower
int beep=A3;//Define the buzzer Digital A3 interface
int sensorValue = 0;
//vel(min), delay_Forward(max) = (5, 2000)
const int vel = 50, vel_Back = 15, vel_turn= 15; //vel(mid), delay_Forward(mid) = (20, 750)
const int delay_Forward = 700, delay_Back = 750, delay_turn = 500; //vel(max), delay_Forward(min)= (256, 50)
//wonderful ---> (10, 700) (50, 500) (100, 100) (100, 300) (100, 500)
const int array_cal[4] = {90,90,90,90}; // Define the angular adjustment of servo (RU, RL, LU, LL )
const int delay_tim = 300; //Delay 750ms
int RU_Degree = 0, LU_Degree = array_cal[2] + 5;
const int num1 = 6;
const int array_forward[num1][4] =
{
{0,-40,0,-20},
{30,-40,40,-20},
{30,0,40,0},
{0,20,0,40},
{-30,20,-30,40},
{-30,0,-30,0},
};
const int num2 = 6;
const int array_back[num2][4] ={
{-40,0,-20,0},
{-40,30,-20,30},
{0,30,0,30},
{40,0,50,0},
{40,-30,50,-40},
{0,-30,0,-40},
};
const int num3 = 5; // Define the array number 5
/* When I encountered obstacles, turned around. Each set requires 5 sets of data. Angle change of each group of 4 servo systems*/
const int array_left[num3][4] =
{
{-40,0,-20,0}, // The left foot turns left at 20 degrees and the right foot turns left at 40 degrees.
{-40,30,-20,30}, // The left foot raises and the right foot supports the ground to keep balance
{0,30,0,30},
{30,0,30,0},
{0,0,0,0},
}; //Define 5 arrays, each containing 4 elements to store the current angular value of the servo。
const int num4 = 5;
const int array_right[num4][4] =
{
{40,0,50,0},
{40,-30,50,-40},
{0,-30,0,-40},
{-30,0,-40,0},
{0,0,0,0},
};
#define RUN
void Servo_Init()
{
RU.attach(3); // Connect the signal wire of the upper-right servo to pin 9
RL.attach(5); // Connect the signal wire of the lower-right servo to pin 10
LU.attach(6); // Connect the signal wire of the upper-left servo to pin 11
LL.attach(9); // Connect the signal wire of the lower-left servo to pin 12
}
void Adjust() // Avoid the servo's fast spinning in initialization
{ // RU,LU goes from array_cal[0] - 5 ,array_cal[2] + 5 degrees to array_cal[0],array_cal[2] degrees
for(RU_Degree = array_cal[0] - 5; RU_Degree <= array_cal[0]; RU_Degree += 1) {
RU.write(RU_Degree); // in steps of 1 degree
LU.write(LU_Degree--); // tell servo to go to RU_Degreeition, LU_Degreeition in variable 'RU_Degree', 'LU_Degree'
delay(15); // waits 15ms for the servo to reach the RU_Degreeition
}
}
void Forward(unsigned int n_delay)
{
for(int x=0; x<num1; x++) {
RU.slowmove (array_cal[0] + array_forward[x][0] , vel);
RL.slowmove (array_cal[1] + array_forward[x][1] , vel);
LU.slowmove (array_cal[2] + array_forward[x][2] , vel);
LL.slowmove (array_cal[3] + array_forward[x][3] , vel);
delay(n_delay); //delay_Forward :700
}
}
void stop()
{
RU.slowmove (array_cal[0] , vel);
RL.slowmove (array_cal[1] , vel);
LU.slowmove (array_cal[2] , vel);
LL.slowmove (array_cal[3] , vel);
delay(delay_tim);
}
void Backward(unsigned int n_delay)
{
for(int y=0; y<num2; y++) {
RU.slowmove (array_cal[0] + array_back[y][0] , vel_Back);
RL.slowmove (array_cal[1] + array_back[y][1] , vel_Back);
LU.slowmove (array_cal[2] + array_back[y][2] , vel_Back);
LL.slowmove (array_cal[3] + array_back[y][3] , vel_Back);
delay(n_delay); //delay_Forward:700
}
}
void left_turn(unsigned int n_delay)
{
for(int z=0; z<2; z++)
{
for(int y=0; y<num3; y++)
{
RU.slowmove (array_cal[0] + array_left[y][0] , vel_Back);
RL.slowmove (array_cal[1] + array_left[y][1] , vel_Back);
LU.slowmove (array_cal[2] + array_left[y][2] , vel_Back);
LL.slowmove (array_cal[3] + array_left[y][3] , vel_Back);
delay(n_delay); //delay_Back:750
}
}
}
void right_turn(unsigned int n_delay)
{
for(int z=0; z<2; z++)
{
for(int y=0; y<num4; y++)
{
RU.slowmove (array_cal[0] + array_right[y][0] , vel_Back);
RL.slowmove (array_cal[1] + array_right[y][1] , vel_Back);
LU.slowmove (array_cal[2] + array_right[y][2] , vel_Back);
LL.slowmove (array_cal[3] + array_right[y][3] , vel_Back);
delay(n_delay); //delay_Back:750
}
}
}
void n_Forward(unsigned char n_step,unsigned int n_delay)
{
unsigned char z;
for(z = 0;z < n_step; z++)
{
Forward(n_delay);
}
}
void n_Backward(unsigned char n_step,unsigned int n_delay)
{
unsigned char z;
for(z = 0;z < n_step; z++)
{
Backward(n_delay);
}
}
void n_left_turn(unsigned char n_step,unsigned int n_delay)
{
unsigned char z;
for(z = 0;z < n_step; z++)
{
left_turn(n_delay);
}
}
void n_right_turn(unsigned char n_step,unsigned int n_delay)
{
unsigned char z;
for(z = 0;z < n_step; z++)
{
right_turn(n_delay);
}
}
void setup()
{
Servo_Init();
RU.slowmove (array_cal[0] , vel);
RL.slowmove (array_cal[1] , vel);
LU.slowmove (array_cal[2] , vel);
LL.slowmove (array_cal[3] , vel);
delay(2000);
Serial.begin(9600);
}
void loop()
{
sensorValue = analogRead(A5);
if (sensorValue > 900 )//ค่าเซนเซอร์มากกว่า 900 ให้เดินหน้า
{
n_Forward(4,250); //go ahead
delay(2000);
}
}
โปรแกรม Arduino ควบคุมหุ่นยนต์เดินด้วย อัลตร้าโซนิคเซ็นเซอร์ Ultrasonic sensor
int Trig = A0; // Trig (P2.1)
int Echo = A1; // Echo (P2.0)
//4. Biped robot ultrasonic obstacle avoidance
#include "VarSpeedServo.h" // VarSpeedServo library
VarSpeedServo RU; //Right Upper
VarSpeedServo RL; //Right Lower
VarSpeedServo LU; //Left Upper
VarSpeedServo LL; //Left Lower
int beep=A3;//Define the buzzer Digital A3 interface
int Trig = A0; // Trig (P2.1)
int Echo = A1; // Echo (P2.0)
int Distance = 0;
//vel(min), delay_Forward(max) = (5, 2000)
const int vel = 20, vel_Back = 10; //vel(mid), delay_Forward(mid) = (20, 750)
const int delay_Forward = 750, delay_Back = 1000; //vel(max), delay_Forward(min)= (256, 50)
//wonderful ---> (10, 700) (50, 500) (100, 100) (100, 300) (100, 500)
const int array_cal[4] = {90,90,90,90}; // Define the angular adjustment of servo (RU, RL, LU, LL )
const int delay_tim = 300; //Delay 750ms
int RU_Degree = 0, LU_Degree = array_cal[2] + 5;
const int num1 = 6;
const int array_forward[num1][4] =
{
{0,-40,0,-20},
{30,-40,30,-20},
{30,0,30,0},
{0,20,0,40},
{-30,20,-30,40},
{-30,0,-30,0},
};
const int num2 = 6;
const int array_back[num2][4] ={
{-40,0,-20,0},
{-40,30,-20,30},
{0,30,0,30},
{40,0,20,0},
{40,-30,20,-30},
{0,-30,0,-30},
};
const int num3 = 5; // Define the array number to be 5.
const int array_left[num3][4] =
{
{-40,0,-20,0}, // The left foot turns left at 20 degrees and the right foot turns left at 40 degrees.
{-40,30,-20,30}, // The left foot raises and the right foot supports the ground to keep balance
{0,30,0,30},
{30,0,30,0},
{0,0,0,0},
};
const int num4 = 5;
const int array_right[num4][4] =
{
{40,0,20,0},
{40,-30,20,-20},
{0,-30,0,-20},
{-30,0,-30,0},
{0,0,0,0},
};
//#define INSTALL
//#define CALIBRATION
#define RUN
void Servo_Init()
{
RU.attach(3); // Connect the signal wire of the upper-right servo to pin 9
RL.attach(5); // Connect the signal wire of the lower-right servo to pin 10
LU.attach(6); // Connect the signal wire of the upper-left servo to pin 11
LL.attach(9); // Connect the signal wire of the lower-left servo to pin 12
}
void Adjust() // Avoid the servo's fast spinning in initialization
{ // RU,LU goes from array_cal[0] - 5 ,array_cal[2] + 5 degrees to array_cal[0],array_cal[2] degrees
for(RU_Degree = array_cal[0] - 5; RU_Degree <= array_cal[0]; RU_Degree += 1) {
RU.write(RU_Degree); // in steps of 1 degree
LU.write(LU_Degree--); // tell servo to go to RU_Degreeition, LU_Degreeition in variable 'RU_Degree', 'LU_Degree'
delay(15); // waits 15ms for the servo to reach the RU_Degreeition
}
}
void TooClose()
{
digitalWrite(Trig, LOW);
delayMicroseconds(2);
digitalWrite(Trig, HIGH);
delayMicroseconds(10);
digitalWrite(Trig, LOW);
float Fdistance = pulseIn(Echo, HIGH);
Fdistance= Fdistance/58;
Serial.print("Distance:");
Serial.println(Fdistance);
Distance = Fdistance;
}
void Forward()
{
for(int x=0; x<num1; x++) {
RU.slowmove (array_cal[0] + array_forward[x][0] , vel);
RL.slowmove (array_cal[1] + array_forward[x][1] , vel);
LU.slowmove (array_cal[2] + array_forward[x][2] , vel);
LL.slowmove (array_cal[3] + array_forward[x][3] , vel);
delay(delay_Forward);
}
}
void Backward()
{
for(int y=0; y<num2; y++) {
RU.slowmove (array_cal[0] + array_back[y][0] , vel_Back);
RL.slowmove (array_cal[1] + array_back[y][1] , vel_Back);
LU.slowmove (array_cal[2] + array_back[y][2] , vel_Back);
LL.slowmove (array_cal[3] + array_back[y][3] , vel_Back);
delay(delay_Back);
}
}
void left_turn()
{
for(int y=0; y<num3; y++) // When turning, the angle and speed of the 4 servos with every step.
{
RU.slowmove (array_cal[0] + array_left[y][0] , vel); // Turn to go, define the calibrated angle of the upper-right servo and the angle as well as the speed when going ahead.
RL.slowmove (array_cal[1] + array_left[y][1] , vel);
LU.slowmove (array_cal[2] + array_left[y][2] , vel);
LL.slowmove (array_cal[3] + array_left[y][3] , vel);
delay(delay_tim); // Delay delay_tim
}
}
void right_turn()
{
for(int y=0; y<num4; y++) // When turning, the angle and speed of the 4 servos with every step.
{
RU.slowmove (array_cal[0] + array_right[y][0] , vel); // Turn to go, define the calibrated angle of the upper-right servo and the angle as well as the speed when going ahead.
RL.slowmove (array_cal[1] + array_right[y][1] , vel);
LU.slowmove (array_cal[2] + array_right[y][2] , vel);
LL.slowmove (array_cal[3] + array_right[y][3] , vel);
delay(delay_tim); // Delay delay_tim
}
}
void setup()
{
#ifdef INSTALL
Servo_Init();
RU.slowmove (90 , vel);
RL.slowmove (90 , vel);
LU.slowmove (90 , vel);
LL.slowmove (90 , vel);
while(1);
#endif
#ifdef CALIBRATION
Servo_Init();
RU.slowmove (array_cal[0] , vel); // Define the angle and speed of the upper-right servo.
RL.slowmove (array_cal[1] , vel);
LU.slowmove (array_cal[2] , vel);
LL.slowmove (array_cal[3] , vel);
delay(2000);
while(1);
#endif
#ifdef RUN
Servo_Init();
RU.slowmove (array_cal[0] , vel);
RL.slowmove (array_cal[1] , vel);
LU.slowmove (array_cal[2] , vel);
LL.slowmove (array_cal[3] , vel);
delay(2000);
#endif
pinMode(Echo, INPUT);
pinMode(Trig, OUTPUT);
pinMode(beep,OUTPUT);
}
void loop()
{
TooClose();
if(Distance>30)
{
Forward();
}
else
{
digitalWrite(beep,HIGH);
for(int z=0; z<4; z++)
{
right_turn();
digitalWrite(beep,LOW);
}
}
}
เริ่มต้นใช้งาน Mixly กับ Robot Biped หุ่นยนต์เดิน 2ขา ด้วย Arduino Nano
รีวิว
ยังไม่มีบทวิจารณ์