Guest User

Untitled

a guest
Aug 12th, 2020
56
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1.  
  2.  
  3. #include <SoftwareSerial.h>
  4. #include <Servo.h>
  5.  
  6. Servo servo01;
  7. Servo servo02;
  8. Servo servo03;
  9. Servo servo04;
  10. Servo servo05;
  11. Servo servo06;
  12.  
  13. SoftwareSerial Bluetooth(0, 1); // Arduino(RX, TX) - HC-05 Bluetooth (TX, RX)
  14.  
  15. int servo1Pos, servo2Pos, servo3Pos, servo4Pos, servo5Pos, servo6Pos; // current position
  16. int servo1PPos, servo2PPos, servo3PPos, servo4PPos, servo5PPos, servo6PPos; // previous position
  17. int servo01SP[50], servo02SP[50], servo03SP[50], servo04SP[50], servo05SP[50], servo06SP[50]; // for storing positions/steps
  18. int speedDelay = 20;
  19. int index = 0;
  20. String dataIn = "";
  21. const String END_OF_MSG = ";";
  22.  
  23. void setup() {
  24.   servo01.attach(8);
  25.   servo02.attach(6);
  26.   servo03.attach(7);
  27.   servo04.attach(9);
  28.   servo05.attach(5);
  29.   servo06.attach(6);
  30.   Serial.begin(9600);
  31.   Bluetooth.begin(9600); // Default baud rate of the Bluetooth module
  32.   Bluetooth.setTimeout(1);
  33.   delay(20);
  34.   // Robot arm initial position
  35.   servo1PPos = 90;
  36.   servo01.write(servo1PPos);
  37.   servo2PPos = 150;
  38.   servo02.write(servo2PPos);
  39.   servo3PPos = 35;
  40.   servo03.write(servo3PPos);
  41.   servo4PPos = 140;
  42.   servo04.write(servo4PPos);
  43.   servo5PPos = 85;
  44.   servo05.write(servo5PPos);
  45.   servo6PPos = 80;
  46.   servo06.write(servo6PPos);
  47. }
  48. void moveServo(Servo servo, int& ppos, int pos, int delayTime) {
  49.   if (ppos > pos) {
  50.     // We use for loops so we can control the speed of the servo
  51.     // If previous position is bigger then current position
  52.     for ( int j = ppos; j >= pos; j--) {   // Run servo down
  53.       servo.write(j);
  54.       delay(delayTime);    // defines the speed at which the servo rotates
  55.     }
  56.   }
  57.   // If previous position is smaller then current position
  58.   if (ppos < pos) {
  59.     for ( int j = ppos; j <= pos; j++) {   // Run servo up
  60.       servo.write(j);
  61.       delay(delayTime);
  62.     }
  63.   }
  64.   ppos = pos;   // set current position as previous position
  65. }
  66.  
  67. void loop() {
  68.   // Check for incoming data
  69.   if (Bluetooth.available() > 0) {
  70.     dataIn = dataIn + Bluetooth.readString(); // Read the data as string
  71.   }
  72.   // Handle servo motor sliders
  73.   int indexOfEnd = dataIn.indexOf(END_OF_MSG);
  74.   Serial.print ("dataIn: ");
  75.   Serial.println(dataIn);
  76.   while (indexOfEnd != -1) {
  77.     //    Serial.print("Data had end of line character: ");
  78.     //    Serial.println(dataIn);
  79.     String servoType = dataIn.substring(0, 2);
  80.     String datum = dataIn.substring(2, indexOfEnd);
  81.  
  82.     //    Serial.print("Possible servo datum: ");
  83.     //    Serial.println(datum);
  84.  
  85.  
  86.     if (dataIn.startsWith("s1")) {
  87.       // Move Servo 1
  88.       servo1Pos = datum.toInt();  // Convert the string into integer
  89.       //      Serial.print("Moving servo 1 to: ");
  90.       //      Serial.println(servo1Pos);
  91.       moveServo(servo01, servo1PPos, servo1Pos, 20);
  92.       // Remove command from buffer
  93.       dataIn = dataIn.substring(indexOfEnd + 1, dataIn.length());
  94.       delay(1500);
  95.     } else if (dataIn.startsWith("s2")) {
  96.       // Move Servo 2
  97.       servo2Pos = datum.toInt();
  98.       //      Serial.print("Moving servo 2 to: ");
  99.       //      Serial.println(servo2Pos);
  100.       moveServo(servo02, servo2PPos, servo2Pos, 20);
  101.       dataIn = dataIn.substring(indexOfEnd + 1, dataIn.length());
  102.       delay(1500);
  103.     } else if (dataIn.startsWith("s3")) {
  104.       // Move Servo 3
  105.       servo3Pos = datum.toInt();
  106.       //      Serial.print("Moving servo 3 to: ");
  107.       //      Serial.println(servo3Pos);
  108.       moveServo(servo03, servo3PPos, servo3Pos, 20);
  109.       dataIn = dataIn.substring(indexOfEnd + 1, dataIn.length());
  110.       delay(1500);
  111.     } else if (dataIn.startsWith("s4")) {
  112.       // Move Servo 4
  113.       servo4Pos = datum.toInt();
  114.       //   Serial.print("Moving servo 4 to: ");
  115.       //Serial.println(servo4Pos);
  116.       moveServo(servo04, servo4PPos, servo4Pos, 20);
  117.       dataIn = dataIn.substring(indexOfEnd + 1, dataIn.length());
  118.       delay(1500);
  119.     } else if (dataIn.startsWith("s5")) {
  120.       // Move Servo 5
  121.       servo5Pos = datum.toInt();
  122.       //Serial.print("Moving servo 5 to: ");
  123.       //Serial.println(servo5Pos);
  124.       moveServo(servo05, servo5PPos, servo5Pos, 20);
  125.       dataIn = dataIn.substring(indexOfEnd + 1, dataIn.length());
  126.       delay(1500);
  127.     } else if (dataIn.startsWith("s6")) {
  128.       //Move Servo 6
  129.       servo6Pos = datum.toInt();
  130.       //Serial.print("Moving servo 6 to: ");
  131.       //Serial.println(servo6Pos);
  132.       moveServo(servo06, servo6PPos, servo6Pos, 20);
  133.       dataIn = dataIn.substring(indexOfEnd + 1, dataIn.length());
  134.  
  135.     } else if (dataIn.startsWith("SAVE;")) {
  136.       servo01SP[index] = servo1PPos;  // save position into the array
  137.       servo02SP[index] = servo2PPos;
  138.       servo03SP[index] = servo3PPos;
  139.       servo04SP[index] = servo4PPos;
  140.       servo05SP[index] = servo5PPos;
  141.       servo06SP[index] = servo6PPos;
  142.       index++;                        // Increase the array index
  143.       dataIn = dataIn.substring(5, dataIn.length());
  144.     } else if (dataIn.startsWith("RUN;")) {
  145.       runservo();  // Automatic mode - run the saved steps
  146.       dataIn = dataIn.substring(4, dataIn.length());
  147.     } else if (dataIn.startsWith("RESET;")) {
  148.       memset(servo01SP, 0, sizeof(servo01SP)); // Clear the array data to 0
  149.       memset(servo02SP, 0, sizeof(servo02SP));
  150.       memset(servo03SP, 0, sizeof(servo03SP));
  151.       memset(servo04SP, 0, sizeof(servo04SP));
  152.       // memset(servo05SP, 0, sizeof(servo05SP));
  153.       // memset(servo06SP, 0, sizeof(servo06SP));
  154.       index = 0;  // Index to 0
  155.       dataIn = dataIn.substring(6, dataIn.length());
  156.     } else {
  157.       break;
  158.     }
  159.     indexOfEnd = dataIn.indexOf(END_OF_MSG);
  160.   }
  161.  
  162.  
  163. }
  164.  
  165.  
  166. // Automatic mode custom function - run the saved steps
  167. void runservo() {
  168.   while (dataIn != "RESET") {   // Run the steps over and over again until "RESET" button is pressed
  169.     for (int i = 0; i <= index - 2; i++) {  // Run through all steps(index)
  170.       if (Bluetooth.available() > 0) {      // Check for incomding data
  171.         dataIn = Bluetooth.readString();
  172.         if ( dataIn == "PAUSE") {           // If button "PAUSE" is pressed
  173.           while (dataIn != "RUN") {         // Wait until "RUN" is pressed again
  174.             if (Bluetooth.available() > 0) {
  175.               dataIn = Bluetooth.readString();
  176.               if ( dataIn == "RESET") {
  177.                 break;
  178.               }
  179.             }
  180.           }
  181.         }
  182.         // If speed slider is changed
  183.         if (dataIn.startsWith("ss")) {
  184.           String dataInS = dataIn.substring(2, dataIn.length());
  185.           speedDelay = dataInS.toInt(); // Change servo speed (delay time)
  186.         }
  187.       }
  188.       // Servo 1
  189.       moveServo(servo01, servo01SP[i], servo01SP[i + 1], speedDelay);
  190.  
  191.       // Servo 2
  192.       moveServo(servo02, servo02SP[i], servo02SP[i + 1], speedDelay);
  193.  
  194.       // Servo 3
  195.       moveServo(servo03, servo03SP[i], servo03SP[i + 1], speedDelay);
  196.  
  197.       // Servo 4
  198.       moveServo(servo04, servo04SP[i], servo04SP[i + 1], speedDelay);
  199.  
  200.       // Servo 5
  201.       //moveServo(servo05, servo05SP[i], servo05SP[i + 1], speedDelay);
  202.  
  203.       // Servo 6
  204.       // moveServo(servo06, servo06SP[i], servo06SP[i + 1], speedDelay);
  205.     }
  206.   }
  207. }
RAW Paste Data

Adblocker detected! Please consider disabling it...

We've detected AdBlock Plus or some other adblocking software preventing Pastebin.com from fully loading.

We don't have any obnoxious sound, or popup ads, we actively block these annoying types of ads!

Please add Pastebin.com to your ad blocker whitelist or disable your adblocking software.

×