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