|
/*
|
|
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);
|
|
Bluetooth.begin(9600); // Default baud rate of the Bluetooth module
|
|
Serial.begin(9600);
|
|
Bluetooth.setTimeout(5);
|
|
delay(20);
|
|
// Robot arm initial position
|
|
servo1PPos = 55;
|
|
servo01.write(servo1PPos);
|
|
servo2PPos = 120;
|
|
servo02.write(servo2PPos);
|
|
servo3PPos = 120;
|
|
servo03.write(servo3PPos);
|
|
servo4PPos = 80;
|
|
servo04.write(servo4PPos);
|
|
servo5PPos = 5;
|
|
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
|
|
Serial.println(dataIn);
|
|
|
|
// 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) {
|
|
Serial.print("Servo 1 : ");
|
|
Serial.print(servo1PPos);
|
|
Serial.print(" => ");
|
|
Serial.println(servo1Pos);
|
|
for ( int j = servo1PPos; j >= servo1Pos; j--) { // Run servo down
|
|
Serial.print("Servo 1 : ");
|
|
Serial.println(j);
|
|
servo01.write(j);
|
|
delay(20); // defines the speed at which the servo rotates
|
|
}
|
|
}
|
|
// If previous position is smaller then current position
|
|
if (servo1PPos < servo1Pos) {
|
|
Serial.print("Servo 1 : ");
|
|
Serial.print(servo1PPos);
|
|
Serial.print(" => ");
|
|
Serial.println(servo1Pos);
|
|
for ( int j = servo1PPos; j <= servo1Pos; j++) { // Run servo up
|
|
Serial.print("Servo 1 : ");
|
|
Serial.println(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) {
|
|
Serial.print("Servo 2 : ");
|
|
Serial.print(servo2PPos);
|
|
Serial.print(" => ");
|
|
Serial.println(servo2Pos);
|
|
for ( int j = servo2PPos; j >= servo2Pos; j--) {
|
|
Serial.print("Servo 2 : ")
|
|
Serial.println(j);
|
|
servo02.write(j);
|
|
delay(50);
|
|
}
|
|
}
|
|
if (servo2PPos < servo2Pos) {
|
|
Serial.print("Servo 2 : ");
|
|
Serial.print(servo2PPos);
|
|
Serial.print(" => ");
|
|
Serial.println(servo2Pos);
|
|
for ( int j = servo2PPos; j <= servo2Pos; j++) {
|
|
Serial.print("Servo2 : ");
|
|
Serial.println(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) {
|
|
Serial.print("Servo 3 : ");
|
|
Serial.print(servo3PPos);
|
|
Serial.print(" => ");
|
|
Serial.println(servo3Pos);
|
|
for ( int j = servo3PPos; j >= servo3Pos; j--) {
|
|
Serial.print("Servo3 : ");
|
|
Serial.println(j);
|
|
servo03.write(j);
|
|
delay(30);
|
|
}
|
|
}
|
|
if (servo3PPos < servo3Pos) {
|
|
for ( int j = servo3PPos; j <= servo3Pos; j++) {
|
|
Serial.print("Servo3 :")
|
|
Serial.println(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) {
|
|
Serial.print("Servo 4 : ");
|
|
Serial.print(servo4PPos);
|
|
Serial.print(" => ");
|
|
Serial.println(servo4Pos);
|
|
for ( int j = servo4PPos; j >= servo4Pos; j--) {
|
|
Serial.print("Servo4 : ");
|
|
Serial.println(j);
|
|
servo04.write(j);
|
|
delay(30);
|
|
}
|
|
}
|
|
if (servo4PPos < servo4Pos) {
|
|
for ( int j = servo4PPos; j <= servo4Pos; j++) {
|
|
Serial.print("Servo4 : ");
|
|
Serial.println(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) {
|
|
Serial.print("Servo 5 : ");
|
|
Serial.print(servo5PPos);
|
|
Serial.print(" => ");
|
|
Serial.println(servo5Pos);
|
|
for ( int j = servo5PPos; j >= servo5Pos; j--) {
|
|
Serial.print("Servo5 : ");
|
|
Serial.println(j);
|
|
servo05.write(j);
|
|
delay(30);
|
|
}
|
|
}
|
|
if (servo5PPos < servo5Pos) {
|
|
Serial.print("Servo 5 : ");
|
|
Serial.print(servo5PPos);
|
|
Serial.print(" => ");
|
|
Serial.println(servo5Pos);
|
|
for ( int j = servo5PPos; j <= servo5Pos; j++) {
|
|
Serial.print("Servo5 : ");
|
|
Serial.println(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) {
|
|
Serial.print("Servo 6 : ");
|
|
Serial.print(servo6PPos);
|
|
Serial.print(" => ");
|
|
Serial.println(servo6Pos);
|
|
for ( int j = servo6PPos; j >= servo6Pos; j--) {
|
|
Serial.print("Servo6 : ");
|
|
Serial.println(j);
|
|
servo06.write(j);
|
|
delay(30);
|
|
}
|
|
}
|
|
if (servo6PPos < servo6Pos) {
|
|
Serial.print("Servo 6 : ");
|
|
Serial.print(servo6PPos);
|
|
Serial.print(" => ");
|
|
Serial.println(servo6Pos);
|
|
for ( int j = servo6PPos; j <= servo6Pos; j++) {
|
|
Serial.print("Servo6 : ");
|
|
Serial.println(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);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
