Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #include <SoftwareSerial.h>
- #include <Servo.h>
- SoftwareSerial softSerial(11, 8); //TX_BT --> D11 / RX_BT --> D8
- int rx = 0;
- int temp = 0;
- Servo servo1;
- Servo servo2;
- Servo servo3;
- Servo servo4;
- Servo servo5;
- int pos1 = 120;
- int pos2 = 60;
- int pos3 = 60;
- int pos4 = 60;
- int pos5 = 60;
- //1: 30-120 = 90
- //2: 60-150 = 90
- //3: 60-150 = 90
- //4: 60-150 = 90
- //5: 60-150 = 90
- void setup() {
- Serial.begin(9600);
- softSerial.begin(9600);
- servoA();
- servo1.write(pos1);
- servo2.write(pos2);
- servo3.write(pos3);
- servo4.write(pos4);
- servo5.write(pos5);
- delay(1000);
- }
- void loop() {
- if (softSerial.available() > 0) {
- rx = softSerial.read();
- softSerial.flush();
- //Serial.println(rx);
- }
- if (rx == 1 && temp != 1) {
- softSerial.flush();
- temp = 1;
- servoA();
- all_fingers_open();
- delay(1000);
- servoD();
- Serial.println("1");
- }
- else if (rx == 2 && temp != 2) {
- softSerial.flush();
- temp = 2;
- servoA();
- all_fingers_close();
- delay(1000);
- servoD();
- Serial.println("2");
- }
- else if (rx == 3 && temp != 3) {
- softSerial.flush();
- temp = 3;
- servoA();
- finger1open();
- four_fingers_close();
- delay(1000);
- servoD();
- Serial.println("3");
- }
- else if (rx == 4 && temp != 4) {
- softSerial.flush();
- temp = 4;
- servoA();
- finger1close();
- four_fingers_open();
- delay(1000);
- servoD();
- Serial.println("4");
- }
- delay(100);
- }// end of LOOP
- void all_fingers_close() {
- servo1.write(30);
- servo2.write(150);
- servo3.write(150);
- servo4.write(150);
- servo5.write(150);
- delay(15);
- }
- void all_fingers_open() {
- servo1.write(120);
- servo2.write(60);
- servo3.write(60);
- servo4.write(60);
- servo5.write(60);
- delay(15);
- }
- void finger1close() {
- servo1.write(30);
- delay(15);
- }
- void finger1open() {
- servo1.write(120);
- delay(15);
- }
- void four_fingers_close() {
- servo2.write(150);
- servo3.write(150);
- servo4.write(150);
- servo5.write(150);
- delay(15);
- }
- void four_fingers_open() {
- servo2.write(60);
- servo3.write(60);
- servo4.write(60);
- servo5.write(60);
- delay(15);
- }
- void servoD() {
- servo1.detach();
- servo2.detach();
- servo3.detach();
- servo4.detach();
- servo5.detach();
- }
- void servoA() {
- servo1.attach(3);
- servo2.attach(5);
- servo3.attach(6);
- servo4.attach(9);
- servo5.attach(10);
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement