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;
}
}