แจกโค้ดเพื่อการศึกษา หุ่นยนต์ สพฐ. ระดับกลาง (Arduino C/C++)

ตัวอย่างโค้ดโปรแกรมภาษาซี C/C++ Arduino สำหรับการแข่งขันหุ่นยนต์ สพฐ. ระดับกลาง ทั้งประถมและมัธยม

ศึกษาแนวทางเบื้องต้น โครงสร้างโปรแกรม ฟังก์ชั่นพื้นฐานในการการสั่งงานหุ่นยนต์ให้ปฏิบัติภารกิจตามโจทย์ที่ได้รับ

สามารถนำหลักการไปพัฒนา/แก้ไข เป็นภาษาอื่น ๆที่ถนัด เพื่อประยุกต์ใช้กับหุ่นยนต์หรือระบบอัตโนมัติต่าง ๆได้หลากหลายตามต้องการครับ

int sValue=350,ball=0,green=0;
int power=100,delay=200;

bool W(int s){ return (analog(s)>sValue) ? true : false; }
bool B(int s){ return (analog(s)<svalue) ? true;  false; }

void trackline(){
    while(true){
        if(analog(8)>0&&ball==0) {keep_up();ball=1;}
        if(B(2)) {ao();sleep(delay);break;}
        else if(W(0)&&W(4)) fd(power);
        else if(B(0)&&W(4)) sr(power);
        else if(W(0)&&B(4)) sl(power);
    }
}

void turn(char t,int setbody){
    if(t=='L') sl(power); else sr(power);
    if(t=='U') sleep(1400); else sleep(700);    
    ao();
    if(setbody==1){
        while(true){
            bk(power);
            if(B(5)){
                ao();
                while(!B(7)) motor(2,-power);
                ao();sleep(delay);break;
            }
            else if(B(7)){
                ao();
                while(!B(5)) motor(1,-power);
                ao();sleep(delay);break;
            }
        }
    }    
}
void moveblock(char d,int ms){
    if(d=='F') fd(100); else bk(100);
    sleep(ms);ao();sleep(10);
}

void setup()  
{
    lcd("NPS ROBOT RAYONG"); 
}
void loop()
{
    switch(green){
      case 0:
        moveblock('F',800);trackline();
        turn('R',1);trackline();
        turn('R',1);trackline();
        turn('R',1);trackline();
        turn('L',1);trackline();
        keep_down();ball=0;
        green++;break;
      case 1:
        turn('U',0);
        moveblock('F',800);
        turn('R',1);
        moveblock('F',2600);
        turn('R',0);trackline();
        turn('R',1);trackline();
        turn('U',1);trackline();
        turn('L',1);
        moveblock('F',1500);
        turn('R',0);trackline();
        turn('R',0);trackline();
        turn('R',1);trackline();
        keep_down();ball=0;   
        green++;break;
      case 2:
        turn('U',0);trackline();
        turn('L',1);trackline();
        turn('L',1);trackline();
        turn('U',1);trackline();
        turn('R',1);
        moveblock('F',1500);
        turn('L',1);trackline();
        turn('L',0);trackline();
        keep_down();ball=0;   
        green++;break;
      case 3:
        turn('U',0);trackline();
        keep_down();ball=0;   
        green++;break;                
      case 4:
        turn('U',0);
        moveblock('F',2000);
        turn('L',1);trackline();
        turn('L',0);
        moveblock('F',1300);
        turn('R',1);trackline();
        moveblock('F',1500);trackline();        
        turn('R',0);trackline();
        turn('R',1);trackline();
        turn('L',1);trackline();
        turn('L',1);trackline();
        turn('L',1);trackline();
        moveblock('F',1000);        
        lcd("SUCCESS!!");
        green++;break;                
    default: 
        ao();break;
    }
}

ตัวอย่างการทำงานของโปรแกรม