Skip to content

Commit 878f6fe

Browse files
altjzaltjz
committed
v1.7.2 fix attach servo if not calibrate
1 parent 93d190a commit 878f6fe

5 files changed

Lines changed: 109 additions & 89 deletions

File tree

‎HISTORY.md‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,12 @@
11
# uArm Arduino Library Release Note
22

3+
## [1.7.2] - 2016-06-30
4+
5+
### Fix
6+
- attach servo even not calibrate
7+
- rewrite set_servo_status replace attach_servo detach_servo
8+
- execute write_stretch_height event not calibrate
9+
310
## [1.7.1] - 2016-06-28
411

512
### Fix

‎examples/UArmProtocol/UArmProtocol.ino‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -20,7 +20,7 @@
2020

2121
#define UARM_FIRMWARE_MAJOR_VERSION 1
2222
#define UARM_FIRMWARE_MINOR_VERSION 7
23-
#define UARM_FIRMWARE_BUGFIX 1
23+
#define UARM_FIRMWARE_BUGFIX 2
2424

2525
#define START_SYSEX 0xF0 // start a MIDI Sysex message
2626
#define END_SYSEX 0xF7 // end a MIDI Sysex message

‎src/uarm_library.cpp‎

Lines changed: 93 additions & 80 deletions
Original file line numberDiff line numberDiff line change
@@ -12,15 +12,18 @@ uArmClass uarm;
1212

1313
uArmClass::uArmClass()
1414
{
15-
1615
}
1716

1817
void uArmClass::init()
1918
{
20-
set_servo_status(true, SERVO_ROT_NUM);
21-
set_servo_status(true, SERVO_LEFT_NUM);
22-
set_servo_status(true, SERVO_RIGHT_NUM);
23-
set_servo_status(true, SERVO_HAND_ROT_NUM);
19+
if (EEPROM.read(CALIBRATION_FLAG) != CONFIRM_FLAG)
20+
{
21+
alert(50, 10, 10);
22+
}
23+
set_servo_status(true, SERVO_ROT_NUM);
24+
set_servo_status(true, SERVO_LEFT_NUM);
25+
set_servo_status(true, SERVO_RIGHT_NUM);
26+
set_servo_status(true, SERVO_HAND_ROT_NUM);
2427
}
2528

2629
/*!
@@ -65,10 +68,6 @@ int uArmClass::write_servo_angle(double servo_rot_angle, double servo_left_angle
6568
*/
6669
int uArmClass::write_servo_angle(double servo_rot_angle, double servo_left_angle, double servo_right_angle)
6770
{
68-
// setServoStatus(true, SERVO_ROT_NUM);
69-
// setServoStatus(true, SERVO_LEFT_NUM);
70-
// setServoStatus(true, SERVO_RIGHT_NUM);
71-
7271
if(servo_left_angle < 10) servo_left_angle = 10;
7372
if(servo_left_angle > 120) servo_left_angle = 120;
7473
if(servo_right_angle < 10) servo_right_angle = 10;
@@ -135,25 +134,10 @@ void uArmClass::write_left_right_servo_angle(double servo_left_angle, double ser
135134
alert(1, 10, 0);
136135
return;
137136
}
138-
// attach_servo(SERVO_LEFT_NUM);
139-
// attach_servo(SERVO_RIGHT_NUM);
140137
g_servo_left.write(servo_left_angle);
141138
g_servo_right.write(servo_right_angle);
142139
}
143140

144-
/*!
145-
\brief Attach All Servo
146-
\note Warning, if you attach left servo & right servo without a movement, it might be caused a demage
147-
*/
148-
// void uArmClass::attach_all()
149-
// {
150-
// attach_servo(SERVO_ROT_NUM);
151-
// attach_servo(SERVO_LEFT_NUM);
152-
// attach_servo(SERVO_RIGHT_NUM);
153-
// attach_servo(SERVO_HAND_ROT_NUM);
154-
// }
155-
156-
157141
/*!
158142
\brief Attach Servo, if servo has not been attached, attach the servo, and read the current Angle
159143
\param servo number SERVO_ROT_NUM, SERVO_LEFT_NUM, SERVO_RIGHT_NUM, SERVO_HAND_ROT_NUM
@@ -162,42 +146,70 @@ boolean uArmClass::set_servo_status(boolean setAttached, byte servoNum){
162146
// Attach or detach a servo, and set the position instantly after doing so to prevent "snap"
163147
// Returns true or false if the servo was a valid number
164148
// Serial.println("Starting new func");
165-
149+
boolean is_linear_calibrated = false;
150+
if (EEPROM.read(CALIBRATION_LINEAR_FLAG) == CONFIRM_FLAG)
151+
{
152+
is_linear_calibrated = true;
153+
}
166154
double angleBefore = 90;
167155
if(servoNum == SERVO_ROT_NUM) {
168156
if(setAttached) {
169-
angleBefore = read_servo_angle(SERVO_ROT_NUM);
170-
// Serial.println("New angle" + String(angleBefore));
171-
uarm.g_servo_rot.attach(SERVO_ROT_PIN);
172-
uarm.g_servo_rot.write(angleBefore);
173-
cur_rot = angleBefore;
157+
if (is_linear_calibrated == true) {
158+
angleBefore = read_servo_angle(SERVO_ROT_NUM);
159+
// Serial.println("New angle" + String(angleBefore));
160+
uarm.g_servo_rot.attach(SERVO_ROT_PIN);
161+
uarm.g_servo_rot.write(angleBefore);
162+
cur_rot = angleBefore;
163+
}
164+
else{
165+
uarm.g_servo_rot.attach(SERVO_ROT_PIN);
166+
uarm.g_servo_rot.write(90);
167+
}
174168
}else{
175169
uarm.g_servo_rot.detach();
176170
}
177171
}else if(servoNum == SERVO_LEFT_NUM) {
178172
if(setAttached) {
179-
angleBefore = uarm.read_servo_angle(SERVO_LEFT_NUM);
180-
uarm.g_servo_left.attach(SERVO_LEFT_PIN);
181-
uarm.g_servo_left.write(angleBefore);
182-
cur_left = angleBefore;
173+
if (is_linear_calibrated == true) {
174+
angleBefore = uarm.read_servo_angle(SERVO_LEFT_NUM);
175+
uarm.g_servo_left.attach(SERVO_LEFT_PIN);
176+
uarm.g_servo_left.write(angleBefore);
177+
cur_left = angleBefore;
178+
}
179+
else{
180+
uarm.g_servo_left.attach(SERVO_LEFT_PIN);
181+
uarm.g_servo_left.write(100);
182+
}
183183
}else{
184184
uarm.g_servo_left.detach();
185185
}
186186
}else if(servoNum == SERVO_RIGHT_NUM) {
187187
if(setAttached) {
188-
angleBefore = uarm.read_servo_angle(SERVO_RIGHT_NUM);
189-
uarm.g_servo_right.attach(SERVO_RIGHT_PIN);
190-
uarm.g_servo_right.write(angleBefore);
191-
cur_right = angleBefore;
188+
if (is_linear_calibrated == true) {
189+
angleBefore = uarm.read_servo_angle(SERVO_RIGHT_NUM);
190+
uarm.g_servo_right.attach(SERVO_RIGHT_PIN);
191+
uarm.g_servo_right.write(angleBefore);
192+
cur_right = angleBefore;
193+
}
194+
else{
195+
uarm.g_servo_right.attach(SERVO_RIGHT_PIN);
196+
uarm.g_servo_right.write(60);
197+
}
192198
}else{
193199
uarm.g_servo_right.detach();
194200
}
195201
}else if(servoNum == SERVO_HAND_ROT_NUM) {
196202
if(setAttached) {
197-
angleBefore = uarm.read_servo_angle(SERVO_HAND_ROT_NUM);
198-
uarm.g_servo_hand_rot.attach(SERVO_HAND_PIN);
199-
uarm.g_servo_hand_rot.write(angleBefore);
200-
cur_hand = angleBefore;
203+
if (is_linear_calibrated == true) {
204+
angleBefore = uarm.read_servo_angle(SERVO_HAND_ROT_NUM);
205+
uarm.g_servo_hand_rot.attach(SERVO_HAND_PIN);
206+
uarm.g_servo_hand_rot.write(angleBefore);
207+
cur_hand = angleBefore;
208+
}
209+
else{
210+
uarm.g_servo_hand_rot.attach(SERVO_HAND_PIN);
211+
uarm.g_servo_hand_rot.write(90);
212+
}
201213
}else{
202214
uarm.g_servo_hand_rot.detach();
203215
}
@@ -265,30 +277,30 @@ double uArmClass::read_servo_angle(byte servo_num)
265277
double uArmClass::read_servo_angle(byte servo_num, boolean withOffset)
266278
{
267279
double angle = 0;
268-
for (byte i = 0; i < 5; i++){
269-
switch (servo_num)
270-
{
271-
case SERVO_ROT_NUM:
272-
angle += analog_to_angle(analogRead(SERVO_ROT_ANALOG_PIN),SERVO_ROT_NUM,withOffset);
273-
break;
274-
275-
case SERVO_LEFT_NUM:
276-
angle += analog_to_angle(analogRead(SERVO_LEFT_ANALOG_PIN),SERVO_LEFT_NUM,withOffset);
277-
break;
278-
279-
case SERVO_RIGHT_NUM:
280-
angle += analog_to_angle(analogRead(SERVO_RIGHT_ANALOG_PIN),SERVO_RIGHT_NUM,withOffset);
281-
break;
282-
283-
case SERVO_HAND_ROT_NUM:
284-
angle += analog_to_angle(analogRead(SERVO_HAND_ROT_ANALOG_PIN),SERVO_HAND_ROT_NUM,withOffset);
285-
break;
286-
287-
default:
288-
break;
289-
290-
}
291-
delay(10);
280+
for (byte i = 0; i < 5; i++) {
281+
switch (servo_num)
282+
{
283+
case SERVO_ROT_NUM:
284+
angle += analog_to_angle(analogRead(SERVO_ROT_ANALOG_PIN),SERVO_ROT_NUM,withOffset);
285+
break;
286+
287+
case SERVO_LEFT_NUM:
288+
angle += analog_to_angle(analogRead(SERVO_LEFT_ANALOG_PIN),SERVO_LEFT_NUM,withOffset);
289+
break;
290+
291+
case SERVO_RIGHT_NUM:
292+
angle += analog_to_angle(analogRead(SERVO_RIGHT_ANALOG_PIN),SERVO_RIGHT_NUM,withOffset);
293+
break;
294+
295+
case SERVO_HAND_ROT_NUM:
296+
angle += analog_to_angle(analogRead(SERVO_HAND_ROT_ANALOG_PIN),SERVO_HAND_ROT_NUM,withOffset);
297+
break;
298+
299+
default:
300+
break;
301+
302+
}
303+
delay(10);
292304
}
293305
return angle/5;
294306
}
@@ -425,15 +437,16 @@ void uArmClass::coordinate_to_angle(double x, double y, double z, double& theta_
425437
\param armHeight Height from -150 to 150
426438
*/
427439
void uArmClass::write_stretch_height(double armStretch, double armHeight){
428-
if(EEPROM.read(CALIBRATION_STRETCH_FLAG) != CONFIRM_FLAG) {
429-
alert(3, 200, 200);
430-
return;
431-
}
432440
double offsetL = 0;
433441
double offsetR = 0;
434-
435-
EEPROM.get(OFFSET_STRETCH_START_ADDRESS, offsetL);
436-
EEPROM.get(OFFSET_STRETCH_START_ADDRESS + 4, offsetR);
442+
if(EEPROM.read(CALIBRATION_STRETCH_FLAG) != CONFIRM_FLAG) {
443+
offsetL = -10;
444+
offsetR = -10;
445+
}
446+
else{
447+
EEPROM.get(OFFSET_STRETCH_START_ADDRESS, offsetL);
448+
EEPROM.get(OFFSET_STRETCH_START_ADDRESS + 4, offsetR);
449+
}
437450
armStretch = constrain(armStretch, ARM_STRETCH_MIN, ARM_STRETCH_MAX) + 68;
438451
armHeight = constrain(armHeight, ARM_HEIGHT_MIN, ARM_HEIGHT_MAX);
439452
double xx = armStretch*armStretch + armHeight*armHeight;
@@ -476,7 +489,7 @@ void uArmClass::get_current_xyz(double theta_1, double theta_2, double theta_3)
476489
\param end_val End Position
477490
\param interp_vals interpolation array
478491
\param ease_type INTERP_EASE_INOUT_CUBIC, INTERP_LINEAR, INTERP_EASE_INOUT, INTERP_EASE_IN, INTERP_EASE_OUT
479-
*/
492+
*/
480493
void uArmClass::interpolate(double start_val, double end_val, double *interp_vals, byte ease_type) {
481494
double delta = end_val - start_val;
482495
for (byte f = 0; f < INTERP_INTVLS; f++) {
@@ -527,7 +540,7 @@ void uArmClass::interpolate(double start_val, double end_val, double *interp_val
527540
\param path_type PATH_LINEAR, PATH_ANGLES
528541
\param enable_hand Enable Hand Axis
529542
\return SUCCESS, FAILED
530-
*/
543+
*/
531544
int uArmClass::move_to(double x, double y, double z, double hand_angle, byte relative_flags, double time, byte path_type, byte ease_type, boolean enable_hand) {
532545
float limit = sqrt((x*x + y*y));
533546
if (limit > 32)
@@ -642,7 +655,7 @@ int uArmClass::move_to(double x, double y, double z, double hand_angle, byte rel
642655
\param theta_2
643656
\param theta_3
644657
\return Y Axis Value
645-
*/
658+
*/
646659
double uArmClass::angle_to_coordinate_y(double theta_1, double theta_2, double theta_3)
647660
{
648661
double l5_2 = (MATH_L2 + MATH_L3*cos(theta_2 / MATH_TRANS) + MATH_L4*cos(theta_3 / MATH_TRANS));
@@ -652,7 +665,7 @@ double uArmClass::angle_to_coordinate_y(double theta_1, double theta_2, double t
652665

653666
/*!
654667
\brief Close Gripper
655-
*/
668+
*/
656669
void uArmClass::gripper_catch()
657670
{
658671
pinMode(GRIPPER, OUTPUT);
@@ -662,7 +675,7 @@ void uArmClass::gripper_catch()
662675

663676
/*!
664677
\brief Release Gripper
665-
*/
678+
*/
666679
void uArmClass::gripper_release()
667680
{
668681
if(g_gripper_reset)
@@ -675,7 +688,7 @@ void uArmClass::gripper_release()
675688

676689
/*!
677690
\brief Turn on Pump
678-
*/
691+
*/
679692
void uArmClass::pump_on()
680693
{
681694

@@ -687,7 +700,7 @@ void uArmClass::pump_on()
687700

688701
/*!
689702
\brief Turn off Pump
690-
*/
703+
*/
691704
void uArmClass::pump_off()
692705
{
693706
pinMode(PUMP_EN, OUTPUT);

‎src/uarm_library.h‎

Lines changed: 7 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -18,7 +18,7 @@
1818

1919
#define UARM_MAJOR_VERSION 1
2020
#define UARM_MINOR_VERSION 7
21-
#define UARM_BUGFIX 1
21+
#define UARM_BUGFIX 2
2222

2323
#define SUCCESS 1
2424
#define FAILED -1
@@ -185,11 +185,7 @@ class uArmClass
185185
int write_servo_angle(double servo_rot_angle, double servo_left_angle, double servo_right_angle, double servo_hand_rot_angle);
186186
int write_servo_angle(double servo_rot_angle, double servo_left_angle, double servo_right_angle);
187187

188-
Servo g_servo_rot;
189-
Servo g_servo_left;
190-
Servo g_servo_right;
191-
Servo g_servo_hand_rot;
192-
Servo g_servo_hand;
188+
193189
unsigned int INTERP_INTVLS;
194190
// void attach_all();
195191
boolean set_servo_status(boolean attach_state, byte servo_num);
@@ -209,7 +205,11 @@ class uArmClass
209205
boolean g_gripper_reset;
210206

211207
private:
212-
208+
Servo g_servo_rot;
209+
Servo g_servo_left;
210+
Servo g_servo_right;
211+
Servo g_servo_hand_rot;
212+
Servo g_servo_hand;
213213

214214
};
215215

‎version‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,2 +1,2 @@
11
[firmware]
2-
version=1.7.1
2+
version=1.7.2

0 commit comments

Comments
 (0)