ลดราคา!
RBBP10 Robot Biped หุ่นยนต์เดิน 2ขา ด้วย Arduino Nano

RBBP10 Robot Biped หุ่นยนต์เดิน 2ขา ด้วย Arduino Nano

Original price was: ฿2,000.00.Current price is: ฿1,850.00.

สินค้าหมดแล้ว

รหัสสินค้า: RBBP10 หมวดหมู่: , ป้ายกำกับ: , , , , ,

คำอธิบาย

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)

มีอะไรเตรียมไว้ให้ในชุด

  1. Biped Robot หุ่นยนต์เดิน 2 ขา ประกอบแล้ว (ไม่รวมแบตเตอรี่)
  2. Arduino Nano Compatible
  3. Robot Shield
  4. Sensor อัลตร้าโซนิค
  5. IR รีโมทคอนโทรล และ ตัวรับ IR
  6. สายโปรแกรม 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

 

  1. โปรแกรมการเซ็ตซีโร่เซอร์โว
  2. โปรแกรมตัวอย่างเดินหน้าถอยหลังเลี้ยวซ้ายเลี้ยว
  3. โปรแกรมหุ่นยนต์แดนซ์
  4. โปรแกรมหุ่นยนต์เดินหลบหลีกสิ่งกีดขวางโดยใช้เซ็นเซอร์อัลตร้าโซนิค
  5. โปรแกรมควบคุมหุ่นยนต์เดินโดยใช้รีโมทคอนโทรล
  6. โปรแกรมควบคุมหุ่นยนต์เดินโดยใช้คอนเดนเซอร์ไมโครโฟน
  7. โปรแกรมควบคุมหุ่นยนต์เดินโดยใช้ 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

ข้อมูลเพิ่มเติม

น้ำหนัก 500 กรัม
ขนาด 20 × 20 × 20 เซนติเมตร

รีวิว

ยังไม่มีบทวิจารณ์

เฉพาะลูกค้าที่เข้าสู่ระบบ และเคยซื้อสินค้าชิ้นนี้แล้วเท่านั้น ที่เขียนบทวิจารณ์ได้

สินค้าที่เกี่ยวข้อง