Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- /*
- DIY Arduino Robot Arm Smartphone Control
- by Dejan, www.HowToMechatronics.com
- */
- #include <SoftwareSerial.h>
- #include <Servo.h>
- Servo servo01;
- Servo servo02;
- Servo servo03;
- Servo servo04;
- Servo servo05;
- Servo servo06;
- SoftwareSerial Bluetooth(3, 4); // Arduino(RX, TX) - HC-05 Bluetooth (TX, RX)
- int servo1Pos, servo2Pos, servo3Pos, servo4Pos, servo5Pos, servo6Pos; // current position
- int servo1PPos, servo2PPos, servo3PPos, servo4PPos, servo5PPos, servo6PPos; // previous position
- int servo01SP[50], servo02SP[50], servo03SP[50], servo04SP[50], servo05SP[50], servo06SP[50]; // for storing positions/steps
- int speedDelay = 20;
- int index = 0;
- String dataIn = "";
- void setup() {
- servo01.attach(5);
- servo02.attach(6);
- servo03.attach(7);
- servo04.attach(8);
- servo05.attach(9);
- servo06.attach(10);
- Serial3.begin(38400);
- //Bluetooth.begin(38400); // Default baud rate of the Bluetooth module
- Bluetooth.setTimeout(1);
- delay(20);
- // Robot arm initial position
- servo1PPos = 90;
- servo01.write(servo1PPos);
- servo2PPos = 150;
- servo02.write(servo2PPos);
- servo3PPos = 35;
- servo03.write(servo3PPos);
- servo4PPos = 140;
- servo04.write(servo4PPos);
- servo5PPos = 85;
- servo05.write(servo5PPos);
- servo6PPos = 80;
- servo06.write(servo6PPos);
- }
- void loop() {
- // Check for incoming data
- if (Bluetooth.available() > 0) {
- dataIn = Bluetooth.readString(); // Read the data as string
- // If "Waist" slider has changed value - Move Servo 1 to position
- if (dataIn.startsWith("s1")) {
- String dataInS = dataIn.substring(2, dataIn.length()); // Extract only the number. E.g. from "s1120" to "120"
- servo1Pos = dataInS.toInt(); // Convert the string into integer
- // We use for loops so we can control the speed of the servo
- // If previous position is bigger then current position
- if (servo1PPos > servo1Pos) {
- for ( int j = servo1PPos; j >= servo1Pos; j--) { // Run servo down
- servo01.write(j);
- delay(20); // defines the speed at which the servo rotates
- }
- }
- // If previous position is smaller then current position
- if (servo1PPos < servo1Pos) {
- for ( int j = servo1PPos; j <= servo1Pos; j++) { // Run servo up
- servo01.write(j);
- delay(20);
- }
- }
- servo1PPos = servo1Pos; // set current position as previous position
- }
- // Move Servo 2
- if (dataIn.startsWith("s2")) {
- String dataInS = dataIn.substring(2, dataIn.length());
- servo2Pos = dataInS.toInt();
- if (servo2PPos > servo2Pos) {
- for ( int j = servo2PPos; j >= servo2Pos; j--) {
- servo02.write(j);
- delay(50);
- }
- }
- if (servo2PPos < servo2Pos) {
- for ( int j = servo2PPos; j <= servo2Pos; j++) {
- servo02.write(j);
- delay(50);
- }
- }
- servo2PPos = servo2Pos;
- }
- // Move Servo 3
- if (dataIn.startsWith("s3")) {
- String dataInS = dataIn.substring(2, dataIn.length());
- servo3Pos = dataInS.toInt();
- if (servo3PPos > servo3Pos) {
- for ( int j = servo3PPos; j >= servo3Pos; j--) {
- servo03.write(j);
- delay(30);
- }
- }
- if (servo3PPos < servo3Pos) {
- for ( int j = servo3PPos; j <= servo3Pos; j++) {
- servo03.write(j);
- delay(30);
- }
- }
- servo3PPos = servo3Pos;
- }
- // Move Servo 4
- if (dataIn.startsWith("s4")) {
- String dataInS = dataIn.substring(2, dataIn.length());
- servo4Pos = dataInS.toInt();
- if (servo4PPos > servo4Pos) {
- for ( int j = servo4PPos; j >= servo4Pos; j--) {
- servo04.write(j);
- delay(30);
- }
- }
- if (servo4PPos < servo4Pos) {
- for ( int j = servo4PPos; j <= servo4Pos; j++) {
- servo04.write(j);
- delay(30);
- }
- }
- servo4PPos = servo4Pos;
- }
- // Move Servo 5
- if (dataIn.startsWith("s5")) {
- String dataInS = dataIn.substring(2, dataIn.length());
- servo5Pos = dataInS.toInt();
- if (servo5PPos > servo5Pos) {
- for ( int j = servo5PPos; j >= servo5Pos; j--) {
- servo05.write(j);
- delay(30);
- }
- }
- if (servo5PPos < servo5Pos) {
- for ( int j = servo5PPos; j <= servo5Pos; j++) {
- servo05.write(j);
- delay(30);
- }
- }
- servo5PPos = servo5Pos;
- }
- // Move Servo 6
- if (dataIn.startsWith("s6")) {
- String dataInS = dataIn.substring(2, dataIn.length());
- servo6Pos = dataInS.toInt();
- if (servo6PPos > servo6Pos) {
- for ( int j = servo6PPos; j >= servo6Pos; j--) {
- servo06.write(j);
- delay(30);
- }
- }
- if (servo6PPos < servo6Pos) {
- for ( int j = servo6PPos; j <= servo6Pos; j++) {
- servo06.write(j);
- delay(30);
- }
- }
- servo6PPos = servo6Pos;
- }
- // If button "SAVE" is pressed
- if (dataIn.startsWith("SAVE")) {
- servo01SP[index] = servo1PPos; // save position into the array
- servo02SP[index] = servo2PPos;
- servo03SP[index] = servo3PPos;
- servo04SP[index] = servo4PPos;
- servo05SP[index] = servo5PPos;
- servo06SP[index] = servo6PPos;
- index++; // Increase the array index
- }
- // If button "RUN" is pressed
- if (dataIn.startsWith("RUN")) {
- runservo(); // Automatic mode - run the saved steps
- }
- // If button "RESET" is pressed
- if ( dataIn == "RESET") {
- memset(servo01SP, 0, sizeof(servo01SP)); // Clear the array data to 0
- memset(servo02SP, 0, sizeof(servo02SP));
- memset(servo03SP, 0, sizeof(servo03SP));
- memset(servo04SP, 0, sizeof(servo04SP));
- memset(servo05SP, 0, sizeof(servo05SP));
- memset(servo06SP, 0, sizeof(servo06SP));
- index = 0; // Index to 0
- }
- }
- }
- // Automatic mode custom function - run the saved steps
- void runservo() {
- while (dataIn != "RESET") { // Run the steps over and over again until "RESET" button is pressed
- for (int i = 0; i <= index - 2; i++) { // Run through all steps(index)
- if (Bluetooth.available() > 0) { // Check for incomding data
- dataIn = Bluetooth.readString();
- if ( dataIn == "PAUSE") { // If button "PAUSE" is pressed
- while (dataIn != "RUN") { // Wait until "RUN" is pressed again
- if (Bluetooth.available() > 0) {
- dataIn = Bluetooth.readString();
- if ( dataIn == "RESET") {
- break;
- }
- }
- }
- }
- // If speed slider is changed
- if (dataIn.startsWith("ss")) {
- String dataInS = dataIn.substring(2, dataIn.length());
- speedDelay = dataInS.toInt(); // Change servo speed (delay time)
- }
- }
- // Servo 1
- if (servo01SP[i] == servo01SP[i + 1]) {
- }
- if (servo01SP[i] > servo01SP[i + 1]) {
- for ( int j = servo01SP[i]; j >= servo01SP[i + 1]; j--) {
- servo01.write(j);
- delay(speedDelay);
- }
- }
- if (servo01SP[i] < servo01SP[i + 1]) {
- for ( int j = servo01SP[i]; j <= servo01SP[i + 1]; j++) {
- servo01.write(j);
- delay(speedDelay);
- }
- }
- // Servo 2
- if (servo02SP[i] == servo02SP[i + 1]) {
- }
- if (servo02SP[i] > servo02SP[i + 1]) {
- for ( int j = servo02SP[i]; j >= servo02SP[i + 1]; j--) {
- servo02.write(j);
- delay(speedDelay);
- }
- }
- if (servo02SP[i] < servo02SP[i + 1]) {
- for ( int j = servo02SP[i]; j <= servo02SP[i + 1]; j++) {
- servo02.write(j);
- delay(speedDelay);
- }
- }
- // Servo 3
- if (servo03SP[i] == servo03SP[i + 1]) {
- }
- if (servo03SP[i] > servo03SP[i + 1]) {
- for ( int j = servo03SP[i]; j >= servo03SP[i + 1]; j--) {
- servo03.write(j);
- delay(speedDelay);
- }
- }
- if (servo03SP[i] < servo03SP[i + 1]) {
- for ( int j = servo03SP[i]; j <= servo03SP[i + 1]; j++) {
- servo03.write(j);
- delay(speedDelay);
- }
- }
- // Servo 4
- if (servo04SP[i] == servo04SP[i + 1]) {
- }
- if (servo04SP[i] > servo04SP[i + 1]) {
- for ( int j = servo04SP[i]; j >= servo04SP[i + 1]; j--) {
- servo04.write(j);
- delay(speedDelay);
- }
- }
- if (servo04SP[i] < servo04SP[i + 1]) {
- for ( int j = servo04SP[i]; j <= servo04SP[i + 1]; j++) {
- servo04.write(j);
- delay(speedDelay);
- }
- }
- // Servo 5
- if (servo05SP[i] == servo05SP[i + 1]) {
- }
- if (servo05SP[i] > servo05SP[i + 1]) {
- for ( int j = servo05SP[i]; j >= servo05SP[i + 1]; j--) {
- servo05.write(j);
- delay(speedDelay);
- }
- }
- if (servo05SP[i] < servo05SP[i + 1]) {
- for ( int j = servo05SP[i]; j <= servo05SP[i + 1]; j++) {
- servo05.write(j);
- delay(speedDelay);
- }
- }
- // Servo 6
- if (servo06SP[i] == servo06SP[i + 1]) {
- }
- if (servo06SP[i] > servo06SP[i + 1]) {
- for ( int j = servo06SP[i]; j >= servo06SP[i + 1]; j--) {
- servo06.write(j);
- delay(speedDelay);
- }
- }
- if (servo06SP[i] < servo06SP[i + 1]) {
- for ( int j = servo06SP[i]; j <= servo06SP[i + 1]; j++) {
- servo06.write(j);
- delay(speedDelay);
- }
- }
- }
- }
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement