#include <SoftwareSerial.h>
#include <Servo.h>
Servo S1;
Servo S2;
int pos = 90;
int i;
SoftwareSerial BTSerial(8, 9); // RX | TX
int STBY = 2; //standby
/*Motor*/
int PWMA = 10; //Speed control
int AIN1 = 11; //Direction0
int AIN2 = 12; //Direction
int val = 0;
void setup()
{
pinMode(STBY, OUTPUT);
pinMode(PWMA, OUTPUT);
pinMode(AIN1, OUTPUT);
pinMode(AIN2, OUTPUT);
Serial.begin(9600);
BTSerial.begin(38400);
BTSerial.println("Dog2 is ready");
S1.attach(5);
S2.attach(6);
}
void move(int motor, int speed, int direction) {
digitalWrite(STBY, HIGH);
boolean inPin1 = LOW;
boolean inPin2 = HIGH;
if (direction == 1) {
inPin1 = HIGH;
inPin2 = LOW;
}
if (motor == 1) {
digitalWrite(AIN1, inPin1);
digitalWrite(AIN2, inPin2);
analogWrite(PWMA, speed);
}
}
void SerCatch()
{
for (pos = 90; pos >= 43 ; pos--) {
// in steps of 1 degree
S1.write(pos);
S2.write(180 - pos);
delay(15);
}
}
void Serrelease()
{
for (pos = 45; pos <= 135; pos++) {
// in steps of 1 degree
S1.write(pos);
S2.write(180 - pos);
delay(15);
}
}
void loop()
{
if (BTSerial.available()) {
val = BTSerial.read();
switch (val) {
case 'a' : {
move(1, 255, 1);
BTSerial.write("Dog2 is lifting");
} break;
case 'b' : {
move(1, 255, 2);
BTSerial.write("Dog2 is lowering");
} break;
case 'c' : {
move(1, 0, 2);
BTSerial.write(" Dog2 stops");
} break;
case 'd' : {
SerCatch();
BTSerial.write("Dog2 up");
} break;
case 'e' : {
Serrelease();
BTSerial.write("Dog2 down");
} break;
default: BTSerial.write(val); break;
}
}
}
——2018.9.8 于寝室327