Part2.4 機器人校正

#include <SoftwareSerial.h>

#include <Servo.h>

Servo servoA;

SoftwareSerial BT(15,14); // TX, RX

int y,z ;

int S0,S1,S2,S3,S4,S5;

int num[3];

void setup() {

Serial.begin(9600);

BT.begin(38400);//bluetooth baud rate

BT.setTimeout(10);

}

void loop() {

bt();

servoA.attach(S0);

servoA.write(S1);

}

void bt(){

if(BT.available()>0){

int x = BT.read();

if(y==97){

num[1]=x;

S0=num[1];

Serial.print("A0=");

Serial.println(num[1]);

y=0;

x=0;

}

if(y==98){

num[1]=x;

S1=num[1];

Serial.print("A1=");

Serial.println(num[1]);

y=0;

x=0;

}

y=x;

}

}