EGRA_SG-90_SAMPLE_CODE.ino

/*
   E-GRA SG-90 SAMPLE CODES
 
 THIS IS A COMPLETE CODE FOR CONTROLLING THE
 ROBOTIC ARM (GRIPPER, ELBOW, SHOULDER, BASE)
 INTERFACE TO GIZDUINO 328 UNO AND KEYPAD.
 OPEN THE SERIAL MONITOR TO SEE THE POSITIONS
 OR DEGREES.
 
 LIBRARY USED:
 SERVO LIBRARY
 KEYPAD LIBRARY
 
 THE WIRING CONNECTIONS:
 EGRA TO GIZDUINO 328
 GRIPPER  - PIN 3
 ELBOW    - PIN 5
 SHOULDER - PIN 6
 BASE     - PIN 9
 (NOTE: USE THE PWM PINS FOR THIS APPLICATION)
 
 KEYPAD TO GIZDUINO 328
 COLUMNS:
 C1,C2,C3,C4 - 13,12,11,10
 ROWS:
 R1,R2,R3,R4 - 14,15,16,17
 
 KEYPAD FUNCTIONS:
 A = GRIPPER GRAB/HOLD
 B = GRIPPER RELEASE
 3 = ELBOW EXTENDED
 9 = ELBOW RETRACT
 2 = SHOULDER TURN UP
 8 = SHOULDER TURN DOWN
 4 = BASE TURN LEFT
 6 = BASE TURN RIGHT
 
 
 CODE BY E-GIZMO MECHATRONIX CENTRAL
 http://www.e-Gizmo.net
 JANUARY 16,2018
 
 */
#include <Keypad.h>
#include <Servo.h>

const byte ROWS = 4; //four rows
const byte COLS = 4; //three columns
char KEYS[ROWS][COLS] = {
  {
    '1','2','3','A'                                      }
  ,
  {
    '4','5','6','B'                                      }
  ,
  {
    '7','8','9','C'                                      }
  ,
  {
    '*','0','#','D'                                      }
};
byte ROWPINS[ROWS] = {
  14, 15, 16, 17}; //connect to the row pinouts of the keypad
byte COLPINS[COLS] = {
  13, 12, 11, 10}; //connect to the column pinouts of the keypad

Keypad keypad = Keypad( makeKeymap(KEYS), ROWPINS, COLPINS, ROWS, COLS );
char HOLDKEY;
unsigned long T_HOLD;
//SERVOS
Servo GRIPPER;
Servo ELBOW;

Servo SHOULDER;
Servo BASE;
//INITIAL POSITIONS (MUST CHANGE)
// TO GET THE EXACT CALIBRATION
int G_POS = 120;
int E_POS = 70;
int S_POS = 130;
int B_POS = 80;

int DEGREES = 10;


void setup(){
  Serial.begin(9600);

  GRIPPER.attach(3);
  ELBOW.attach(5);
  SHOULDER.attach(6);
  BASE.attach(9);


  //INITIAL POSITION
  GRIPPER.write(G_POS);
  ELBOW.write(E_POS);
  SHOULDER.write(S_POS);
  BASE.write(B_POS);


}

void loop(){
  char KEY = keypad.getKey();
  if(KEY){
    HOLDKEY = KEY;
    Serial.println(KEY);
  }
  if(keypad.getState() == HOLD){
    if ((millis() - T_HOLD)>100){
      switch(HOLDKEY){
      case 'A':
        G_GRABING();
        break;
      case 'B':
        G_RELEASING();
        break;
      case '2':
        S_TURNUP();
        break;
      case '8':
        S_TURNDOWN();
        break;
      case '4':
        B_TURNLEFT();
        break;
      case '6':
        B_TURNRIGHT();
        break;
      case '3':
        E_EXTENDED();
        break;
      case '9':
        E_RETRACT();
        break;
      }
      T_HOLD = millis();
    }
  }
}
// E-GRA FUNCTIONS //
// YOU MAY MODIFY THIS SECTION, FOR CALIBRATION OF SERVO MOTORS //
void G_GRABING(){
  G_POS = G_POS+DEGREES;
  if(G_POS>=120){
    G_POS=120; 
  }
  GRIPPER.write(G_POS);
  Serial.println(G_POS);
}
void G_RELEASING(){
  G_POS = G_POS-DEGREES;
  if(G_POS<=80){
    G_POS=80;
  } 
  GRIPPER.write(G_POS); 
  Serial.println(G_POS);
}
void E_EXTENDED(){
  E_POS = E_POS+DEGREES;
  if(E_POS>=180){
    E_POS=180; 
  }
  ELBOW.write(E_POS);
  Serial.println(E_POS);
}
void E_RETRACT(){
  E_POS = E_POS-DEGREES;
  if(E_POS<=70){
    E_POS=70; 
  }
  ELBOW.write(E_POS);
  Serial.println(E_POS); 
}
void S_TURNDOWN(){
  S_POS = S_POS+DEGREES;
  if(S_POS>=180){
    S_POS=180.;
  }
  SHOULDER.write(S_POS);
  Serial.println(S_POS);
}
void S_TURNUP(){
  S_POS = S_POS-DEGREES;
  if(S_POS<=80){
    S_POS=80.;
  }
  SHOULDER.write(S_POS);
  Serial.println(S_POS);
}
void B_TURNLEFT(){
  B_POS = B_POS+DEGREES;
  if(B_POS>=160){
    B_POS=160.;
  }
  BASE.write(B_POS);
  Serial.println(B_POS);
}
void B_TURNRIGHT(){
  B_POS = B_POS-DEGREES;
  if(B_POS<=10){
    B_POS=10;
  }
  BASE.write(B_POS);
  Serial.println(B_POS);
}