Paso 7: código de
Luego combinar el código en la 2 º prueba con servos y activar que los servos se mueven en rangos designados.
#include <SoftwareSerial.h>#include <Servo.h>SoftwareSerial BT(52, 53); //set TX and RX on bluetooth to pin 52 and 53 respectivelyServo servo1;Servo servo2;Servo servo3;char command;int motorPin = 8; //right side to IB - forwardint motorPin2 = 9; //left side to IA - forwardint motorPin3 = 10; //right side to IA - backwardint motorPin4 = 11; //left side to IB - backwardint pos1 = 90;int pos2 = 90;int pos3 = 90;void setup() { BT.begin (9600); Serial.begin (9600); pinMode(motorPin, OUTPUT); pinMode(motorPin2, OUTPUT); pinMode(motorPin3, OUTPUT); pinMode(motorPin4, OUTPUT); servo1.attach(5); servo1.write(pos1); servo2.attach(6); servo2.write(pos2); servo3.attach(7); servo3.write(pos3);}void stop() { digitalWrite(motorPin, LOW); digitalWrite(motorPin2, LOW); digitalWrite(motorPin3, LOW); digitalWrite(motorPin4, LOW);}void forward(){ digitalWrite(motorPin, HIGH); digitalWrite(motorPin2, HIGH); digitalWrite(motorPin3, LOW); digitalWrite(motorPin4, LOW); } void backward() { digitalWrite(motorPin, LOW); digitalWrite(motorPin2, LOW); digitalWrite(motorPin3, HIGH); digitalWrite(motorPin4, HIGH); } void turnLeft() { digitalWrite(motorPin, HIGH); digitalWrite(motorPin2, LOW); digitalWrite(motorPin3, LOW); digitalWrite(motorPin4, HIGH); } void turnRight() { digitalWrite(motorPin, LOW); digitalWrite(motorPin2, HIGH); digitalWrite(motorPin3, HIGH); digitalWrite(motorPin4, LOW); }void open() { for(pos3 = 110; pos3 > 40; pos3 -= 1) {servo3.write(pos3); }}void close() { for(pos3 = 40; pos3 < 110; pos3 += 1) {servo3.write(pos3); }}void antiClockwise() { for(pos2 = 90; pos2 > 0; pos2 -= 1) {servo2.write(pos2); }}void clockwise() { for(pos2 = 0; pos2 < 90; pos2 += 1) {servo2.write(pos2); }}void up () { for(pos1 = 50; pos1 < 180; pos1 +=1) {servo1.write(pos1); }}void down() { for(pos1 = 180; pos1 > 50; pos1 -= 1) {servo1.write(pos1); }} void loop() { if (BT.available() > 0) { command = BT.read(); switch (command) { case 'w': forward(); break; case 'x': backward(); break; case 'a': turnLeft(); break; case 'd': turnRight(); break; case 's': stop(); break; case 'y': open(); break; case 'u': close(); break; case 'h': antiClockwise(); break; case 'j': clockwise(); break; case 'n': up(); break; case 'm': down();break; } }}
