Page 54 - MDP2020-1
P. 54
| 인천전자마이스터고등학교 ·············································································································
48
int joint3[180];
int empty[180];
int top = 179;
// status
boolean playmode = false, Step = false, isBluetoothmode = false;
void setup()
{
pinMode(4, INPUT);
pinMode(13, OUTPUT);
digitalWrite(13, HIGH);
servo_0.attach(3);
servo_1.attach(5);
servo_2.attach(10);
servo_3.attach(11);
Serial.begin(38400);
Bluetooth.begin(38400);
Bluetooth.setTimeout(1);
Serial.println("mini robot ready...");
digitalWrite(13, LOW);
servo0Ppos = 90; servo1Ppos = 90; servo2Ppos = 90; servo3Ppos = 90;
}
void loop()
{
currentMillis = millis();
currentMicros = micros();
if(isBluetoothmode) {
if(Bluetooth.available()) {
dataIn = Bluetooth.readString();
if(dataIn.startsWith("s1")) {
String dataInS = dataIn.substring(2, dataIn.length());
servo0Pos = dataInS.toInt();
if(servo0Ppos > servo0Pos) {
for(int j = servo0Ppos; j >= servo0Pos; j--) {
servo_0.writeMicroseconds(j);
}
}
if(servo0Ppos < servo0Pos) {
