More Example Code SparkFun Robotic Arm

February 23, 2013
Tags: ,

I have continued to tweak the code for the robot demo. I call this code Robot Instructions. It allows me to call two different functions. The first function moves the arm very quickly and the second function moves the arm slowly. This is for the Arudino Uno with a three servo robotic arm sold from SparkFun.com.

/*
Robot instructions
Brad Poulton
*/

#include <servo.h>

Servo servo1; //base-pan
Servo servo2; //arm-tilt
Servo servo3; //claw

void setup()
{
  Serial.begin(9600);
  servo1.attach(9);  //pan range is 20 to 140
  servo2.attach(10); //tilt range is 0 to 179
  servo3.attach(11); //claw range is 70 to 130
}

void loop()
{
  //Put your instructions here
  //DANCE Moves
  //robotInst(100,20,120,10);
  //robotInst(80,65,100,10);
  //robotInst(140,160,70,10);

  //move a BLOCK
  robotInst(130,65,70,25);
  robotMove(130,165,70);
  robotInst(130,165,120,25);
  robotMove(50,165,120);
  robotMove(50,130,70);

}

void robotMove(int a, int b, int c)
{
  //move the servos fast!
  servo1.write(a);
  delay(25);
  servo2.write(b);
  delay(25);
  servo3.write(c);
  delay(25);

}

void robotInst(int a, int b, int c, int d)
{
  int pos;
  ////////
  //
  //PAN
  //
  ///////
  if (a < servo1.read()){     
    for(pos = servo1.read(); pos > a; pos -= 1)
    {
      servo1.write(pos);
      Serial.print("PanNum: ");
      Serial.println(pos);

      delay(d);
    }

  } else if (a > servo1.read()){
    for(pos = servo1.read(); pos < a; pos += 1)
    {
      servo1.write(pos);
      Serial.print("PanNum: ");
      Serial.println(pos);

      delay(d);
    }

  } else {

    Serial.println("equals");
  }

  ////////
  //
  //TILT
  //
  ///////
  if (b < servo2.read()){     
    for(pos = servo2.read(); pos > b; pos -= 1)
    {
      servo2.write(pos);
      Serial.print("TiltNum: ");
      Serial.println(pos);

      delay(d);
    }

  } else if (b > servo2.read()){
    for(pos = servo2.read(); pos < b; pos += 1)
    {
      servo2.write(pos);
      Serial.print("TiltNum: ");
      Serial.println(pos);

      delay(d);
    }

  } else {

    Serial.println("equals");
  }

  ////////
  //
  //Claw
  //
  ///////
  if (c < servo3.read()){     
    for(pos = servo3.read(); pos > c; pos -= 1)
    {
      servo3.write(pos);
      Serial.print("ClawNum: ");
      Serial.println(pos);

      delay(d);
    }

  } else if (c > servo3.read()){
    for(pos = servo3.read(); pos < c; pos += 1)
    {
      servo3.write(pos);
      Serial.print("ClawNum: ");
      Serial.println(pos);

      delay(d);
    }

  } else {

    Serial.println("equals");
  }

  Serial.print("ClawNum Final: ");
  Serial.println(pos);

  Serial.print("Servos: ");
  Serial.print(servo1.read());
  Serial.print(" ");
  Serial.print(servo2.read());
  Serial.print(" ");
  Serial.println(servo3.read());
}

 

One Response to “More Example Code SparkFun Robotic Arm”

  1. […] More Example Code SparkFun Robotic Arm […]

Leave a Reply