續上篇
程式的部分，
#include NXShield.h 呼叫NXShield.h
#include Wire.h 呼叫Wire.h
#include Servo.h 呼叫Servo.h
#include PSPNx.h 呼叫PSPNx.h
NXShield nxshield; 將NXShield 的名稱換成nxshield 方便後面程式撰寫
PSPNx psp; 同上述，將PSPNx換成psp
Servo servo_r_arm; 定義右邊上方馬達
Servo servo_r_shoulder; 定義右邊下方馬達
Servo servo_l_arm; 定義左邊上方馬達
Servo servo_l_shoulder; 定義左邊下方馬達
int r_arm=100,r_shoulder=110,l_arm=63,l_shoulder=100; 控制馬達的變數
void setup(){ 初始化設定
Serial.begin(115200); 設定Baurand
delay(100); 延遲100ms讓arduino緩衝一下
nxshield.init( SH_HardwareI2C );
psp.init( &nxshield, SH_BBS2);
Serial.println("Press GO buttonto coutine");
nxshield.waitForButtonPress(BTN_GO); 按下NXShield下"go"按鈕執行後面的程式
servo_r_arm.attach(2); 將右上馬達定義在PIN2腳位
servo_r_arm.write(100);
servo_r_shoulder.attach(3); 將右下馬達定義在PIN3腳位
servo_r_shoulder.write(110);
servo_l_arm.attach(4); 將左上馬達定義在PIN4腳位
servo_l_arm.write(63);
servo_l_shoulder.attach(5); 將左上馬達定義在PIN5腳位
servo_l_shoulder.write(100);
}
