Skip to content

Commit 6b3b8c6

Browse files
altjzaltjz
committed
v1.6.1 fix MoveTo lock Hand Servo issue
1 parent 35ddb22 commit 6b3b8c6

5 files changed

Lines changed: 57 additions & 20 deletions

File tree

‎HISTORY.md‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,9 @@
1+
## [1.6.1] - 2016-06-22
2+
3+
### Fix
4+
- Fix MoveTo Lock Hand Servo issue.
5+
6+
17
## [1.6.0] - 2016-06-20
28

39
### Fix

‎examples/UArmProtocol/UArmProtocol.ino‎

Lines changed: 6 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -16,11 +16,11 @@
1616

1717
#define UARM_FIRMATA_MAJOR_VERSION 1
1818
#define UARM_FIRMATA_MINOR_VERSION 3
19-
#define UARM_FIRMATA_BUGFIX 1
19+
#define UARM_FIRMATA_BUGFIX 2
2020

2121
#define UARM_FIRMWARE_MAJOR_VERSION 1
2222
#define UARM_FIRMWARE_MINOR_VERSION 6
23-
#define UARM_FIRMWARE_BUGFIX 0
23+
#define UARM_FIRMWARE_BUGFIX 1
2424

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

‎library.properties‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,5 @@
11
name=uArmLibrary
2-
version=1.6.0
2+
version=1.6.1
33
author=UFactory <developer@ufactory.cc>
44
maintainer=Joey Song <joey@ufactory.cc>, Alex Tan<alex@ufactory.cc>, Dave Corboy<dave@corboy.com>
55
sentence=uArm Library for Arduino

‎uarm_library.cpp‎

Lines changed: 32 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -76,6 +76,18 @@ void uArmClass::alert(byte times, byte runTime, byte stopTime)
7676
void uArmClass::writeAngle(double servo_rot_angle, double servo_left_angle, double servo_right_angle, double servo_hand_rot_angle)
7777
{
7878
attachAll();
79+
writeAngle(servo_rot_angle, servo_left_angle, servo_right_angle);
80+
writeServoAngle(SERVO_HAND_ROT_NUM,servo_hand_rot_angle,true);
81+
cur_hand = servo_hand_rot_angle;
82+
}
83+
84+
void uArmClass::writeAngle(double servo_rot_angle, double servo_left_angle, double servo_right_angle)
85+
{
86+
// attachAll();
87+
88+
attachServo(SERVO_ROT_NUM);
89+
attachServo(SERVO_LEFT_NUM);
90+
attachServo(SERVO_RIGHT_NUM);
7991

8092
if(servo_left_angle < 10) servo_left_angle = 10;
8193
if(servo_left_angle > 120) servo_left_angle = 120;
@@ -90,14 +102,14 @@ void uArmClass::writeAngle(double servo_rot_angle, double servo_left_angle, doub
90102
writeServoAngle(SERVO_ROT_NUM,servo_rot_angle,true);
91103
writeServoAngle(SERVO_LEFT_NUM,servo_left_angle,true);
92104
writeServoAngle(SERVO_RIGHT_NUM,servo_right_angle,true);
93-
writeServoAngle(SERVO_HAND_ROT_NUM,servo_hand_rot_angle,true);
105+
// writeServoAngle(SERVO_HAND_ROT_NUM,servo_hand_rot_angle,true);
94106

95107

96108
// refresh logical servo angle cache
97109
cur_rot = servo_rot_angle;
98110
cur_left = servo_left_angle;
99111
cur_right = servo_right_angle;
100-
cur_hand = servo_hand_rot_angle;
112+
// cur_hand = servo_hand_rot_angle;
101113
}
102114

103115
/* Write the angle to Servo
@@ -487,15 +499,20 @@ void uArmClass::calXYZ()
487499
}
488500
}
489501

490-
int uArmClass::moveToOpts(double x, double y, double z, double hand_angle, byte relative_flags, double time, byte path_type, byte ease_type) {
502+
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) {
491503
float limit = sqrt((x*x + y*y));
492504
if (limit > 32)
493505
{
494506
float k = 32/limit;
495507
x = x * k;
496508
y = y * k;
497509
}
498-
attachAll();
510+
// attachAll();
511+
attachServo(SERVO_ROT_NUM);
512+
attachServo(SERVO_LEFT_NUM);
513+
attachServo(SERVO_RIGHT_NUM);
514+
if(enable_hand)
515+
attachServo(SERVO_HAND_ROT_NUM);
499516

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

553570
for (byte i = 0; i < INTERP_INTVLS; i++)
554571
{
555-
572+
if(enable_hand)
556573
writeAngle(rot_array[i], left_array[i], right_array[i], hand_array[i]);
574+
else
575+
writeAngle(rot_array[i], left_array[i], right_array[i]);
557576
//writeServoAngle(SERVO_ROT_NUM, rot_array[i], true);
558577
// writeServoAngle(SERVO_LEFT_NUM, left_array[i],true);
559578
// writeServoAngle(SERVO_RIGHT_NUM, right_array[i],true);
@@ -588,7 +607,10 @@ int uArmClass::moveToOpts(double x, double y, double z, double hand_angle, byte
588607
//writeLeftRightServoAngle(left, right, true);
589608
// if(enable_hand)
590609
//writeServoAngle(SERVO_HAND_ROT_NUM, hand_array[i], true);
591-
writeAngle(rot, left, right, hand_array[i]);
610+
if(enable_hand)
611+
writeAngle(rot, left, right, hand_array[i]);
612+
else
613+
writeAngle(rot, left, right);
592614
delay(time * 1000 / INTERP_INTVLS);
593615
}
594616
}
@@ -600,7 +622,10 @@ int uArmClass::moveToOpts(double x, double y, double z, double hand_angle, byte
600622
// writeServoAngle(SERVO_RIGHT_NUM, tgt_right, true);
601623
//writeLeftRightServoAngle(tgt_left, tgt_right, true);
602624
//writeServoAngle(SERVO_HAND_ROT_NUM, hand_angle, true);
603-
writeAngle(tgt_rot, tgt_left, tgt_right, hand_angle);
625+
if (enable_hand)
626+
writeAngle(tgt_rot, tgt_left, tgt_right, hand_angle);
627+
else
628+
writeAngle(tgt_rot, tgt_left, tgt_right);
604629
}
605630

606631
double uArmClass::calYonly(double theta_1, double theta_2, double theta_3)

‎uarm_library.h‎

Lines changed: 12 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -20,7 +20,7 @@
2020

2121
#define UARM_MAJOR_VERSION 1
2222
#define UARM_MINOR_VERSION 6
23-
#define UARM_BUGFIX 0
23+
#define UARM_BUGFIX 1
2424

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

156-
int moveToOpts(double x, double y, double z, double hand_angle, byte relative_flags, double time, byte path_type, byte ease_type);
156+
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);
157157
void moveTo(double x, double y,double z) {
158-
moveToOpts(x, y, z, 0, F_HAND_RELATIVE, 1.0, PATH_LINEAR, INTERP_EASE_INOUT_CUBIC);
158+
moveToOpts(x, y, z, 0, F_HAND_RELATIVE, 1.0, PATH_LINEAR, INTERP_EASE_INOUT_CUBIC, false);
159+
}
160+
void moveTo(double x, double y,double z,double hand_angle) {
161+
moveToOpts(x, y, z, hand_angle, F_HAND_RELATIVE, 1.0, PATH_LINEAR, INTERP_EASE_INOUT_CUBIC, true);
159162
}
160163
void moveTo(double x, double y,double z,int relative, double time) {
161-
moveToOpts(x, y, z, 0, F_HAND_RELATIVE | (relative ? F_POSN_RELATIVE : 0), time, PATH_LINEAR, INTERP_EASE_INOUT_CUBIC);
164+
moveToOpts(x, y, z, 0, F_HAND_RELATIVE | (relative ? F_POSN_RELATIVE : 0), time, PATH_LINEAR, INTERP_EASE_INOUT_CUBIC, false);
162165
}
163166
void moveTo(double x, double y,double z,int relative, double time, double servo_4_angle) {
164-
moveToOpts(x, y, z, servo_4_angle, relative ? F_POSN_RELATIVE : 0, time, PATH_LINEAR, INTERP_EASE_INOUT_CUBIC);
167+
moveToOpts(x, y, z, servo_4_angle, relative ? F_POSN_RELATIVE : 0, time, PATH_LINEAR, INTERP_EASE_INOUT_CUBIC, true);
165168
}
166169
void moveTo(double x, double y, double z, int relative, double time, int servo_4_relative, double servo_4_angle) {
167-
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);
170+
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);
168171
}
169172

170173
void moveToAtOnce(double x, double y, double z) {
171-
moveToOpts(x, y, z, 0, F_HAND_RELATIVE, 0.0, PATH_LINEAR, INTERP_LINEAR);
174+
moveToOpts(x, y, z, 0, F_HAND_RELATIVE, 0.0, PATH_LINEAR, INTERP_LINEAR, false);
172175
}
173176
void moveToAtOnce(double x, double y, double z, int relative, double servo_4_angle) {
174-
moveToOpts(x, y, z, servo_4_angle, relative ? F_POSN_RELATIVE : 0, 0.0, PATH_LINEAR, INTERP_LINEAR);
177+
moveToOpts(x, y, z, servo_4_angle, relative ? F_POSN_RELATIVE : 0, 0.0, PATH_LINEAR, INTERP_LINEAR, true);
175178
}
176179

177180
void writeStretch(double armStretch, double armHeight);
178181
void writeAngle(double servo_rot_angle, double servo_left_angle, double servo_right_angle, double servo_hand_rot_angle);
182+
void writeAngle(double servo_rot_angle, double servo_left_angle, double servo_right_angle);
179183

180184
double getCalX() {
181185
return g_cal_x;

0 commit comments

Comments
 (0)