antdroid.ino

/**
   Antdroid Simplex
   Simple movements on android

   Coxa   - Servo that connects leg to body 'the first or basal segment of the leg of an insect'
   Femur  - Middle leg servo
   Tibia  - Lower leg (the blue legs)
*/

#include <Servo.h>

/** How long to wait inbetween each servo update in ms */
#define SERVO_WAIT_TIME   40
#define SERVO_SETUP_DELAY 200

/** PINS  @TODO*/
#define LFC_PIN 22
#define LFF_PIN 23
#define LFT_PIN 24

#define LMC_PIN 25
#define LMF_PIN 26
#define LMT_PIN 27

#define LBC_PIN 28
#define LBF_PIN 29
#define LBT_PIN 30

#define RFC_PIN 31
#define RFF_PIN 32
#define RFT_PIN 33

#define RMC_PIN 34
#define RMF_PIN 35
#define RMT_PIN 36

#define RBC_PIN 37
#define RBF_PIN 38
#define RBT_PIN 39

/** Servos */ // Maybe an array of servos would be better
Servo LF_Coxa;
Servo LF_Femur;
Servo LF_Tibia;
Servo LM_Coxa;
Servo LM_Femur;
Servo LM_Tibia;
Servo LB_Coxa;
Servo LB_Femur;
Servo LB_Tibia;
Servo RF_Coxa;            // THIS SERVO IS HAVING ISSUES
Servo RF_Femur;
Servo RF_Tibia;
Servo RM_Coxa;
Servo RM_Femur;
Servo RM_Tibia;
Servo RB_Coxa;
Servo RB_Femur;
Servo RB_Tibia;

Servo servo_array[18];

/** Inital Positions */
#define LFC_INITPOS 90 // 0
#define LFF_INITPOS 48
#define LFT_INITPOS 77

#define LMC_INITPOS 127 // 3
#define LMF_INITPOS 52
#define LMT_INITPOS 115

#define LBC_INITPOS 130 // 6
#define LBF_INITPOS 10
#define LBT_INITPOS 100

#define RFC_INITPOS 90    // 9 // @TODO THIS SERVO IS HAVING ISSUES
#define RFF_INITPOS 155
#define RFT_INITPOS 65

#define RMC_INITPOS 123 // 12
#define RMF_INITPOS 180
#define RMT_INITPOS 98

#define RBC_INITPOS 95 // 15
#define RBF_INITPOS 180
#define RBT_INITPOS 95

/** Ardunio Setup */
void setup() {
  Serial.begin(9600);
  Serial.println("Antdroid starting....");

  /** Attach and initialize the servos */
  initializeServo(LF_Coxa, LFC_PIN, LFC_INITPOS);
  initializeServo(LF_Femur, LFF_PIN, LFF_INITPOS);
  initializeServo(LF_Tibia, LFT_PIN, LFT_INITPOS);
  initializeServo(LM_Coxa, LMC_PIN, LMC_INITPOS);
  initializeServo(LM_Femur, LMF_PIN, LMF_INITPOS);
  initializeServo(LM_Tibia, LMT_PIN, LMT_INITPOS);
  initializeServo(LB_Coxa, LBC_PIN, LBC_INITPOS);
  initializeServo(LB_Femur, LBF_PIN, LBF_INITPOS);
  initializeServo(LB_Tibia, LBT_PIN, LBT_INITPOS);
  //initializeServo(RF_Coxa, RFC_PIN, RFC_INITPOS); // @TODO THIS SERVO IS HAVING ISSUES
  initializeServo(RF_Femur, RFF_PIN, RFF_INITPOS);
  initializeServo(RF_Tibia, RFT_PIN, RFT_INITPOS);
  initializeServo(RM_Coxa, RMC_PIN, RMC_INITPOS);
  initializeServo(RM_Femur, RMF_PIN, RMF_INITPOS);
  initializeServo(RM_Tibia, RMT_PIN, RMT_INITPOS);
  initializeServo(RB_Coxa, RBC_PIN, RBC_INITPOS);
  initializeServo(RB_Femur, RBF_PIN, RBF_INITPOS);
  initializeServo(RB_Tibia, RBT_PIN, RBT_INITPOS);

  buildArray();

  Serial.println("Setup complete...");
  /** Done setup */

  MotionPrepareForStand();

  delay(1000);

  MotionUpTouchGround();

  delay(1000);

  MotionStandUpright();

  delay(1000);
}

/** Arduino loop */
void loop() {
  // Wait for serial commands to execute
  while(Serial.available() > 0 ) {
    String str = Serial.readString();
    stringSet(str);
  }
}

/** Fill servo_array with servos */
void buildArray() {
  servo_array[0] = LF_Coxa;
  servo_array[1] = LF_Femur;
  servo_array[2] = LF_Tibia;
  servo_array[3] = LM_Coxa;
  servo_array[4] = LM_Femur;
  servo_array[5] = LM_Tibia;
  servo_array[6] = LB_Coxa;
  servo_array[7] = LB_Femur;
  servo_array[8] = LB_Tibia;
  servo_array[9] = RF_Coxa;
  servo_array[10] = RF_Femur;
  servo_array[11] = RF_Tibia;
  servo_array[12] = RM_Coxa;
  servo_array[13] = RM_Femur;
  servo_array[14] = RM_Tibia;
  servo_array[15] = RB_Coxa;
  servo_array[16] = RB_Femur;
  servo_array[17] = RB_Tibia; 
}

/**************** START MOTION FUNCTIONS ********************/

// Dont use this - its just bad
void MotionStandV1() {
  Serial.println("Stand()");
  int femurOffset = 0;
  int tibiaOffset = 10;

  // Move femur's
  for(int i = 0; i < femurOffset; i++) {
    SetFemurs(i);
  }

  for(int i = 0; i < tibiaOffset; i++) {
    SetTibias(i);
  }
}

/** Move outer legs out */
void MotionPrepareForStand() {
  Serial.println("MotionPrepareForStand()");
  
  smoothSet(servo_array[0], 150);
  smoothSet(servo_array[6], 100);
  // smoothSet(servo_array[9], 0); // BAD SERVO
  smoothSet(servo_array[15], 130);
}

/** All legs touch the ground. Not suitable for standing */
void MotionTouchGround() {
  Serial.println("MotionTouchGround()");
  
  SetTibias(15);
  SetFemurs(20);
}

/** All legs touch the ground. Suitable for standing */
void MotionUpTouchGround() {
  Serial.println("MotionUpTouchGround()");
  
  SetTibias(40);
}

/** Touch the ground, then move Femurs to push off the ground */
void MotionStandUpright() {
  Serial.println("MotionStandUpright()");
  
  MotionUpTouchGround();
  SetFemurs(25);
}

/***************** END MOTION FUNCTIONS *********************/
void SetFemurs(int pos) {
  Serial.println("SetFemurs(). Pos: " + (String)pos);
  
  for(int i = 0; i < pos; i++) {
     // Left
    justSet(LF_Femur, LFF_INITPOS + i);
    justSet(LM_Femur, LMF_INITPOS + i);
    justSet(LB_Femur, LBF_INITPOS + i);

    // Right
    justSet(RF_Femur, RFF_INITPOS - i);
    justSet(RM_Femur, RMF_INITPOS - i);
    justSet(RB_Femur, RBF_INITPOS - i);

    delay(SERVO_WAIT_TIME);
  }
}

void SetTibias(int pos) {
  Serial.println("SetTibias(). Pos: " + (String)pos);
  
  for(int i = 0; i < pos; i++) {
    // Left
    justSet(LF_Tibia, LFT_INITPOS + i);
    justSet(LM_Tibia, LMT_INITPOS + i);
    justSet(LB_Tibia, LBT_INITPOS + i);

    // Right
    justSet(RF_Tibia, RFT_INITPOS - i);
    justSet(RM_Tibia, RMT_INITPOS - i);
    justSet(RB_Tibia, RBT_INITPOS - i);

    delay(SERVO_WAIT_TIME);
  }
}

/** Return to initial resting position */
void ReturnToInitial() {
  Serial.println("ReturnToInital()");
  
  smoothSet(LF_Coxa, LFC_INITPOS);
  smoothSet(LF_Femur, LFF_INITPOS);
  smoothSet(LF_Tibia, LFT_INITPOS);
  smoothSet(LM_Coxa, LMC_INITPOS);
  smoothSet(LM_Femur, LMF_INITPOS);
  smoothSet(LM_Tibia, LMT_INITPOS);
  smoothSet(LB_Coxa, LBC_INITPOS);
  smoothSet(LB_Femur, LBF_INITPOS);
  smoothSet(LB_Tibia, LBT_INITPOS);
  // smoothSet(RF_Coxa, RFC_INITPOS); // @TODO THIS SERVO IS HAVING ISSUES
  smoothSet(RF_Femur, RFF_INITPOS);
  smoothSet(RF_Tibia, RFT_INITPOS);
  smoothSet(RM_Coxa, RMC_INITPOS);
  smoothSet(RM_Femur, RMF_INITPOS);
  smoothSet(RM_Tibia, RMT_INITPOS);
  smoothSet(RB_Coxa, RBC_INITPOS);
  smoothSet(RB_Femur, RBF_INITPOS);
  smoothSet(RB_Tibia, RBT_INITPOS);
}

// pin,pos
void stringSet(String input) {
  String _servo = getValue(input, ',', 0);
  String _pos   = getValue(input, ',', 1);

  int pos   = _pos.toInt();

  Serial.println("servo_array[" + _servo + "], pos: " + (String)pos);

  if(_servo == "t") {
    SetTibias(pos);
  } 
  else if(_servo == "f") {
    SetFemurs(pos);
  } 
  else {
    int servo = _servo.toInt();
    smoothSet(servo_array[servo], pos);
  }

}

// https://stackoverflow.com/questions/9072320/split-string-into-string-array
String getValue(String data, char separator, int index)
{
  int found = 0;
  int strIndex[] = {0, -1};
  int maxIndex = data.length()-1;

  for(int i=0; i<=maxIndex && found<=index; i++){
    if(data.charAt(i)==separator || i==maxIndex){
        found++;
        strIndex[0] = strIndex[1]+1;
        strIndex[1] = (i == maxIndex) ? i+1 : i;
    }
  }

  return found>index ? data.substring(strIndex[0], strIndex[1]) : "";
}

/** attach the servo and write the initial position */
void initializeServo(Servo _servo, int pinNumber, int initialPos) {
  Serial.println("Initializing servo " + (String)pinNumber + " to position " + (String)initialPos);
  _servo.attach(pinNumber);
  _servo.write(initialPos);
  delay(SERVO_SETUP_DELAY);
}

/** 
 *  Smoothly move servo from last position to new position in steps
 *  Uses default SERVO_WAIT_TIME for wait variable
 *  
 *  @param s    Servo to set
 *  @param pos  New position to travel to
 */
void smoothSet(Servo s, int pos) {
  smoothSet(s, pos, SERVO_WAIT_TIME);
}

void justSet(Servo s, int pos) {
  Serial.println("justSet(): " + (String)pos);
  s.write(pos);
}


/**
 * Smoothly move servo from last position to new position in steps
 * 
 * @param s     Servo to set
 * @param pos   New position to travel to
 * @param wait  Delay between each increment. Sets the speed of the motion
 */
void smoothSet(Servo s, int pos, int wait) {
  int current = s.read();
  int diff = pos - current;

  Serial.println("Moving servo. Current: " + (String)current + ", New: " + (String)pos);
  
  if(diff > 0) {
    
    for(int i = current; i <= pos; i++) {
      s.write(i);
      delay(wait);
    }
  } else {
    for(int i = current; i >= pos; i--) {
      s.write(i);
      delay(wait);
    }
  }
}