Hardware components | ||||||
_ztBMuBhMHo.jpg?auto=compress%2Cformat&w=48&h=48&fit=fill&bg=ffffff) |
| × | 1 | |||
 |
| × | 1 | |||
 |
| × | 4 | |||
| × | 1 | ||||
 |
| × | 1 | |||
 |
| × | 1 | |||
| × | 1 | ||||
I wanted to control a robotic arm with 4 servos.
I wanted to be able to know the value of the angle at every moment to later pre-program routines too.
#include <Servo.h>
#include <Keypad.h>
#include <LiquidCrystal_I2C.h>
LiquidCrystal_I2C lcd(0x27,16,2);
Servo servoA; //Hand (open/close)
Servo servoB; // forward Backward
Servo servoC; //up down
Servo servoD; // Rotate (left-right)
// The kepad
const byte ROWS = 4;
const byte COLS = 4;
char keys[ROWS][COLS] = {
{'1','2','3','A'},
{'4','5','6','B'},
{'7','8','9','C'},
{'*','0','#','D'}};
byte rowPins[ROWS] = { 9, 8, 7, 13 };
byte colPins[COLS] = { 12, 4, 3, 2 };
Keypad keypad = Keypad( makeKeymap(keys), rowPins, colPins, ROWS, COLS );
String command;
char leter ='A'; // (Leter) servo to perform the command
int angle = 0; // (angle) Initial command
void setup()
{
// initialize the lcd
lcd.init();
lcd.backlight();
lcd.setCursor(0,0);
lcd.print("Enter command");
// Pins for the servos
servoA.attach(5);
servoB.attach(6);
servoC.attach(10);
servoD.attach(11);
//Starting point
servoA.write(80);
servoB.write(30);
servoC.write(120);
servoD.write(90);
}
void loop(){
char key = keypad.getKey();
if (key){
lcd.init();
lcd.setCursor(0,0);
lcd.print(String("Servo ") + String(leter));
lcd.setCursor(0,1);
lcd.print(String("Command ")+String(command + key));
// * on the keyapd will reset the command
if(key == '*') {
command = "";
lcd.init();
lcd.setCursor(0,0);
lcd.print(String("Servo ") + String(leter));
lcd.setCursor(0,1);
lcd.print("Command ");
}
//# on the keypad will execute the command on the chosen servo defined by (leter)
else if(key == '#')
{
lcd.init(); // initialize the lcd
lcd.setCursor(0,0);
lcd.print(String("Servo ") + String(leter)); // show on the LCD the servo that is chosen
lcd.setCursor(0,1);
lcd.print(String("Execute ")+String(command)); // show on the LCD the servo the comand is performing
int i = command.toInt(); // capture on (i) the integer of the command (only numbers)
// For SERVO A
if(leter == 'A') {
angle =servoA.read();
if ( i >= angle) {
for (angle = angle; angle <= i; angle=angle +1)
{servoA.write(angle);
delay(10);
}
}
else
{ for (angle = angle; angle >= i; angle=angle- 1)
{servoA.write(angle);
delay(10);
}
}
}
//For SERVO A
if(leter == 'B') {
angle =servoB.read();
if ( i >= angle) {
for (angle = angle; angle <= i; angle=angle +1)
{servoB.write(angle);
delay(10);
}
}
else
{ for (angle = angle; angle >= i; angle=angle- 1)
{servoB.write(angle);
delay(10);
}
}
}
// For SERVO C
if(leter == 'C') {
angle =servoA.read();
if ( i >= angle) {
for (angle = angle; angle <= i; angle=angle +1)
{servoC.write(angle);
delay(10);
}
}
else
{ for (angle = angle; angle >= i; angle=angle- 1)
{servoC.write(angle);
delay(10);
}
}
}
// For SERVO D
if(leter == 'D'){
angle =servoA.read();
if ( i >= angle) {
for (angle = angle; angle <= i; angle=angle +1)
{servoD.write(angle);
delay(20);
}
}
else
{ for (angle = angle; angle >= i; angle=angle- 1)
{servoD.write(angle);
delay(20);
} }
}
lcd.init();
lcd.setCursor(0,0);
lcd.print(String("Servo ") + String(leter)); //write on the screen the servo chosen
command = "";// empty command
}
// Chosing the servo
else if(key == 'A' or key == 'B' or key == 'C' or key == 'D')
{
leter = key;
lcd.init();
lcd.setCursor(0,0);
lcd.print(String("Servo ") + String(leter));//print the name of the servo chosen
if(leter == 'A') {
lcd.println(servoA.read()); //print the value from the servo chosen
}
// SERVO A
if(leter == 'B') {
lcd.println(servoB.read()); //print the value from the servo chosen
}
// SERVO C
if(leter == 'C') {
lcd.println(servoC.read()); //print the value from the servo chosen
}
// SERVO D
if(leter == 'D') {
lcd.println(servoD.read()); //print the value of the servo chosen
}
lcd.setCursor(0,1);
lcd.print(String("Command ")+String(command)); //print the value of the command
}
else {
command += key; //Add a digit to the command
}
}}
Comments