Skip to content

Commit

Permalink
v1.6.1 fix MoveTo lock Hand Servo issue
Browse files Browse the repository at this point in the history
  • Loading branch information
@altjz
altjz committed Jun 22, 2016
1 parent 35ddb22 commit 6b3b8c6
Show file tree
Hide file tree
Showing 5 changed files with 57 additions and 20 deletions.
6 changes: 6 additions & 0 deletions HISTORY.md
View file
@@ -1,3 +1,9 @@
## [1.6.1] - 2016-06-22

### Fix
- Fix MoveTo Lock Hand Servo issue.


## [1.6.0] - 2016-06-20

### Fix
Expand Down
10 changes: 6 additions & 4 deletions examples/UArmProtocol/UArmProtocol.ino
View file
Expand Up @@ -16,11 +16,11 @@

#define UARM_FIRMATA_MAJOR_VERSION 1
#define UARM_FIRMATA_MINOR_VERSION 3
#define UARM_FIRMATA_BUGFIX 1
#define UARM_FIRMATA_BUGFIX 2

#define UARM_FIRMWARE_MAJOR_VERSION 1
#define UARM_FIRMWARE_MINOR_VERSION 6
#define UARM_FIRMWARE_BUGFIX 0
#define UARM_FIRMWARE_BUGFIX 1

#define START_SYSEX 0xF0 // start a MIDI Sysex message
#define END_SYSEX 0xF7 // end a MIDI Sysex message
Expand Down Expand Up @@ -136,9 +136,11 @@ boolean handleSysex(byte command, byte argc, byte *argv)
float time_spend = argv[17] + (argv[18] << 7) + float(argv[19])/100;
byte path_type = argv[20];
byte ease_type = argv[21];
// boolean enable_hand = argv[22];
boolean enable_hand = false;
if (argv[22] == 1 || argv[22] ==0)
enable_hand = argv[22];
delay(5);
uarm.moveToOpts(x,y,z,hand_angle,relative_flags,time_spend,path_type,ease_type);
uarm.moveToOpts(x,y,z,hand_angle,relative_flags,time_spend,path_type,ease_type,enable_hand);
delay(10);
}

Expand Down
2 changes: 1 addition & 1 deletion library.properties
View file
@@ -1,5 +1,5 @@
name=uArmLibrary
version=1.6.0
version=1.6.1
author=UFactory <developer@ufactory.cc>
maintainer=Joey Song <joey@ufactory.cc>, Alex Tan<alex@ufactory.cc>, Dave Corboy<dave@corboy.com>
sentence=uArm Library for Arduino
Expand Down
39 changes: 32 additions & 7 deletions uarm_library.cpp
View file
Expand Up @@ -76,6 +76,18 @@ void uArmClass::alert(byte times, byte runTime, byte stopTime)
void uArmClass::writeAngle(double servo_rot_angle, double servo_left_angle, double servo_right_angle, double servo_hand_rot_angle)
{
attachAll();
writeAngle(servo_rot_angle, servo_left_angle, servo_right_angle);
writeServoAngle(SERVO_HAND_ROT_NUM,servo_hand_rot_angle,true);
cur_hand = servo_hand_rot_angle;
}

void uArmClass::writeAngle(double servo_rot_angle, double servo_left_angle, double servo_right_angle)
{
// attachAll();

attachServo(SERVO_ROT_NUM);
attachServo(SERVO_LEFT_NUM);
attachServo(SERVO_RIGHT_NUM);

if(servo_left_angle < 10) servo_left_angle = 10;
if(servo_left_angle > 120) servo_left_angle = 120;
Expand All @@ -90,14 +102,14 @@ void uArmClass::writeAngle(double servo_rot_angle, double servo_left_angle, doub
writeServoAngle(SERVO_ROT_NUM,servo_rot_angle,true);
writeServoAngle(SERVO_LEFT_NUM,servo_left_angle,true);
writeServoAngle(SERVO_RIGHT_NUM,servo_right_angle,true);
writeServoAngle(SERVO_HAND_ROT_NUM,servo_hand_rot_angle,true);
// writeServoAngle(SERVO_HAND_ROT_NUM,servo_hand_rot_angle,true);


// refresh logical servo angle cache
cur_rot = servo_rot_angle;
cur_left = servo_left_angle;
cur_right = servo_right_angle;
cur_hand = servo_hand_rot_angle;
// cur_hand = servo_hand_rot_angle;
}

/* Write the angle to Servo
Expand Down Expand Up @@ -487,15 +499,20 @@ void uArmClass::calXYZ()
}
}

int uArmClass::moveToOpts(double x, double y, double z, double hand_angle, byte relative_flags, double time, byte path_type, byte ease_type) {
int uArmClass::moveToOpts(double x, double y, double z, double hand_angle, byte relative_flags, double time, byte path_type, byte ease_type, boolean enable_hand) {
float limit = sqrt((x*x + y*y));
if (limit > 32)
{
float k = 32/limit;
x = x * k;
y = y * k;
}
attachAll();
// attachAll();
attachServo(SERVO_ROT_NUM);
attachServo(SERVO_LEFT_NUM);
attachServo(SERVO_RIGHT_NUM);
if(enable_hand)
attachServo(SERVO_HAND_ROT_NUM);

// find current position using cached servo values
double current_x;
Expand Down Expand Up @@ -552,8 +569,10 @@ int uArmClass::moveToOpts(double x, double y, double z, double hand_angle, byte

for (byte i = 0; i < INTERP_INTVLS; i++)
{

if(enable_hand)
writeAngle(rot_array[i], left_array[i], right_array[i], hand_array[i]);
else
writeAngle(rot_array[i], left_array[i], right_array[i]);
//writeServoAngle(SERVO_ROT_NUM, rot_array[i], true);
// writeServoAngle(SERVO_LEFT_NUM, left_array[i],true);
// writeServoAngle(SERVO_RIGHT_NUM, right_array[i],true);
Expand Down Expand Up @@ -588,7 +607,10 @@ int uArmClass::moveToOpts(double x, double y, double z, double hand_angle, byte
//writeLeftRightServoAngle(left, right, true);
// if(enable_hand)
//writeServoAngle(SERVO_HAND_ROT_NUM, hand_array[i], true);
writeAngle(rot, left, right, hand_array[i]);
if(enable_hand)
writeAngle(rot, left, right, hand_array[i]);
else
writeAngle(rot, left, right);
delay(time * 1000 / INTERP_INTVLS);
}
}
Expand All @@ -600,7 +622,10 @@ int uArmClass::moveToOpts(double x, double y, double z, double hand_angle, byte
// writeServoAngle(SERVO_RIGHT_NUM, tgt_right, true);
//writeLeftRightServoAngle(tgt_left, tgt_right, true);
//writeServoAngle(SERVO_HAND_ROT_NUM, hand_angle, true);
writeAngle(tgt_rot, tgt_left, tgt_right, hand_angle);
if (enable_hand)
writeAngle(tgt_rot, tgt_left, tgt_right, hand_angle);
else
writeAngle(tgt_rot, tgt_left, tgt_right);
}

double uArmClass::calYonly(double theta_1, double theta_2, double theta_3)
Expand Down
20 changes: 12 additions & 8 deletions uarm_library.h
View file
Expand Up @@ -20,7 +20,7 @@

#define UARM_MAJOR_VERSION 1
#define UARM_MINOR_VERSION 6
#define UARM_BUGFIX 0
#define UARM_BUGFIX 1

#define SUCCESS 1
#define FAILED -1
Expand Down Expand Up @@ -153,29 +153,33 @@ class uArmClass
double analogToAngle(int input_angle, byte servo_num, boolean withOffset);
void writeAngle(byte servo_rot_angle, byte servo_left_angle, byte servo_right_angle, byte servo_hand_rot_angle, byte trigger);

int moveToOpts(double x, double y, double z, double hand_angle, byte relative_flags, double time, byte path_type, byte ease_type);
int moveToOpts(double x, double y, double z, double hand_angle, byte relative_flags, double time, byte path_type, byte ease_type, boolean enable_hand);
void moveTo(double x, double y,double z) {
moveToOpts(x, y, z, 0, F_HAND_RELATIVE, 1.0, PATH_LINEAR, INTERP_EASE_INOUT_CUBIC);
moveToOpts(x, y, z, 0, F_HAND_RELATIVE, 1.0, PATH_LINEAR, INTERP_EASE_INOUT_CUBIC, false);
}
void moveTo(double x, double y,double z,double hand_angle) {
moveToOpts(x, y, z, hand_angle, F_HAND_RELATIVE, 1.0, PATH_LINEAR, INTERP_EASE_INOUT_CUBIC, true);
}
void moveTo(double x, double y,double z,int relative, double time) {
moveToOpts(x, y, z, 0, F_HAND_RELATIVE | (relative ? F_POSN_RELATIVE : 0), time, PATH_LINEAR, INTERP_EASE_INOUT_CUBIC);
moveToOpts(x, y, z, 0, F_HAND_RELATIVE | (relative ? F_POSN_RELATIVE : 0), time, PATH_LINEAR, INTERP_EASE_INOUT_CUBIC, false);
}
void moveTo(double x, double y,double z,int relative, double time, double servo_4_angle) {
moveToOpts(x, y, z, servo_4_angle, relative ? F_POSN_RELATIVE : 0, time, PATH_LINEAR, INTERP_EASE_INOUT_CUBIC);
moveToOpts(x, y, z, servo_4_angle, relative ? F_POSN_RELATIVE : 0, time, PATH_LINEAR, INTERP_EASE_INOUT_CUBIC, true);
}
void moveTo(double x, double y, double z, int relative, double time, int servo_4_relative, double servo_4_angle) {
moveToOpts(x, y, z, servo_4_angle, (relative ? F_POSN_RELATIVE : 0) | (servo_4_relative ? F_HAND_RELATIVE : 0), time, PATH_LINEAR, INTERP_EASE_INOUT_CUBIC);
moveToOpts(x, y, z, servo_4_angle, (relative ? F_POSN_RELATIVE : 0) | (servo_4_relative ? F_HAND_RELATIVE : 0), time, PATH_LINEAR, INTERP_EASE_INOUT_CUBIC, true);
}

void moveToAtOnce(double x, double y, double z) {
moveToOpts(x, y, z, 0, F_HAND_RELATIVE, 0.0, PATH_LINEAR, INTERP_LINEAR);
moveToOpts(x, y, z, 0, F_HAND_RELATIVE, 0.0, PATH_LINEAR, INTERP_LINEAR, false);
}
void moveToAtOnce(double x, double y, double z, int relative, double servo_4_angle) {
moveToOpts(x, y, z, servo_4_angle, relative ? F_POSN_RELATIVE : 0, 0.0, PATH_LINEAR, INTERP_LINEAR);
moveToOpts(x, y, z, servo_4_angle, relative ? F_POSN_RELATIVE : 0, 0.0, PATH_LINEAR, INTERP_LINEAR, true);
}

void writeStretch(double armStretch, double armHeight);
void writeAngle(double servo_rot_angle, double servo_left_angle, double servo_right_angle, double servo_hand_rot_angle);
void writeAngle(double servo_rot_angle, double servo_left_angle, double servo_right_angle);

double getCalX() {
return g_cal_x;
Expand Down

0 comments on commit 6b3b8c6

Please sign in to comment.