Skip to content

Commit c17a4c0

Browse files
committed
v1.5.10 moveToAtOnce(x,y,z)
1 parent 7d3eedd commit c17a4c0

3 files changed

Lines changed: 102 additions & 93 deletions

File tree

HISTORY.md

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,8 @@
1+
## [1.5.10] - 2016-06-07
2+
3+
### Fix
4+
- At moveToAtOnce(x,y,z)
5+
16
## [1.5.9] - 2016-06-01
27

38
### Fix

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.5.8
2+
version=1.5.10
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.h

Lines changed: 96 additions & 92 deletions
Original file line numberDiff line numberDiff line change
@@ -1,13 +1,13 @@
11
/******************************************************************************
2-
* File Name : uArm_library.h
3-
* Author : Joey Song
4-
* Updated : Joey Song, Alex Tan, Dave Corboy
5-
* Email : joey@ufactory.cc
6-
* Date : 12 Dec, 2014
7-
* Description :
8-
* License :
9-
* Copyright(C) 2016 UFactory Team. All right reserved.
10-
*******************************************************************************/
2+
* File Name : uArm_library.h
3+
* Author : Joey Song
4+
* Updated : Joey Song, Alex Tan, Dave Corboy
5+
* Email : joey@ufactory.cc
6+
* Date : 12 Dec, 2014
7+
* Description :
8+
* License :
9+
* Copyright(C) 2016 UFactory Team. All right reserved.
10+
*******************************************************************************/
1111

1212
#include <Arduino.h>
1313
#include <EEPROM.h>
@@ -20,7 +20,7 @@
2020

2121
#define UARM_MAJOR_VERSION 1
2222
#define UARM_MINOR_VERSION 5
23-
#define UARM_BUGFIX 9
23+
#define UARM_BUGFIX 10
2424

2525
#define SERVO_ROT_NUM 0
2626
#define SERVO_LEFT_NUM 1
@@ -118,90 +118,94 @@
118118
class uArmClass
119119
{
120120
public:
121-
uArmClass();
122-
123-
double readServoOffset(byte servo_num);
124-
void readSerialNumber(byte (&byte_sn_array)[14]);
125-
void writeSerialNumber(byte (&byte_sn_array)[14]);
126-
void readLinearOffset(byte servo_num, double& intercept_val, double& slope_val);
127-
void detachServo(byte servo_num);
128-
void alert(byte times, byte runTime, byte stopTime);
129-
void detachAll();
130-
void writeServoAngle(byte servoNumber, double servoAngle, boolean writeWithoffset);
131-
void writeLeftRightServoAngle(double servo_left_angle, double servo_right_angle, boolean writeWithoffset);
132-
byte inputToReal(byte servo_num, byte input_angle);
133-
double readAngle(byte servo_num);
134-
double readAngle(byte servo_num, boolean withOffset);
135-
double analogToAngle(int input_angle, byte servo_num, boolean withOffset);
136-
void writeAngle(byte servo_rot_angle, byte servo_left_angle, byte servo_right_angle, byte servo_hand_rot_angle, byte trigger);
137-
138-
void moveToOpts(double x, double y, double z, double hand_angle, byte relative_flags, double time, byte path_type, byte ease_type);
139-
void moveTo(double x, double y,double z) {
140-
moveToOpts(x, y, z, 0, F_HAND_RELATIVE, 2.0, PATH_LINEAR, INTERP_EASE_INOUT_CUBIC);
141-
}
142-
void moveTo(double x, double y,double z,int relative, double time) {
143-
moveToOpts(x, y, z, 0, F_HAND_RELATIVE | (relative ? F_POSN_RELATIVE : 0), time, PATH_LINEAR, INTERP_EASE_INOUT_CUBIC);
144-
}
145-
void moveTo(double x, double y,double z,int relative, double time, double servo_4_angle) {
146-
moveToOpts(x, y, z, servo_4_angle, relative ? F_POSN_RELATIVE : 0, time, PATH_LINEAR, INTERP_EASE_INOUT_CUBIC);
147-
}
148-
void moveTo(double x, double y, double z, int relative, double time, int servo_4_relative, double servo_4_angle) {
149-
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);
150-
}
151-
void moveToAtOnce(double x, double y, double z, int relative, double servo_4_angle) {
152-
moveToOpts(x, y, z, servo_4_angle, relative ? F_POSN_RELATIVE : 0, 0.0, PATH_LINEAR, INTERP_LINEAR);
153-
}
154-
155-
void writeStretch(double armStretch, double armHeight);
156-
void writeAngle(double servo_rot_angle, double servo_left_angle, double servo_right_angle, double servo_hand_rot_angle);
157-
158-
double getCalX() {
159-
return g_cal_x;
160-
}
161-
double getCalY() {
162-
return g_cal_y;
163-
}
164-
double getCalZ() {
165-
return g_cal_z;
166-
}
167-
void getCalXYZ(double& x, double& y, double &z) {
168-
calXYZ(); x = g_cal_x; y = g_cal_y; z = g_cal_z;
169-
}
170-
void getCalXYZ(double theta_1, double theta_2, double theta_3, double& x, double& y, double &z) {
171-
calXYZ(theta_1, theta_2, theta_3); x = g_cal_x; y = g_cal_y; z = g_cal_z;
172-
}
173-
174-
void calAngles(double x, double y, double z, double& theta_1, double& theta_2, double& theta_3);
175-
176-
void calXYZ(double theta_1, double theta_2, double theta_3);
177-
void calXYZ();
178-
179-
void gripperCatch();
180-
void gripperRelease();
181-
void interpolate(double start_val, double end_val, double (&interp_vals)[INTERP_INTVLS], byte ease_type);
182-
void pumpOn();
183-
void pumpOff();
184-
185-
Servo g_servo_rot;
186-
Servo g_servo_left;
187-
Servo g_servo_right;
188-
Servo g_servo_hand_rot;
189-
Servo g_servo_hand;
121+
uArmClass();
122+
123+
double readServoOffset(byte servo_num);
124+
void readSerialNumber(byte (&byte_sn_array)[14]);
125+
void writeSerialNumber(byte (&byte_sn_array)[14]);
126+
void readLinearOffset(byte servo_num, double& intercept_val, double& slope_val);
127+
void detachServo(byte servo_num);
128+
void alert(byte times, byte runTime, byte stopTime);
129+
void detachAll();
130+
void writeServoAngle(byte servoNumber, double servoAngle, boolean writeWithoffset);
131+
void writeLeftRightServoAngle(double servo_left_angle, double servo_right_angle, boolean writeWithoffset);
132+
byte inputToReal(byte servo_num, byte input_angle);
133+
double readAngle(byte servo_num);
134+
double readAngle(byte servo_num, boolean withOffset);
135+
double analogToAngle(int input_angle, byte servo_num, boolean withOffset);
136+
void writeAngle(byte servo_rot_angle, byte servo_left_angle, byte servo_right_angle, byte servo_hand_rot_angle, byte trigger);
137+
138+
void moveToOpts(double x, double y, double z, double hand_angle, byte relative_flags, double time, byte path_type, byte ease_type);
139+
void moveTo(double x, double y,double z) {
140+
moveToOpts(x, y, z, 0, F_HAND_RELATIVE, 2.0, PATH_LINEAR, INTERP_EASE_INOUT_CUBIC);
141+
}
142+
void moveTo(double x, double y,double z,int relative, double time) {
143+
moveToOpts(x, y, z, 0, F_HAND_RELATIVE | (relative ? F_POSN_RELATIVE : 0), time, PATH_LINEAR, INTERP_EASE_INOUT_CUBIC);
144+
}
145+
void moveTo(double x, double y,double z,int relative, double time, double servo_4_angle) {
146+
moveToOpts(x, y, z, servo_4_angle, relative ? F_POSN_RELATIVE : 0, time, PATH_LINEAR, INTERP_EASE_INOUT_CUBIC);
147+
}
148+
void moveTo(double x, double y, double z, int relative, double time, int servo_4_relative, double servo_4_angle) {
149+
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);
150+
}
151+
152+
void moveToAtOnce(double x, double y, double z) {
153+
moveToOpts(x, y, z, 0, F_HAND_RELATIVE, 0.0, PATH_LINEAR, INTERP_LINEAR);
154+
}
155+
void moveToAtOnce(double x, double y, double z, int relative, double servo_4_angle) {
156+
moveToOpts(x, y, z, servo_4_angle, relative ? F_POSN_RELATIVE : 0, 0.0, PATH_LINEAR, INTERP_LINEAR);
157+
}
158+
159+
void writeStretch(double armStretch, double armHeight);
160+
void writeAngle(double servo_rot_angle, double servo_left_angle, double servo_right_angle, double servo_hand_rot_angle);
161+
162+
double getCalX() {
163+
return g_cal_x;
164+
}
165+
double getCalY() {
166+
return g_cal_y;
167+
}
168+
double getCalZ() {
169+
return g_cal_z;
170+
}
171+
void getCalXYZ(double& x, double& y, double &z) {
172+
calXYZ(); x = g_cal_x; y = g_cal_y; z = g_cal_z;
173+
}
174+
void getCalXYZ(double theta_1, double theta_2, double theta_3, double& x, double& y, double &z) {
175+
calXYZ(theta_1, theta_2, theta_3); x = g_cal_x; y = g_cal_y; z = g_cal_z;
176+
}
177+
178+
void calAngles(double x, double y, double z, double& theta_1, double& theta_2, double& theta_3);
179+
180+
void calXYZ(double theta_1, double theta_2, double theta_3);
181+
void calXYZ();
182+
183+
void gripperCatch();
184+
void gripperRelease();
185+
void interpolate(double start_val, double end_val, double (&interp_vals)[INTERP_INTVLS], byte ease_type);
186+
void pumpOn();
187+
void pumpOff();
188+
189+
Servo g_servo_rot;
190+
Servo g_servo_left;
191+
Servo g_servo_right;
192+
Servo g_servo_hand_rot;
193+
Servo g_servo_hand;
190194

191195
protected:
192-
void attachAll();
193-
void attachServo(byte servo_num);
194-
double cur_rot;
195-
double cur_left;
196-
double cur_right;
197-
double cur_hand;
198-
double calYonly(double theta_1, double theta_2, double theta_3);
199-
200-
double g_cal_x;
201-
double g_cal_y;
202-
double g_cal_z;
203-
204-
boolean g_gripper_reset;
196+
void attachAll();
197+
void attachServo(byte servo_num);
198+
double cur_rot;
199+
double cur_left;
200+
double cur_right;
201+
double cur_hand;
202+
double calYonly(double theta_1, double theta_2, double theta_3);
203+
204+
double g_cal_x;
205+
double g_cal_y;
206+
double g_cal_z;
207+
208+
boolean g_gripper_reset;
205209

206210
};
207211

0 commit comments

Comments
 (0)