#include <Servo.h>

#include <LiquidCrystal.h>


#define BT Serial1
String command;
String device;
int time = 2000; // <== Could be necessary to adjust
/* Define Arm parameters to be used */
// MeArm 4-DOF
#define minGrip 15
#define minBase 0
#define minShou 60
#define minElbw 60
#define maxGrip 45
#define maxBase 170
#define maxShou 180
#define maxElbw 150
#define midGrip 30
#define midBase 87
#define midShou 138
#define midElbw 100

/* SS 3-DOF
#define minGrip 75
#define minBase 5
#define minShou 5
#define minElbw O
#define maxGrip 125
#define maxBase 150 #define maxShou 155
#define maxElbw 0
#define midGrip 100
#define midBase 90
#define midshou 90
#define midElbw 0
/*********/
LiquidCrystal lcd(12,11,2,4,6,8);
const int servoBasePin = 3;
// first servo - base
const int servoShouPin = 10; const int servoGripPin = 5;
const int servoElbwPin = 9;
// second servo shouder // third servo- gripp
// forth servo (ELBOW used at meArm)
const int recordPositionPin = 7; // Button to record position
const int ledGreen = 13; // LED to inform that a position was recorded
const int execTaskPin = 15; // Button to record position 
const int ledBlue =14;// LED to inform that a recorded task is underway
const int ledYellow = 21;
 int remoteCommand = 0;
const int resetSwitchPin= 17;

 int resetSwitch = 0; // 
const int ledRed = 20;
// LED to inform that a recorded task is aborted
const int joyBase = 2;
const int joyShou = 3;
const int potpin = 0; const int joyElbw = 1;
// Pot or Joystick used for Basemouth control
// Pot or Joystick used for shouation control // Pot used for Gripp control
// Pot used for Gripp control
const int buzzerPin = 16;
// variables to read the value from the analog pin
// variables to store the value from the analog pin at last loop
int servoBaseVal;
int servoShouVal;
int servoGripVal;
int servoElbwVal;
int servoBasePastVal;
int servoShouPastVal;
int servoGripPastVal;
int servoElbwPastVal;
int movement;
int recordPosition = 0;
int execTaskCmd = 0;
// if HIGH, record servo position // if HIGH, run program
Servo myservoBase; // create servo object to control a servo
Servo myservoShou; // create servo object to control a servo
Servo myservoGrip; // create servo object to control a servo
Servo myservoElbw; // create servo object to control a servo
int gripPosition[100];
int basePosition [100];
int shouPosition[100];
int elbwPosition[100];
int positionIndex = 0;
// Index to be used at position array used for recorded tasks




/***************************************************
* Read and Map sliders at Android for Servos positioning
***************************************************/
bool readSliders()
{
char armServo = command[0];
byte value = command[2];
//Serial.print(armServo);
// Serial.println(value);
if ( armServo == 'g')
{
servoGripVal = value;
servoGripVal = map(servoGripVal, 0, 255, minGrip, maxGrip); // scale it to use it with theservo
}
if ( armServo == 'b')
{
servoBaseVal = value;
servoBaseVal = map(servoBaseVal, 0, 255, minBase, maxBase); // scale it to use it withthe servo
}
if ( armServo == 's')
{
servoShouVal = value;
servoShouVal = map(servoShouVal, 0, 255, minShou, maxShou); // scale it to use it withthe servo
}
if ( armServo == 'e')
{
servoElbwVal = value;
servoElbwVal = map(servoElbwVal, 0, 255, minElbw, maxElbw); // scale it to use it withthe servo
}
if (servoGripVal != servoGripPastVal || servoBaseVal != servoBasePastVal || servoShouVal !=
servoShouPastVal || servoElbwVal != servoElbwPastVal)
{
movement = true;
}else movement = false;
return movement;
}
/***************************************************
* Read and Map Potenciometers for Servos positioning
***************************************************/
bool readPotenciometers()
{
servoGripVal = analogRead(potpin);
servoGripVal = map(servoGripVal, 0, 1023, minGrip, maxGrip); // scale it to use it with theservo
// Read the Basemouth value (value between 0 and 1023)
servoBaseVal = analogRead(joyBase);
servoBaseVal = map(servoBaseVal, 0, 1023, minBase, maxBase); // scale it to use it withthe servo
// Read the shouation value (value between 0 and 1023)
servoShouVal = analogRead(joyShou);
servoShouVal = map(servoShouVal, 0, 1023, minShou, maxShou); // scale it to use it withthe servo
// Read the elbow value (value between 0 and 1023)
servoElbwVal = analogRead(joyElbw);
servoElbwVal = map(servoElbwVal, 0, 1023, minElbw, maxElbw); // scale it to use it with theservo
if (servoGripVal != servoGripPastVal || servoBaseVal != servoBasePastVal || servoShouVal !=
servoShouPastVal || servoElbwVal != servoElbwPastVal)
{
movement = true;
}else movement = false;
return movement;
}
/***************************************************
* Read and Map Potenciometers at begginging
***************************************************/
void readPotenciometersInitial()
{
servoGripVal = analogRead(potpin);
servoGripVal = map(servoGripVal, 0, 1023, minGrip, maxGrip); // scale it to use it with theservo
// Read the Basemouth value (value between 0 and 1023)
servoBaseVal = analogRead(joyBase);
servoBaseVal = map(servoBaseVal, 0, 1023, minBase, maxBase); // scale it to use it withthe servo
// Read the shouation value (value between 0 and 1023)
servoShouVal = analogRead(joyShou);
servoShouVal = map(servoShouVal, 0, 1023, minShou, maxShou); // scale it to use it withthe servo
// Read the elbow value (value between 0 and 1023)
servoElbwVal = analogRead(joyElbw);
servoElbwVal = map(servoElbwVal, 0, 1023, minElbw, maxElbw); // scale it to use it with theservo
servoBasePastVal = servoBaseVal;
servoShouPastVal = servoShouVal;
servoGripPastVal = servoGripVal;
servoElbwPastVal = servoElbwVal;
outputPosition();
}
/***************************************************
* Put Arm at given position
***************************************************/
void armPosition(int gripp, int basee, int shouder, int elbow )
{
myservoGrip.write(gripp);
delay(15);
myservoBase.write(basee);
delay(15);
myservoShou.write(shouder);
delay(15);
myservoElbw.write(elbow);
delay(15);
}
/***************************************************
* Put Arm at "position zero"
***************************************************/
void armPositionZero()
{
servoBaseVal = midBase;
servoShouVal = midShou;
servoGripVal = midGrip;
servoElbwVal = midElbw;
myservoBase.write(servoBaseVal);
delay(15);
myservoShou.write(servoShouVal);
delay(15);
myservoGrip.write(servoGripVal);
delay(15);
myservoElbw.write(servoElbwVal);
delay(15);
servoBasePastVal = servoBaseVal;
servoShouPastVal = servoShouVal;
servoGripPastVal = servoGripVal;
servoElbwPastVal = servoElbwVal;
}
/*************************************************
* Display Servo Positions at Serial Monitor
**************************************************/
void outputPosition()
{
Serial.print(" Gripp: ");
Serial.print (servoGripVal);
Serial.print(" Base: ");
Serial.print(servoBaseVal);
Serial.print(" Shoulder: ");
Serial.print(servoShouVal);
Serial.print(" Elbow: ");
Serial.println (servoElbwVal);
}
/**************************************************
* Print at Serial Monotor the actual Arm Position
***************************************************/
void serialMonitorArmPosition()
{
Serial.print("Position ==>");
Serial.print(positionIndex);
Serial.print(" Gripp: ");
Serial.println(gripPosition[positionIndex]);
Serial.print(" Base: ");
Serial.print(basePosition[positionIndex]);
Serial.print(" Shoulder: ");
Serial.print(shouPosition[positionIndex]);
Serial.print(" Elbow: ");
Serial.println(elbwPosition[positionIndex]);
}
/***************************************************
* Display Arm Position ar LCD
****************************************************/
void lcdArmPosition()
{
lcd.begin(16, 2);
lcd.print("Grp Bas Sho Elw ");
lcd.setCursor(0, 1);
lcd.print(" ");
lcd.setCursor(0, 1);
lcd.print(" ");
lcd.setCursor(0, 1);
lcd.print(servoGripVal);
lcd.setCursor(4, 1);
lcd.print(" ");
lcd.setCursor(4, 1);
lcd.print(servoBaseVal);
lcd.setCursor(8, 1);
lcd.print(" ");
lcd.setCursor(8, 1);
lcd.print(servoShouVal);
lcd.setCursor(12, 1);
lcd.print(" ");
lcd.setCursor(12, 1);
lcd.print(servoElbwVal);
}
/***************************************************
* Display Arm proggramed Position (steps) at LCD
****************************************************/
void lcdArmPgmPosition(int pos)
{
lcd.begin(16, 2);
lcd.setCursor(0, 0);
lcd.print(" ");
lcd.setCursor(0, 0);
lcd.print("Position # ");
lcd.setCursor(12, 0);
lcd.print(" ");
lcd.setCursor(12, 0);
lcd.print(pos);
lcd.setCursor(0, 1);
lcd.print(" ");
lcd.setCursor(0, 1);
lcd.print(" ");
lcd.setCursor(0, 1);
lcd.print(servoGripVal);
lcd.setCursor(4, 1);
lcd.print(" ");
lcd.setCursor(4, 1);
lcd.print(servoBaseVal);
lcd.setCursor(8, 1);
lcd.print(" ");
lcd.setCursor(8, 1);
lcd.print(servoShouVal);
lcd.setCursor(12, 1);
lcd.print(" ");
lcd.setCursor(12, 1);
lcd.print(servoElbwVal);
}
/***********************************************************
* Warning with sound start of a programed task
************************************************************/
void warningStartTask()
{
lcd.begin(16, 2);
lcd.setCursor(0, 0);
lcd.print(" ");
lcd.setCursor(0, 0);
lcd.print("WARNING PGM INIT");
lcd.setCursor(0, 1);
lcd.print(" ");
lcd.setCursor(0, 1);
lcd.print("<<<<<<<<>>>>>>>>");
for (int cont=30; cont<100; cont++)
{ // Play Music
beep(buzzerPin, 10*cont, 10);
}
for (int cont=100; cont>30; cont--)
{
beep(buzzerPin, 10*cont, 10);
}
for (int cont=30; cont<100; cont++)
{ // Play Music
beep(buzzerPin, 10*cont, 10);
}
for (int cont=100; cont>30; cont--)
{
beep(buzzerPin, 10*cont, 10);
}
for(int cont = 0; cont < 3; cont++){ //Bip 2 times
beep(buzzerPin, 1000, 100);
delay(100);
}
}
/***********************************************************
* Warning with sound the start programming a new task
************************************************************/
void warningFinishTask()
{
lcd.begin(16, 2);
lcd.setCursor(0, 0);
lcd.print(" ");
lcd.setCursor(0, 0);
lcd.print(" Start New PGM ");
lcd.setCursor(0, 1);
lcd.print(" ");
lcd.setCursor(0, 1);
lcd.print(">>>>>>><<<<<<<<<");
for (int cont=30; cont<100; cont++)
{ // Play Music
beep(buzzerPin, 5*cont, 10);
}
for (int cont=100; cont>30; cont--)
{
beep(buzzerPin, 5*cont, 10);
}
for (int cont=30; cont<100; cont++)
{ // Play Music
beep(buzzerPin, 10*cont, 10);
}
for (int cont=100; cont>30; cont--)
{
beep(buzzerPin, 10*cont, 10);
}
for(int cont = 0; cont < 3; cont++){ //Bip 2 times
beep(buzzerPin, 1000, 300);
delay(100);
}
}
/***********************************************************
* Alarm command received
************************************************************/
void alarmRemoteCommand()
{
lcd.begin(16, 2);
lcd.setCursor(0, 0);
lcd.print(" ");
lcd.setCursor(0, 0);
lcd.print("REMOTE - CONTROL");
lcd.setCursor(0, 1);
lcd.print(" ");
lcd.setCursor(0, 1);
lcd.print("<<<<<<<<>>>>>>>>");
for (int cont=30; cont<100; cont++){ // Play Music
beep(buzzerPin, 10*cont, 10);
}
for (int cont=100; cont>30; cont--){
beep(buzzerPin, 10*cont, 10);
}
for (int cont=30; cont<100; cont++){ // Play Music
beep(buzzerPin, 10*cont, 10);
}
for (int cont=100; cont>30; cont--){
beep(buzzerPin, 10*cont, 10);
}
lcd.setCursor(0, 0);
lcd.print(" Start ");;
lcd.setCursor(0, 1);
lcd.print(" Programing ");
command = "remote";
remoteCommand = 0;
}




/************************************************************************************************************
*****************
* 3 or 4-DOF Arm Programming to perform a specific repetitive task
* Programing local or remote using Android/BT
* ==> In mode "Local" the servos are controled only by Potenciometers.
* ==> In mode "Remote" the servos are controled only by Sliders at Android.
* ==> Physical Switchs (Buttons) work in paralell with Android.
* ==> Arm Robot Starts as "Local" mode control. Pressing "Alarm" or "Remote/Local" button at
Android, will put in mode "Remote"
* ==> Press button PROGRAM localy or at Android to Record a step (LED Grenn will light up)
* ==> Press button RUN localy or at Android to execute sequencially the recorded steps, the
"program" (LED blue will light up)
* ==> Press button RESTART at Android to abort program (LED red will light up) - Note that
should be at end of a completre pgm.
* ==> Servos Ranges can be defined at ArmDefines.h or at by Android app
Develop by Marcelo Jose Rovai
Please visit http://MJRoBot.org for more information
The Android app can be dowload free at Google Play Store:
https://play.google.com/store/apps/details?id=appinventor.ai_mjrovai.MJRoBot_BT_ARM_CTRL
_V2_1&hl=en
16Fev16
*************************************************************************************************************
*******************/
#include <Servo.h>
#include <LiquidCrystal.h>

void setup()
{
Serial.begin(9600);
BT.begin(9600);
lcdMessage("Arm Robot prog.", " AU ROBOT"); // Display message at LCD
myservoBase.attach(servoBasePin); // attaches the servo
myservoShou.attach(servoShouPin); // attaches the servo
myservoGrip.attach(servoGripPin); // attaches the servo
myservoElbw.attach(servoElbwPin); // attaches the servo
pinMode (ledYellow, OUTPUT);
pinMode (ledGreen, OUTPUT);
pinMode (ledRed, OUTPUT);
pinMode (buzzerPin, OUTPUT);
pinMode (recordPositionPin, INPUT);
pinMode (execTaskPin, INPUT);
pinMode (resetSwitchPin, INPUT);
digitalWrite(ledRed, LOW);
digitalWrite(ledGreen, LOW);
digitalWrite(ledYellow, HIGH);
armPositionZero(); //Put Arm at "position zero" that is define as "mid<servo>" position
delay (time);
lcdArmPosition(); // Display Servo Positions at LCD
outputPosition(); // Display Servo Positions at Serial Monitor
warningStartTask();
digitalWrite(ledGreen, HIGH);
}
void loop()
{
checkBTcmd();
defineLocalRemote();
execTaskCmd = digitalRead (execTaskPin);
if(execTaskCmd == HIGH || command == "runon")
{
runProgram();
}
else recArmPosition();
command = "";
}
/***************************************************
* Defime mode Local / Remote
***************************************************/
void defineLocalRemote()
{
if (command == "alarmon")
{
alarmRemoteCommand();
}
if (command == "remote")
{
remoteCommand = !remoteCommand;
digitalWrite(ledYellow, !remoteCommand);
BT.print("change command");
}
}
/***************************************************
* Run Program
***************************************************/
void runProgram()
{
digitalWrite(ledBlue, HIGH);
digitalWrite(ledGreen, LOW);
executeTask();
lcdArmPosition();
outputPosition();
digitalWrite(ledBlue, LOW);
digitalWrite(ledRed, LOW);
digitalWrite(ledGreen, HIGH);
}
/***************************************************
* Record Arm Positon
***************************************************/
void recArmPosition()
{
recordPosition = debounce (recordPositionPin);
if(recordPosition == HIGH || command == "pgmon")
{
positionIndex++;
gripPosition[positionIndex] = servoGripVal;
basePosition[positionIndex] = servoBaseVal;
shouPosition[positionIndex] = servoShouVal;
elbwPosition[positionIndex] = servoElbwVal;
digitalWrite(ledGreen, HIGH);
beep(buzzerPin, 1000, 100);
serialMonitorArmPosition();
lcdArmPgmPosition(positionIndex);
delay(1000);
}
if (!remoteCommand && readPotenciometers())
{
lcdArmPosition();
outputPosition();
digitalWrite(ledGreen, LOW);
armPosition(servoGripVal, servoBaseVal, servoShouVal, servoElbwVal);
servoGripPastVal = servoGripVal;
servoBasePastVal = servoBaseVal;
servoShouPastVal = servoShouVal;
servoElbwPastVal = servoElbwVal;
}
if (remoteCommand && readSliders())
{
lcdArmPosition();
outputPosition();
digitalWrite(ledGreen, LOW);
armPosition(servoGripVal, servoBaseVal, servoShouVal, servoElbwVal);
servoGripPastVal = servoGripVal;
servoBasePastVal = servoBaseVal;
servoShouPastVal = servoShouVal;
servoElbwPastVal = servoElbwVal;
}
}
/***************************************************
* Executed recorded task program
***************************************************/
void executeTask()
{
int i;
warningStartTask();
resetSwitch = digitalRead(resetSwitchPin);
while (resetSwitch == LOW)
{
for (i=1; i<=positionIndex; i++)
{
lcdArmPgmPosition(i);
armPosition(gripPosition[i], basePosition[i], shouPosition[i], elbwPosition[i]);
delay (time);
}
i=0;
checkBTcmd();
Serial.println (command);
if (command == "rston") {resetSwitch = HIGH;}
else {resetSwitch = digitalRead(resetSwitchPin);}
}
digitalWrite(ledRed, HIGH);
positionIndex = 0;
BT.print("reset");
warningFinishTask();
armPositionZero();
}






/***********************************************************
* Verify if there is data at BT if YES, storage at "command"
************************************************************/
void checkBTcmd()
{
while (BT.available()) //Check if there is an available byte to read
{
delay(10); //Delay added to make thing stable
char c = BT.read(); //Conduct a serial read
device += c; //build the string.
}
if (device.length() > 0)
{
Serial.print("Command received from BT ==> ");
Serial.println(device);
command = device;
device =""; //Reset the variable
}
}
/***********************************************************
* Sound Generator
************************************************************/
void beep(int pin, int freq, long ms) //square wave - freq ==> ms
{
int i;
long semiper = (long) (1000000/(freq*2));
long loops = (long)((ms*1000)/(semiper*2));
for (i=0;i<loops;i++){
digitalWrite(pin, HIGH); //set buzzer pin high
delayMicroseconds(semiper); //for half of the period
digitalWrite(pin, LOW); //set buzzer pin low
delayMicroseconds(semiper); //for the other half of the period
}
}
/***************************************************
* Debouncing a key
****************************************************/
boolean debounce(int pin)
{
boolean state;
boolean previousState;
const int debounceDelay = 60;
previousState = digitalRead(pin);
for(int counter=0; counter< debounceDelay; counter++)
{
delay(1);
state = digitalRead(pin);
if(state != previousState)
{
counter =0;
previousState = state;
}
}
return state;
}
/***************************************************
* Display message at LCD
****************************************************/
void lcdMessage(String line1, String line2)
{
lcd.begin(16, 2);
lcd.print(" ");
lcd.print(line1);
lcd.setCursor(0, 1);
lcd.print(" ");
lcd.print(line2);
}