[自製初音機器人]頭部CODE

PSPNx psp;

Servo servo_h_nod;
Servo servo_h_swing;
int h_nod=120;//Control servo's position
int changemod=0,change_a=100;

void setup(){
 
  Serial.begin(115200);
  delay(100);
 
  nxshield.init( SH_HardwareI2C );

  psp.init( &nxshield, SH_BBS2);

  Serial.println("Press GO buttonto coutine");
  nxshield.waitForButtonPress(BTN_GO);

    
  servo_h_nod.attach(6);
  servo_h_nod.write(100);
 
  servo_h_swing.attach(7);
  servo_h_swing.write(120);
}

////////////////////////////Main///////////////////////////////////////////////
void loop(){
 
  char str[50];
 
   int8_t yl, xl, yr, xr, button_set1[8], button_set2[8];

  psp.getButtons(button_set1,button_set2);
  
   else if(button_set1[BS1_Button_Start]==0){
     
     Nod_head();
   
    }
    else if(button_set1[BS1_Button_Select]==0){
      changemod=1;
       delay(10);
      while(changemod){
      
        if(button_set1[BS1_Button_L3]==0){
          changemod=0;
           delay(10);
        }
        else{
          shake_hand();
           Serial.print("  changemod: ");
            Serial.println(changemod);
        }
      }
     
    }
     else if(button_set1[BS1_Button_Up]==0){
      if(h_nod>270){}
      else{
          h_nod=h_nod+1;
          servo_h_nod.write(h_nod);
          delay(5);
      }
    } 
 
    else if(button_set1[BS1_Button_Down]==0){
       if(h_nod<0){}
       else{
         h_nod=h_nod-1;
          servo_h_nod.write(h_nod);
          delay(5);
       }
    }
      
 
}

//////////////////////////////Subroutine//////////////////////////////////////////
void Swing_head(){
  int a = 120,b = 0;
  while(b<3){
    while(a<=270){
      servo_h_swing.write(a);
      delay(5);
      a+=1;
    }
    while(a>=20){
      servo_h_swing.write(a);
      delay(5);
      a-=1;
    }
    b+=1;
  }
}

void Nod_head(){
 int c = 100,d = 0;
  while(d<3){
    while(c<=200){
      servo_h_nod.write(c);
      delay(5);
      c+=1;
    }
    while(c>=40){
      servo_h_nod.write(c);
      delay(5);
      c-=1;
    }
    d+=1;
  }

}

發佈留言

發佈留言必須填寫的電子郵件地址不會公開。 必填欄位標示為 *