'for' loop code to control robot arm movement

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);
}

A post was merged into an existing topic: 4 DOF Robot Arm - Holding a pen horizontal