Hi,
Thanks for all help,
Id made more changes to the program, now its working pretty well… Think that the code looks a lot now like your example code… For who is interested ill post it here…
Its still far from perfect, so every suggestions are welcome!!
The reason why the controls did not work before was that the Arduino needs to send the serial data in one go to the ssc32, when I realized that things went a lot better.
Robert
[code]#include <SoftwareSerial.h>
#include <PS2X_lib.h>
// Arm dimensions (mm)
#define BASE_HGT 68.0 // Base height to X/Y plane
#define HUMERUS 157.0 // Shoulder-to-elbow
#define ULNA 157.0 // Elbow-to-wrist
#define GRIPPER 73.025 // Gripper length, to middle of grip surface 2.875" (3.375" - 0.5")
// Arduino pin numbers for PS2 controller connections
#define PS2_CLK_PIN 9 // Clock
#define PS2_CMD_PIN 7 // Command
#define PS2_ATT_PIN 8 // Attention
#define PS2_DAT_PIN 6 // Data
// Set physical limits (in degrees) per servo/joint.
// Will vary for each servo/joint, depending on mechanical range of motion.
// The MID setting is the required servo input needed to achieve a
// 90 degree joint angle, to allow compensation for horn misalignment
#define BAS_MIN 0.0 // Fully CCW
#define BAS_MID 90.0
#define BAS_MAX 180.0 // Fully CW
#define SHL_MIN 20.0 // Max forward motion
#define SHL_MID 81.0
#define SHL_MAX 140.0 // Max rearward motion
#define ELB_MIN 20.0 // Max upward motion
#define ELB_MID 88.0
#define ELB_MAX 165.0 // Max downward motion
#define WRI_MIN 0.0 // Max downward motion
#define WRI_MID 93.0
#define WRI_MAX 180.0 // Max upward motion
#define WRO_MIN 0.0 //
#define WRO_MID 90.0
#define WRO_MAX 180.0 //
#define GRI_MIN 25.0 // Fully open
#define GRI_MID 90.0
#define GRI_MAX 165.0 // Fully closed
// Speed adjustment parameters
// Percentages (1.0 = 100%) - applied to all arm movements
#define SPEED_MIN 0.5
#define SPEED_MAX 1.5
#define SPEED_DEFAULT 1.0
#define SPEED_INCREMENT 0.25
#define Y_MIN 100.0 // mm
// PS2 controller characteristics
#define JS_MIDPOINT 128 // Numeric value for joystick midpoint
#define JS_DEADBAND 4 // Ignore movement this close to the center position
#define JS_IK_SCALE 50.0 // Divisor for scaling JS output for IK control
#define JS_SCALE 100.0 // Divisor for scaling JS output for raw servo control
#define Z_INCREMENT 2.0 // Change in Z axis (mm) per button press
#define G_INCREMENT 2.0 // Change in Gripper jaw opening (servo angle) per button press
// Audible feedback sounds
#define TONE_READY 1000 // Hz
#define TONE_IK_ERROR 200 // Hz
#define TONE_DURATION 100 // ms
#define SPK_PIN 5 // Pin on arduino
// IK function return values
#define IK_SUCCESS 0
#define IK_ERROR 1 // Desired position not possible
// Ready-To-Run arm position. See descriptions below
// NOTE: Have the arm near this position before turning on the
// servo power to prevent whiplash
#define READY_X 0.0
#define READY_Y 170.0
#define READY_Z 45.0
#define READY_GA 0.0
#define READY_G GRI_MID
#define READY_WR WRO_MID
#define Deg_to_uS 10 // degree from IK calculations need to be converted to time + an offset of 500uS
#define Offset 600
// Global variables for arm position, and initial settings
float X = READY_X; // Left/right distance (mm) from base centerline - 0 is straight
float Y = READY_Y; // Distance (mm) out from base center
float Z = READY_Z; // Height (mm) from surface (i.e. X/Y plane)
float GA = READY_GA; // Gripper angle. Servo degrees, relative to X/Y plane - 0 is horizontal
float G = READY_G; // Gripper jaw opening. Servo degrees - midpoint is halfway open
float WR = READY_WR; // Wrist Rotate. Servo degrees - midpoint is horizontal
float Speed = SPEED_DEFAULT;
// Pre-calculations
float hum_sq = HUMERUSHUMERUS;
float uln_sq = ULNAULNA;
// PS2 Controller object
PS2X Ps2x;
// Servo objects
int Bas_Servo;
int Shl_Servo;
int Elb_Servo;
int Wri_Servo;
int Gri_Servo;
int Wro_Servo;
//Serial object that represents the SSC-32
SoftwareSerial SSC32 (12, 13);
void setup()
{
SSC32.begin(38400);
Serial.begin(38400);
// Setup PS2 controller. Loop until ready.
byte ps2_stat;
do {
ps2_stat = Ps2x.config_gamepad(PS2_CLK_PIN, PS2_CMD_PIN, PS2_ATT_PIN, PS2_DAT_PIN);
if (ps2_stat == 1)
Serial.println("No controller found. Re-trying ...");
}
while (ps2_stat == 1);
switch (ps2_stat)
{
case 0:
Serial.println(“Found Controller, configured successfully.”);
break;
case 2:
Serial.println(“Controller found but not accepting commands.”);
break;
case 3:
Serial.println("Controller refusing to enter ‘Pressures’ mode, may not support it. ");
break;
}
// Move servos to parking position
// All servos at midpoint
Bas_Servo = (BAS_MID * Deg_to_uS) + Offset;
Shl_Servo = (SHL_MID * Deg_to_uS) + Offset;
Elb_Servo = (ELB_MID * Deg_to_uS) + Offset;
Wri_Servo = (WRI_MID * Deg_to_uS) + Offset;
Gri_Servo = (GRI_MID * Deg_to_uS) + Offset;
Wro_Servo = (WRO_MID * Deg_to_uS) + Offset;
SSC32.print ("#0 P");
SSC32.print (Bas_Servo);
SSC32.print ("#1 P");
SSC32.print (Shl_Servo);
SSC32.print ("#2 P");
SSC32.print (Elb_Servo);
SSC32.print ("#3 P");
SSC32.print (Wri_Servo);
SSC32.print ("#4 P");
SSC32.print (Wro_Servo);
SSC32.print ("#5 P");
SSC32.print (Gri_Servo);
SSC32.println();
delay(500);
// Sound tone to indicate it’s safe to turn on servo power
tone(SPK_PIN, TONE_READY, TONE_DURATION);
delay(TONE_DURATION * 2);
tone(SPK_PIN, TONE_READY, TONE_DURATION);
}
void loop()
{
// Send the arm back to its home position.
if (Ps2x.ButtonPressed(PSB_START))
goto_startposition();
// Speed increase/decrease
if (Ps2x.ButtonPressed(PSB_PAD_UP) || Ps2x.ButtonPressed(PSB_PAD_DOWN))
{
if (Ps2x.ButtonPressed(PSB_PAD_UP))
{
Speed += SPEED_INCREMENT; // increase speed
}
else
{
Speed -= SPEED_INCREMENT; // decrease speed
}
// Constrain to limits
Speed = constrain(Speed, SPEED_MIN, SPEED_MAX);
tone(SPK_PIN, (TONE_READY * Speed), TONE_DURATION);
}
// Store desired position in tmp variables until confirmed by set_arm() logic
float x_tmp = X;
float y_tmp = Y;
float z_tmp = Z;
float ga_tmp = GA;
// Used to indicate whether an input occurred that can move the arm
boolean arm_move = false;
Ps2x.read_gamepad(); //read controller
// Read the left and right joysticks and translate the
// normal range of values (0-255) to zero-centered values (-128 - 128)
int ly_trans = JS_MIDPOINT - Ps2x.Analog(PSS_LY);
int lx_trans = Ps2x.Analog(PSS_LX) - JS_MIDPOINT;
int ry_trans = JS_MIDPOINT - Ps2x.Analog(PSS_RY);
int rx_trans = Ps2x.Analog(PSS_RX) - JS_MIDPOINT;
// X Position (in mm)
// Can be positive or negative. Servo range checking in IK code
if (abs(rx_trans) > JS_DEADBAND)
{
x_tmp += ((float)rx_trans / JS_IK_SCALE * Speed);
arm_move = true;
}
// Y Position (in mm)
// Must be > Y_MIN. Servo range checking in IK code
if (abs(ry_trans) > JS_DEADBAND)
{
y_tmp += ((float)ry_trans / JS_IK_SCALE * Speed);
y_tmp = max(y_tmp, Y_MIN);
arm_move = true;
if (y_tmp == Y_MIN)
{
// Provide audible feedback of reaching limit
tone(SPK_PIN, TONE_IK_ERROR, TONE_DURATION);
}
}
// Z Position (in mm)
// Must be positive. Servo range checking in IK code
if (Ps2x.Button(PSB_R1) || Ps2x.Button(PSB_R2)) {
if (Ps2x.Button(PSB_R1))
{
z_tmp += Z_INCREMENT * Speed; // up
}
else
{
z_tmp -= Z_INCREMENT * Speed; // down
}
z_tmp = max(z_tmp, 0);
arm_move = true;
}
// Gripper angle (in degrees) relative to horizontal
// Can be positive or negative. Servo range checking in IK code
if (abs(ly_trans) > JS_DEADBAND)
{
ga_tmp -= ((float)ly_trans / JS_SCALE * Speed);
arm_move = true;
}
// Wrist rotate (in degrees)
// Restrict to MIN/MAX range of servo
if (abs(lx_trans) > JS_DEADBAND)
{
WR += ((float)lx_trans / JS_SCALE * Speed);
WR = constrain(WR, WRO_MIN, WRO_MAX);
Wro_Servo = (WR * Deg_to_uS) + Offset;
}
// Gripper jaw position (in degrees - determines width of jaw opening)
// Restrict to MIN/MAX range of servo
if (Ps2x.Button(PSB_L1) || Ps2x.Button(PSB_L2))
{
if (Ps2x.Button(PSB_L1))
{
G += G_INCREMENT; // close
}
else
{
G -= G_INCREMENT; // open
}
G = constrain(G, GRI_MIN, GRI_MAX);
Gri_Servo = (G * Deg_to_uS) + Offset;
}
// Fully open gripper
if (Ps2x.ButtonPressed(PSB_BLUE))
{
Gri_Servo = (GRI_MIN * Deg_to_uS) + Offset;
G = GRI_MIN;
}
delay (10);
if (set_arm(x_tmp, y_tmp, z_tmp, ga_tmp) == IK_SUCCESS)
{
// If the arm was positioned successfully, record
// the new vales. Otherwise, ignore them.
X = x_tmp;
Y = y_tmp;
Z = z_tmp;
GA = ga_tmp;
}
else
{
// Sound tone for audible feedback of error
tone(SPK_PIN, TONE_IK_ERROR, TONE_DURATION);
}
// Reset the flag
arm_move = false;
}
// Arm positioning routine utilizing Inverse Kinematics.
// Z is height, Y is distance from base center out, X is side to side. Y, Z can only be positive.
// Input dimensions are for the gripper, just short of its tip, where it grabs things.
// If resulting arm position is physically unreachable, return error code.
int set_arm(float x, float y, float z, float grip_angle_d)
{
//grip angle in radians for use in calculations
float grip_angle_r = radians(grip_angle_d);
// Base angle and radial distance from x,y coordinates
float bas_angle_r = atan2(x, y);
float rdist = sqrt((x * x) + (y * y));
// rdist is y coordinate for the arm
y = rdist;
// Grip offsets calculated based on grip angle
float grip_off_z = (sin(grip_angle_r)) * GRIPPER;
float grip_off_y = (cos(grip_angle_r)) * GRIPPER;
// Wrist position
float wrist_z = (z - grip_off_z) - BASE_HGT;
float wrist_y = y - grip_off_y;
// Shoulder to wrist distance (AKA sw)
float s_w = (wrist_z * wrist_z) + (wrist_y * wrist_y);
float s_w_sqrt = sqrt(s_w);
// s_w angle to ground
float a1 = atan2(wrist_z, wrist_y);
// s_w angle to humerus
float a2 = acos(((hum_sq - uln_sq) + s_w) / (2 * HUMERUS * s_w_sqrt));
// Shoulder angle
float shl_angle_r = a1 + a2;
float shl_angle_d = degrees(shl_angle_r);
// Elbow angle
float elb_angle_r = acos((hum_sq + uln_sq - s_w) / (2 * HUMERUS * ULNA));
float elb_angle_d = degrees(elb_angle_r);
float elb_angle_dn = -(180.0 - elb_angle_d);
// Wrist angle
float wri_angle_d = (grip_angle_d - elb_angle_dn) - shl_angle_d;
// Calculate servo angles
float bas_pos = BAS_MID + degrees(bas_angle_r);
float shl_pos = SHL_MID + (shl_angle_d - 90.0);
float elb_pos = ELB_MID - (elb_angle_d - 90.0);
float wri_pos = WRI_MID + wri_angle_d;
// If any servo ranges are exceeded, return an error
if (bas_pos < BAS_MIN || bas_pos > BAS_MAX || shl_pos < SHL_MIN || shl_pos > SHL_MAX || elb_pos < ELB_MIN || elb_pos > ELB_MAX || wri_pos < WRI_MIN || wri_pos > WRI_MAX)
return IK_ERROR;
// Position the servos
Bas_Servo = (bas_pos * Deg_to_uS) + Offset;
Shl_Servo = (shl_pos * Deg_to_uS) + Offset;
Elb_Servo = (elb_pos * Deg_to_uS) + Offset;
Wri_Servo = (wri_pos * Deg_to_uS) + Offset;
SSC32.print ("#0 P");
SSC32.print (Bas_Servo);
SSC32.print ("#1 P");
SSC32.print (Shl_Servo);
SSC32.print ("#2 P");
SSC32.print (Elb_Servo);
SSC32.print ("#3 P");
SSC32.print (Wri_Servo);
SSC32.print ("#4 P");
SSC32.print (Wro_Servo);
SSC32.print ("#5 P");
SSC32.print (Gri_Servo);
SSC32.println();
Serial.print(" X: “);
Serial.print(X);
Serial.print(” Y: “);
Serial.print(Y);
Serial.print(” Z: “);
Serial.print(Z);
Serial.print(” GA: “);
Serial.print(GA);
Serial.print(” Base: “);
Serial.print(Bas_Servo);
Serial.print(” Shoulder: “);
Serial.print(Shl_Servo);
Serial.print(” Elbow: “);
Serial.print(Elb_Servo);
Serial.print(” Wrist: “);
Serial.print(Wri_Servo);
Serial.print(” Wrist rotate : “);
Serial.print(Wro_Servo);
Serial.print(” Gripper: ");
Serial.print(Gri_Servo);
Serial.println();
return IK_SUCCESS;
}
void goto_startposition()
{
Gri_Servo = (GRI_MID * Deg_to_uS) + Offset;
Wro_Servo = (WRO_MID * Deg_to_uS) + Offset;
X = READY_X;
Y = READY_Y;
Z = READY_Z;
GA = READY_GA;
}
[/code]
