Skip to content

Commit

Permalink
v1.5.10 moveToAtOnce(x,y,z)
Browse files Browse the repository at this point in the history
  • Loading branch information
altjz committed Jun 7, 2016
1 parent 7d3eedd commit c17a4c0
Show file tree
Hide file tree
Showing 3 changed files with 102 additions and 93 deletions.
5 changes: 5 additions & 0 deletions HISTORY.md
@@ -1,3 +1,8 @@
## [1.5.10] - 2016-06-07

### Fix
- At moveToAtOnce(x,y,z)

## [1.5.9] - 2016-06-01

### Fix
Expand Down
2 changes: 1 addition & 1 deletion library.properties
@@ -1,5 +1,5 @@
name=uArmLibrary
version=1.5.8
version=1.5.10
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
188 changes: 96 additions & 92 deletions uarm_library.h
@@ -1,13 +1,13 @@
/******************************************************************************
* File Name : uArm_library.h
* Author : Joey Song
* Updated : Joey Song, Alex Tan, Dave Corboy
* Email : joey@ufactory.cc
* Date : 12 Dec, 2014
* Description :
* License :
* Copyright(C) 2016 UFactory Team. All right reserved.
*******************************************************************************/
* File Name : uArm_library.h
* Author : Joey Song
* Updated : Joey Song, Alex Tan, Dave Corboy
* Email : joey@ufactory.cc
* Date : 12 Dec, 2014
* Description :
* License :
* Copyright(C) 2016 UFactory Team. All right reserved.
*******************************************************************************/

#include <Arduino.h>
#include <EEPROM.h>
Expand All @@ -20,7 +20,7 @@

#define UARM_MAJOR_VERSION 1
#define UARM_MINOR_VERSION 5
#define UARM_BUGFIX 9
#define UARM_BUGFIX 10

#define SERVO_ROT_NUM 0
#define SERVO_LEFT_NUM 1
Expand Down Expand Up @@ -118,90 +118,94 @@
class uArmClass
{
public:
uArmClass();

double readServoOffset(byte servo_num);
void readSerialNumber(byte (&byte_sn_array)[14]);
void writeSerialNumber(byte (&byte_sn_array)[14]);
void readLinearOffset(byte servo_num, double& intercept_val, double& slope_val);
void detachServo(byte servo_num);
void alert(byte times, byte runTime, byte stopTime);
void detachAll();
void writeServoAngle(byte servoNumber, double servoAngle, boolean writeWithoffset);
void writeLeftRightServoAngle(double servo_left_angle, double servo_right_angle, boolean writeWithoffset);
byte inputToReal(byte servo_num, byte input_angle);
double readAngle(byte servo_num);
double readAngle(byte servo_num, boolean withOffset);
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);

void moveToOpts(double x, double y, double z, double hand_angle, byte relative_flags, double time, byte path_type, byte ease_type);
void moveTo(double x, double y,double z) {
moveToOpts(x, y, z, 0, F_HAND_RELATIVE, 2.0, PATH_LINEAR, INTERP_EASE_INOUT_CUBIC);
}
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);
}
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);
}
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);
}
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);
}

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);

double getCalX() {
return g_cal_x;
}
double getCalY() {
return g_cal_y;
}
double getCalZ() {
return g_cal_z;
}
void getCalXYZ(double& x, double& y, double &z) {
calXYZ(); x = g_cal_x; y = g_cal_y; z = g_cal_z;
}
void getCalXYZ(double theta_1, double theta_2, double theta_3, double& x, double& y, double &z) {
calXYZ(theta_1, theta_2, theta_3); x = g_cal_x; y = g_cal_y; z = g_cal_z;
}

void calAngles(double x, double y, double z, double& theta_1, double& theta_2, double& theta_3);

void calXYZ(double theta_1, double theta_2, double theta_3);
void calXYZ();

void gripperCatch();
void gripperRelease();
void interpolate(double start_val, double end_val, double (&interp_vals)[INTERP_INTVLS], byte ease_type);
void pumpOn();
void pumpOff();

Servo g_servo_rot;
Servo g_servo_left;
Servo g_servo_right;
Servo g_servo_hand_rot;
Servo g_servo_hand;
uArmClass();

double readServoOffset(byte servo_num);
void readSerialNumber(byte (&byte_sn_array)[14]);
void writeSerialNumber(byte (&byte_sn_array)[14]);
void readLinearOffset(byte servo_num, double& intercept_val, double& slope_val);
void detachServo(byte servo_num);
void alert(byte times, byte runTime, byte stopTime);
void detachAll();
void writeServoAngle(byte servoNumber, double servoAngle, boolean writeWithoffset);
void writeLeftRightServoAngle(double servo_left_angle, double servo_right_angle, boolean writeWithoffset);
byte inputToReal(byte servo_num, byte input_angle);
double readAngle(byte servo_num);
double readAngle(byte servo_num, boolean withOffset);
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);

void moveToOpts(double x, double y, double z, double hand_angle, byte relative_flags, double time, byte path_type, byte ease_type);
void moveTo(double x, double y,double z) {
moveToOpts(x, y, z, 0, F_HAND_RELATIVE, 2.0, PATH_LINEAR, INTERP_EASE_INOUT_CUBIC);
}
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);
}
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);
}
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);
}

void moveToAtOnce(double x, double y, double z) {
moveToOpts(x, y, z, 0, F_HAND_RELATIVE, 0.0, PATH_LINEAR, INTERP_LINEAR);
}
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);
}

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);

double getCalX() {
return g_cal_x;
}
double getCalY() {
return g_cal_y;
}
double getCalZ() {
return g_cal_z;
}
void getCalXYZ(double& x, double& y, double &z) {
calXYZ(); x = g_cal_x; y = g_cal_y; z = g_cal_z;
}
void getCalXYZ(double theta_1, double theta_2, double theta_3, double& x, double& y, double &z) {
calXYZ(theta_1, theta_2, theta_3); x = g_cal_x; y = g_cal_y; z = g_cal_z;
}

void calAngles(double x, double y, double z, double& theta_1, double& theta_2, double& theta_3);

void calXYZ(double theta_1, double theta_2, double theta_3);
void calXYZ();

void gripperCatch();
void gripperRelease();
void interpolate(double start_val, double end_val, double (&interp_vals)[INTERP_INTVLS], byte ease_type);
void pumpOn();
void pumpOff();

Servo g_servo_rot;
Servo g_servo_left;
Servo g_servo_right;
Servo g_servo_hand_rot;
Servo g_servo_hand;

protected:
void attachAll();
void attachServo(byte servo_num);
double cur_rot;
double cur_left;
double cur_right;
double cur_hand;
double calYonly(double theta_1, double theta_2, double theta_3);

double g_cal_x;
double g_cal_y;
double g_cal_z;

boolean g_gripper_reset;
void attachAll();
void attachServo(byte servo_num);
double cur_rot;
double cur_left;
double cur_right;
double cur_hand;
double calYonly(double theta_1, double theta_2, double theta_3);

double g_cal_x;
double g_cal_y;
double g_cal_z;

boolean g_gripper_reset;

};

Expand Down

0 comments on commit c17a4c0

Please sign in to comment.