4 DOF Robot Arm - Holding a pen horizontal

how do I write a for loop for this function (in void setup) , that is to move wrist according to elbow and shoulder angles. I need help with the for loop. my code below does not achieve the desired function.

Im using the robot geek snapper arm for a class project.

As a pre-project assignment the robot gripper needs to hold a pen horizontally as other joints are moving.

Because the tip (gripper) will grab a pen and draw lines (in the project), the gripper’s orientation will have only two orientations: 1) the gripper has to be -90 (if defined as the figure above) to grab the pen on the table; 2) the gripper has to be horizontally oriented so that the pen will be vertically held while drawing lines.

When (Elbow + Shoulder) < -90, if you use the above angle definition,

Wrist = -90 - (Elbow + Shoulder)

Write a program to move Shoulder from -30 to 0 and Elbow -30 to 0, Wrist is determined by the above equation. Base can be any value.

You can use two for-loop commands. You will be able to observe the shoulder and elbow move “forward”, and the gripper stays “head-down” all the time

This is what I have so far…

#include <Servo.h>

#define ROBOT_GEEK_PARALLEL_GRIPPER 2

//The Parrallel gripper has a full robotgeek servo and paralle rails
#define GRIPPER_TYPE ROBOT_GEEK_PARALLEL_GRIPPER

#define DELAY_TIME 2  //milliseconds to wait between

//define analog pins that will be connected to the joystick pins
#define BASE      0  //connected to Rotation Knob / Potentiometer # 1
#define SHOULDER  1  //connected to Vertical Axis on Joystick # 1
#define ELBOW     2  //connected to Vertical Axis on Joystick # 2
#define WRIST     3  //connected to Vertical Axis on Joystick # 3
#define GRIPPER   4  //connected to Rotation Knob / Potentiometer # 2

// Servo position limitations - limits in microseconds
#define BASE_MIN      600     //full counterclockwise for RobotGeek 180 degree servo
#define BASE_MAX      2400    //full clockwise for RobotGeek 180 degree servo
#define SHOULDER_MIN  550
#define SHOULDER_MAX  2350
#define ELBOW_MIN     550
#define ELBOW_MAX     2350
#define WRIST_MIN     600
#define WRIST_MAX     2400 

//mins and maxes depending on gripper type
#define GRIPPER_MIN   750   //fully closed
#define GRIPPER_MAX   2400  //fully open


#define CENTERED      1500


//generic deadband limits - not all joystics will center at 512, so these limits remove 'drift' from joysticks that are off-center.
#define DEADBANDLOW 462 //decrease this value if drift occurs, increase it to increase sensitivity around the center position
#define DEADBANDHIGH 562  //increase this value if drift occurs, decrease it to increase sensitivity around the center position

// Declare servo objects
Servo BAS_SERVO;  //base servo - RobotGeek Servo
Servo SHL_SERVO;  //shoulder servo - RobotGeek Servo 
Servo ELB_SERVO;  //elbow servo - RobotGeek Servo 
Servo WRI_SERVO;  //wrist servo - RobotGeek Servo
Servo WRO_SERVO;  //wrist rotate servo - RobotGeek Servo (unused for snapper arm)        
Servo GRI_SERVO;  //gripper servo - 9g servo

//present positions of the servos 
int Base     = 1500;  //holds the present position of the Base servo, starts at 1500 (centered)
int Shoulder = 1450;  //holds the present position of the Shoulder servo, starts at 1500 (centered)
int Elbow    = 1450;  //holds the present position of the Elbow servo, starts at 1500 (centered)
int Wrist    = 1500;  //holds the present position of the wrist servo, starts at 1500 (centered)
int Gripper  = 1500;  //holds the present position of the gripper servo, starts at 1500 (centered)

//last read values of analog sensors (Native values, 0-1023)
int joyBaseVal = 0;     //present value of the base rotation knob (analog 0)
int joyShoulderVal = 0; //present value of the shoulder joystick (analog 1)
int joyElbowVal = 0;    //present value of the elbow joystick (analog 2)
int joyWristVal = 0;    //present value of the wrist joystick (analog 3)
int joyGripperVal = 0;  //present value of the gripper rotation knob (analog 4)

//last calculated values of analog sensors (Mapped values)
//knob values (base and gripper) will be mapped directly to the servo limits
//joystick values (shoulder, elbow and wrist) will be mapped from -speed to speed, to faciliate incremental control
int joyBaseMapped = 0;      //base knob value, mapped from 1-1023 to BASE_MIN-BASE_MAX
int joyShoulderMapped = 0;  //shoulder joystick value, mapped from 1-1023 to -speed to speed
int joyElbowMapped = 0;     //elbow joystick value, mapped from 1-1023 to -speed to speed
int joyWristMapped = 0;     //wrist joystick value, mapped from 1-1023 to -speed to speed
int joyGripperMapped = 0;   //gripper knob  value, mapped from 1-1023 to GRIPPER_MIN-GRIPPER_MAX

int speed = 10;  //speed modififer, increase this to increase the speed of the movement


void setup() 
{
  
// Attach servo and set limits
  BAS_SERVO.attach(3, BASE_MIN, BASE_MAX);
  SHL_SERVO.attach(5, SHOULDER_MIN, SHOULDER_MAX);
  ELB_SERVO.attach(6, ELBOW_MIN, ELBOW_MAX);
  WRI_SERVO.attach(9, WRIST_MIN, WRIST_MAX);
  GRI_SERVO.attach(10, GRIPPER_MIN, GRIPPER_MAX);
  delay(1000);  //wait 1 second
  
  set_servo();  // Move servos to default positions
  delay(1000);

moveServo(SHL_SERVO, Shoulder, Shoulder-300),  // moving to initial position of 30 degrees
moveServo(ELB_SERVO, Elbow, Elbow-300);

  for (int i = (Elbow+Shoulder); i<-900 ; i++)
  {
   Wrist = -900 - i ;
   moveServo(WRI_SERVO, 1500, Wrist); 
  }

moveServo(SHL_SERVO, Shoulder-300, Shoulder),
moveServo(ELB_SERVO, Elbow-300, Elbow);

}

void loop() 
{
  
}

void moveServo(Servo servo, int initialPosition, int finalPosition)
  {
    for(int i = initialPosition; i<finalPosition; i = i+1)
    {
      servo.writeMicroseconds(i);
      delay(DELAY_TIME);
    }

    for(int i = initialPosition; i>finalPosition; i = i-1)
    {
      servo.writeMicroseconds(i);
      delay(DELAY_TIME);
    }
  }


void set_servo()
{
  delay(1000);
  BAS_SERVO.writeMicroseconds(Base);
  delay(1000);
  WRI_SERVO.writeMicroseconds(Wrist);
  delay(1000);
  SHL_SERVO.writeMicroseconds(Shoulder);
  delay(1000);
  ELB_SERVO.writeMicroseconds(Elbow);
  delay(1000);
  GRI_SERVO.writeMicroseconds(Gripper);
  delay(1000);
}

Do you have question ?

yes how do I write a for loop for this function, that is to move wrist according to elbow and shoulder angles.

the code I have right now does not work.

Please edit your post to enclose the code in code tags.

Explain what “does not work” means.

I need help getting my code to achieve desired function. What I have so far is not moving the wrist at all. The for loop I have is doing nothing. the arm just moves from point to point and for loop is not executing.

Question: how do I write a for loop (suggestion to use 2) for this function, that is to move wrist according to elbow and shoulder angles. I need help with the ‘for’ loop. my code below does not achieve the desired function. it actually makes wrist vertical instead of horizontal.

Im using the robot geek snapper arm for a this project.

For the assignment the robot gripper needs to hold a pen, so the wrist needs to stay horizontal as other joints are moving.

This is what I have so far…

#include <Servo.h>

#define ROBOT_GEEK_PARALLEL_GRIPPER 2

//The Parrallel gripper has a full robotgeek servo and paralle rails
#define GRIPPER_TYPE ROBOT_GEEK_PARALLEL_GRIPPER

#define DELAY_TIME 2  //milliseconds to wait between

//define analog pins that will be connected to the joystick pins
#define BASE      0  //connected to Rotation Knob / Potentiometer # 1
#define SHOULDER  1  //connected to Vertical Axis on Joystick # 1
#define ELBOW     2  //connected to Vertical Axis on Joystick # 2
#define WRIST     3  //connected to Vertical Axis on Joystick # 3
#define GRIPPER   4  //connected to Rotation Knob / Potentiometer # 2

// Servo position limitations - limits in microseconds
#define BASE_MIN      600     //full counterclockwise for RobotGeek 180 degree servo
#define BASE_MAX      2400    //full clockwise for RobotGeek 180 degree servo
#define SHOULDER_MIN  600
#define SHOULDER_MAX  1800
#define ELBOW_MIN     1200
#define ELBOW_MAX     2400
#define WRIST_MIN     600
#define WRIST_MAX     2400 

//mins and maxes depending on gripper type
#define GRIPPER_MIN   750   //fully closed
#define GRIPPER_MAX   2400  //fully open


#define CENTERED      1500


//generic deadband limits - not all joystics will center at 512, so these limits remove 'drift' from joysticks that are off-center.
#define DEADBANDLOW 462 //decrease this value if drift occurs, increase it to increase sensitivity around the center position
#define DEADBANDHIGH 562  //increase this value if drift occurs, decrease it to increase sensitivity around the center position

// Declare servo objects
Servo BAS_SERVO;  //base servo - RobotGeek Servo
Servo SHL_SERVO;  //shoulder servo - RobotGeek Servo 
Servo ELB_SERVO;  //elbow servo - RobotGeek Servo 
Servo WRI_SERVO;  //wrist servo - RobotGeek Servo
Servo WRO_SERVO;  //wrist rotate servo - RobotGeek Servo (unused for snapper arm)        
Servo GRI_SERVO;  //gripper servo - 9g servo

//present positions of the servos 
int Base     = 1500;  //holds the present position of the Base servo, starts at 1500 (centered)
int Shoulder = 1500;  //holds the present position of the Shoulder servo, starts at 1500 (centered)
int Elbow    = 1500;  //holds the present position of the Elbow servo, starts at 1500 (centered)
int Wrist    = 1500;  //holds the present position of the wrist servo, starts at 1500 (centered)
int Gripper  = 1500;  //holds the present position of the gripper servo, starts at 1500 (centered)
int WristDegree = 0;

//last read values of analog sensors (Native values, 0-1023)
int joyBaseVal = 0;     //present value of the base rotation knob (analog 0)
int joyShoulderVal = 0; //present value of the shoulder joystick (analog 1)
int joyElbowVal = 0;    //present value of the elbow joystick (analog 2)
int joyWristVal = 0;    //present value of the wrist joystick (analog 3)
int joyGripperVal = 0;  //present value of the gripper rotation knob (analog 4)

//last calculated values of analog sensors (Mapped values)
//knob values (base and gripper) will be mapped directly to the servo limits
//joystick values (shoulder, elbow and wrist) will be mapped from -speed to speed, to faciliate incremental control
int joyBaseMapped = 0;      //base knob value, mapped from 1-1023 to BASE_MIN-BASE_MAX
int joyShoulderMapped = 0;  //shoulder joystick value, mapped from 1-1023 to -speed to speed
int joyElbowMapped = 0;     //elbow joystick value, mapped from 1-1023 to -speed to speed
int joyWristMapped = 0;     //wrist joystick value, mapped from 1-1023 to -speed to speed
int joyGripperMapped = 0;   //gripper knob  value, mapped from 1-1023 to GRIPPER_MIN-GRIPPER_MAX

int speed = 10;  //speed modififer, increase this to increase the speed of the movement

//============================================================================
// Setup 
//============================================================================

void setup() 
{
  
// Attach servo and set limits
  BAS_SERVO.attach(3, BASE_MIN, BASE_MAX);
  SHL_SERVO.attach(5, SHOULDER_MIN, SHOULDER_MAX);
  ELB_SERVO.attach(6, ELBOW_MIN, ELBOW_MAX);
  WRI_SERVO.attach(9, WRIST_MIN, WRIST_MAX);
  GRI_SERVO.attach(10, GRIPPER_MIN, GRIPPER_MAX);
  delay(1000);  //wait 1 second
  
  set_servo();  // Move servos to default positions
  delay(1000);
  
//initial position of -30 degrees

moveServo(SHL_SERVO, Shoulder, Shoulder-300);
moveServo(ELB_SERVO, Elbow, Elbow-300);

// for loop moving from -30 to 0 degrees

  for (int i = 6; i>0 ; i--)
  {
   moveServo(SHL_SERVO, Shoulder-i*50, Shoulder-(i-1)*50);
   moveServo(ELB_SERVO, Elbow-i*50, Elbow-(i-1)*50);
   
   WristDegree = -900 - ((Shoulder-(i-1)*50) + (Elbow-(i-1)*50)) ;
   moveServo(WRI_SERVO, Wrist, WristDegree); 
  }


}

void loop() 
{
  
}

void moveServo(Servo servo, int initialPosition, int finalPosition)
  {
    for(int i = initialPosition; i<finalPosition; i = i+1)
    {
      servo.writeMicroseconds(i);
      delay(DELAY_TIME);
    }

    for(int i = initialPosition; i>finalPosition; i = i-1)
    {
      servo.writeMicroseconds(i);
      delay(DELAY_TIME);
    }
  }


void set_servo()
{
  delay(1000);
  BAS_SERVO.writeMicroseconds(Base);
  delay(1000);
  WRI_SERVO.writeMicroseconds(Wrist);
  delay(1000);
  SHL_SERVO.writeMicroseconds(Shoulder);
  delay(1000);
  ELB_SERVO.writeMicroseconds(Elbow);
  delay(1000);
  GRI_SERVO.writeMicroseconds(Gripper);
  delay(1000);
}

Your duplicate topics have been merged

Cross-posting is against the rules of the forum. The reason is that duplicate posts can waste the time of the people trying to help. Someone might spend 15 minutes (or more) writing a detailed answer on this topic, without knowing that someone else already did the same in the other topic.

Repeated cross-posting will result in a timeout from the forum.

In the future, please take some time to pick the forum board that best suits the topic of your question and then only post once to that forum board. This is basic forum etiquette, as explained in the sticky “How to use this forum - please read.” post you will find at the top of every forum board. It contains a lot of other useful information. Please read it.

Thanks in advance for your cooperation.

If you want a loop from -30 to 0, why are you counting from 6 to 1 and multiplying by 50? Doesn’t that give you +300 to 50 in steps of 50?

I would think you woud start with something like:

 // for loop moving from -30 to 0 degrees

  for (int Degrees = -30; Degrees <= 0 ; Degrees++)
  {
    int uS = map(Degrees, -90, 90, 1000, 2000); // Convert degrees to microseconds
    Shoulder = uS;
    Elbow = uS;
    SHL_SERVO.writeMicroseconds(Shoulder);
    ELB_SERVO.writeMicroseconds(Elbow);

    int WristDegrees = -90 - (ShoulderDegrees + ShoulderDegrees);
    WristDegrees = constrain(WristDegrees, -90, 90);
    Wrist = map(WristDegrees, -90, 90, 1000, 2000); // Convert degrees to microseconds

    WRI_SERVO.writeMicroseconds(Wrist);
    delay(20);
  }

This topic was automatically closed 120 days after the last reply. New replies are no longer allowed.