MeArm Controlled by Arduino Uno and TTP229-BSF Touchpad

// MeArm controlled by TTP229-BSF 16-key capacitive touchpad
// by Alan Wang

#include <Servo.h>

// hardware setup for TTP229-BSF:
// use resistors to connect P1-3 (TP2), P1-4 (TP3) and P2-5 (TP4) on the touchpad, 
//   in order to enable 16-key mode and all multiple-key mode.
//   see https://www.sunrom.com/get/611100 for more information.
// VCC -> 3.3V
// GND -> GND
// SCL -> 13
// SDO -> 12
#define SCL_PIN               13
#define SDO_PIN               12

// servo and touchpad settings
#define SERVO1_PIN            3   // base servo (servo 1)
#define SERVO2_PIN            5   // right side servo (servo 2)
#define SERVO3_PIN            6   // left side servo (servo 3)
#define SERVO4_PIN            9   // claw servo (servo 4)
#define SERVO_DEGREE_DELTA    3   // servo 1-3's gradually turning angle (bigger -> faster)
#define LOOP_DELAY_MS         25  // delay time for loop() (smaller -> faster)
#define SERVO_DEFAULT_DEGREE  90  // servo 1-3's default position
#define SERVO_MIN_DEGREE      30  // all servo's min angle
#define SERVO_MAX_DEGREE      150 // all servo's max angle
#define SERVO1_TO_MIN_KEY     11  // Touchpad key for moving servo 1 toward min position
#define SERVO1_TO_MAX_KEY     10  // Touchpad key for moving servo 1 toward max position
#define SERVO2_TO_MIN_KEY     8   // Touchpad key for moving servo 2 toward min position
#define SERVO2_TO_MAX_KEY     4   // Touchpad key for moving servo 2 toward max position
#define SERVO3_TO_MIN_KEY     5   // Touchpad key for moving servo 3 toward min position
#define SERVO3_TO_MAX_KEY     1   // Touchpad key for moving servo 3 toward max position
#define SERVO4_SWITCH_KEY     13  // Touchpad key for cycling servo 4 between min/max position
#define MEARM_TO_DEFAULT_KEY  16  // Touchpad key for turning all servos to default position


// variables
Servo servo[4];
int servo_pin[4] = {SERVO1_PIN, SERVO2_PIN, SERVO3_PIN, SERVO4_PIN};
int servo_degree[4] = {SERVO_DEFAULT_DEGREE, SERVO_DEFAULT_DEGREE, SERVO_DEFAULT_DEGREE, SERVO_MIN_DEGREE};
boolean claw_open = true;
int keypad[16];
int key_mapping[3][2] = {{SERVO1_TO_MIN_KEY, SERVO1_TO_MAX_KEY}, {SERVO2_TO_MIN_KEY, SERVO2_TO_MAX_KEY}, {SERVO3_TO_MIN_KEY, SERVO3_TO_MAX_KEY}};


// read the status of touchpad
void readKeypad() {

  for (int i = 0; i < 16; i++) {
    digitalWrite(SCL_PIN, LOW);
    keypad[i] = digitalRead(SDO_PIN);
    Serial.print(keypad[i]);
    digitalWrite(SCL_PIN, HIGH);
  }
  Serial.println("");
}


// initialization
void setup() {

  Serial.begin(9600);
  pinMode(SCL_PIN, OUTPUT);
  pinMode(SDO_PIN, INPUT);

  // attach servos and turn to default positions
  for (int i = 0; i < 4; i++) {
    servo[i].attach(servo_pin[i]);
    servo[i].write(servo_degree[i]);
  }
  delay(250);
}


// main program
void loop() {

  // read the status of touchpad
  readKeypad();

  // turn all servos back to default if the key is pressed
  if (keypad[MEARM_TO_DEFAULT_KEY - 1] == 0) {
    for (int i = 0; i < 3; i++) {
      servo_degree[i] = SERVO_DEFAULT_DEGREE;
    }
    servo_degree[3] = SERVO_MIN_DEGREE;
    claw_open = true;
    for (int i = 0; i < 4; i++) servo[i].write(servo_degree[i]);
    delay(250);
  }

  // cycle claw servo if the key is pressed
  if (keypad[SERVO4_SWITCH_KEY - 1] == 0) {
    claw_open = !claw_open;
    if (claw_open) servo_degree[3] = SERVO_MIN_DEGREE;
    else servo_degree[3] = SERVO_MAX_DEGREE;
    servo[3].write(servo_degree[3]);
    delay(250);
  }

  // gragually turn other 3 servos if keys are pressed
  for (int i = 0; i < 3; i++) {
    if (keypad[key_mapping[i][0] - 1] == 0) {
      servo_degree[i] -= SERVO_DEGREE_DELTA;
      if (servo_degree[i] < SERVO_MIN_DEGREE) servo_degree[i] = SERVO_MIN_DEGREE;
      servo[i].write(servo_degree[i]);
    } else if (keypad[key_mapping[i][1] - 1] == 0) {
      servo_degree[i] += SERVO_DEGREE_DELTA;
      if (servo_degree[i] > SERVO_MAX_DEGREE) servo_degree[i] = SERVO_MAX_DEGREE;
      servo[i].write(servo_degree[i]);
    }
  }

  delay(LOOP_DELAY_MS);
}