ArduinoCode

/* Used to (hopefully) communicate with both arms and head on our robot */


#include "Arduino.h"
#include "MeccaBrain.h"
#include <ros.h>
#include <std_msgs/UInt16.h>
#include <std_msgs/Empty.h>
#include <std_msgs/String.h>
#include <std_msgs/MultiArrayLayout.h>
#include <std_msgs/MultiArrayDimension.h>
#include <std_msgs/Int8MultiArray.h>

ros::NodeHandle  nh;

const int pwnPin1 = 9; //left arm
const int pwnPin2 = 10; //right arm
const int pwnPin3 = 11; //head
MeccaBrain chain1(pwnPin1);
MeccaBrain chain2(pwnPin2);
MeccaBrain headChain(pwnPin3);




//Stuff for headpositioning
int servoPin = 11;
MeccaBrain mb = MeccaBrain(servoPin);
byte positions[2] = {0x77, 0x97}; // 0x00 = full clockwise, 0xEF = full counter clockwise
byte currentPositions[2] = {0x77, 0x97};
byte ledColor[] = {0x07,0x00,0x00,0x00}; // r,g,b,fade
int splits = 20;


//Callback function - when subscriber recieves an "Empty" message, execute the following
void servo_cb( const std_msgs::Empty& cmd_msg){

   //Wave-routine for arm one
   /*raiseShoulderTwo();
   delay(500);
   elbow();

   //swipeE();
   delay(500);

   //This should wave 3 times
   wave();
   delay(500);
   lowerShoulder();
   delay(500);

   */

   //Wave-routine for arm two
    raiseShoulderTwo();
    delay(500);
    elbowTwo();
    delay(500);
    //This should wave 3 times
    waveTwo();
    delay(500);
    lowerShoulderTwo();
    delay(500);


    //Moves head
    moveHead();
    delay(500);

    waveDone();
    //Toggles the LED
    digitalWrite(13, HIGH-digitalRead(13));  //toggle led
}

void positionCallback(const std_msgs::Int8MultiArray& position_msg){
  positions[0] = position_msg.data[0];
  positions[1] = position_msg.data[1];

  if(positions[0] == 0x77 && positions[1] == 0x97){
     mb.setLEDColor(7,0,0,0);
  }else{
     mb.setLEDColor(0,7,0,0);
  }

  mb.setServoPosition(0,position_msg.data[0]);
  mb.setServoPosition(1,position_msg.data[1]);
  mb.communicate();
  /*
  byte diff = abs(positions[0] - currentPositions[0])/20.0;
  int i;
  for(i=0; i<splits; i++){
    mb.setServoPosition(0,currentPositions[0]+(diff*(i+1)));
    mb.communicate();
    delay(20);
  }

  currentPositions[0] = position_msg.data[0];
  currentPositions[1] = position_msg.data[1];
  */
 //setInCenterPosition();
}

void setInCenterPosition(){
  mb.setServoPosition(0,0x77);
  mb.setServoPosition(1,0x77);
  mb.communicate();
}

std_msgs::Empty empty_msg;
ros::Publisher waveDonePub("/wave_done", &empty_msg);

ros::Subscriber<std_msgs::Int8MultiArray> sub("/face_location_pos", positionCallback );

ros::Subscriber<std_msgs::Empty> armSub("servo", servo_cb);

//Used for sweep
int pos = 0;

void setup(){
  pinMode(13, OUTPUT);
  nh.initNode();
  setInCenterPosition();
  nh.subscribe(sub);
  nh.subscribe(armSub);
  nh.advertise(waveDonePub);
}

void loop(){
  nh.spinOnce();
  delay(1);
}



//-----------------------------------------------------------------------------


//Moves the head
void moveHead() {
  int pos = 0;
    //setToLIMAndGetPos(0)

    for (pos = 90; pos <= 128; pos += 10) {
      headChain.setServoPosition(1,pos);
      headChain.communicate();
      delay(20);
    }


    // Why do we have to similar for-loops?

     for (pos = 0; pos <= 128; pos += 10) {
      headChain.setServoPosition(1,pos);
      headChain.communicate();
      delay(20);
    }



    //RÖD
    headChain.setServoColor(0, 0xF1);
    headChain.communicate();
    delay(500);
}

void waveDone() {
  //std_msgs::Empty empty_msg;
  waveDonePub.publish( &empty_msg);
  delay(100);
}



void swipeE() {
  int pos = 0;
    //setToLIMAndGetPos(0)

    for (pos = 0; pos <= 255; pos += 10) {
      chain1.setServoPosition(1,pos);
      chain1.communicate();
      delay(20);
    }
    for (pos = 255; pos >= 0; pos -= 10) {
      chain1.setServoPosition(1,pos);
      chain1.communicate();
      delay(20);
      }
      }


//Wave-function för arm 1
void raiseShoulder() {

    int pos = 0;
    //setToLIMAndGetPos(0)

    for (pos = 0; pos <= 128; pos += 10) {
      chain1.setServoPosition(0,pos);
      chain1.communicate();
      delay(20);
    }

    //RÖD
    chain1.setServoColor(0, 0xF1);
    chain1.communicate();
    delay(500);
}

void elbow() {

    int pos = 0;

    //chain = c;
    for (pos = setToLIMAndGetPos(1); pos <= 128; pos += 10) {
      chain1.setServoPosition(1, pos);
      chain1.communicate();
      delay(10);
    }

    //Någon oklar färg gul
    chain1.setServoColor(1, 0xF3);
    chain1.communicate();
    delay(500);
}

void wave() {

  int pos = 0;
  int i = 0;
  for (i = 0; i <= 2; i++) {
    for (pos = 128; pos >= 0; pos -= 10) {
      chain1.setServoPosition(2, pos);
      chain1.communicate();
      delay(15);
    }

    for (pos = 0; pos <= 128; pos += 10) {
      chain1.setServoPosition(2, pos);
      chain1.communicate();
      delay(15);
    }
    //delay(200);
  }

  //Lila
  chain1.setServoColor(2, 0xF5);
  chain1.communicate();
  delay(500);

}


void lowerShoulder() {

  int pos1 = 0;
  int pos2 = 0;
  int pos3 = 0;

  for (pos1 = setToLIMAndGetPos(2); pos1 <= 128; pos1 += 10) {
     chain1.setServoPosition(2, pos1);
     chain1.communicate();
     delay(20);
  }
  //128
  //setToLIMAndGetPos(1)
  for (pos2 = setToLIMAndGetPos(1); pos2 >= 0; pos2 -= 10) {
     chain1.setServoPosition(1, pos2);
     chain1.communicate();
     delay(20);
  }

  //128
  for (pos3 = setToLIMAndGetPos(0); pos3 <= 255; pos3 +=10) {

    chain1.setServoPosition(0, pos3);
    chain1.communicate();
    delay(20);
  }

  chain1.setServoColor(0, 0xF7);
  chain1.setServoColor(1, 0xF7);
  chain1.setServoColor(2, 0xF7);
  chain1.communicate();
  delay(500);
  //c = c;
  /* Gammal kod
  chain1.setServoPosition(2,128);
  chain1.communicate();
  delay(600);
  chain1.setServoPosition(1,24);
  chain1.communicate();
  delay(600);
  chain1.setServoPosition(0,232);
  chain1.communicate();
  delay(600);
  */
}

//Sets servo to LIM , allowing it to return its position. Then calls getServoPosition.
int setToLIMAndGetPos (int servoNum) {
  chain1.setServotoLIM(servoNum);
  int pos = chain1.getServoPosition(servoNum);
  chain1.communicate();
  delay(100);
  return pos;
}

//------------------------------------------------------
//Andra armen

void raiseShoulderTwo() {

    int pos = 255;
    //setToLIMAndGetPos(0)

    for (pos = 255; pos >= 128; pos -= 10) {
      chain2.setServoPosition(0,pos);
      chain2.communicate();
      //delay(10);
    }

    //RÖD
    chain2.setServoColor(0, 0xF1);
    chain2.communicate();
    delay(500);
}



void elbowTwo() {

    int pos = 0;

    //chain = c;
    for (pos = setToLIMAndGetPos(1); pos <= 150; pos += 20) {
      chain2.setServoPosition(1, pos);
      chain2.communicate();
      delay(10);
    }

    //Någon oklar färg
    chain2.setServoColor(1, 0xF3);
    chain2.communicate();
    delay(500);
}

void waveTwo() {

  int pos = 255;
  int i = 0;
  //for (i = 0; i <= 1; i++) {
    for (pos = 128; pos <= 255; pos += 20) {
      chain2.setServoPosition(2, pos);
      chain2.communicate();
      delay(10);
    }

    for (pos = 255; pos >= 128; pos -= 20) {
      chain2.setServoPosition(2, pos);
      chain2.communicate();
      delay(10);
    }
    //delay(200);
  //}

  //Lila
  chain2.setServoColor(2, 0xF5);
  chain2.communicate();
  //delay(500);

}


void lowerShoulderTwo() {

  int pos1 = 0; // Servo two
  int pos2 = 0; // Servo 1
  int pos3 = 0; // Servo 0

  for (pos1 = setToLIMAndGetPos(2); pos1 >= 128; pos1 -= 20) {
     chain2.setServoPosition(2, pos1);
     chain2.communicate();
     //delay(10);
  }
  //128 change to 255 resp 0 on servo 0 and 1
  //setToLIMAndGetPos(1)
  for (pos2 = 128; pos2 <= 255; pos2 += 20) {
     chain2.setServoPosition(1, pos2);
     chain2.communicate();
     //delay(10);
  }

  //128
  for (pos3 = setToLIMAndGetPos(0); pos3 >= 0; pos3 -=20) {

    chain2.setServoPosition(0, pos3);
    chain2.communicate();
    //delay(10);
  }

  chain2.setServoColor(0, 0xF7);
  chain2.setServoColor(1, 0xF7);
  chain2.setServoColor(2, 0xF7);
  chain2.communicate();
  //delay(500);

}