char* Scan::scanServo() {
int i = 0;
for(i = 0; i <= 180 ; i += INCRANGLE ) {
writeAngle(i);
}
for(i = 180; i >= 0 ; i -= INCRANGLE ) {
writeAngle(i);
}
return scanner->data;
}
void uArmClass::moveToOpts(double x, double y, double z, double hand_angle, byte relative_flags, double time, byte path_type, byte ease_type) {
attachAll();
// find current position using cached servo values
double current_x;
double current_y;
double current_z;
getCalXYZ(cur_rot, cur_left, cur_right, current_x, current_y, current_z);
// deal with relative xyz positioning
byte posn_relative = (relative_flags & F_POSN_RELATIVE) ? 1 : 0;
x = current_x * posn_relative + x;
y = current_y * posn_relative + y;
z = current_z * posn_relative + z;
// find target angles
double tgt_rot;
double tgt_left;
double tgt_right;
calAngles(x, y, z, tgt_rot, tgt_left, tgt_right);
// deal with relative hand orientation
if (relative_flags & F_HAND_RELATIVE) {
hand_angle += cur_hand; // rotates a relative amount, ignoring base rotation
} else if (relative_flags & F_HAND_ROT_REL) {
hand_angle = hand_angle + cur_hand + (tgt_rot - cur_rot); // rotates relative to base servo, 0 value keeps an object aligned through movement
}
if (time > 0) {
if (path_type == PATH_ANGLES) {
// we will calculate angle value targets
double rot_array[INTERP_INTVLS];
double left_array[INTERP_INTVLS];
double right_array[INTERP_INTVLS];
double hand_array[INTERP_INTVLS];
interpolate(cur_rot, tgt_rot, rot_array, ease_type);
interpolate(cur_left, tgt_left, left_array, ease_type);
interpolate(cur_right, tgt_right, right_array, ease_type);
interpolate(cur_hand, hand_angle, hand_array, ease_type);
for (byte i = 0; i < INTERP_INTVLS; i++)
{
writeAngle(rot_array[i], left_array[i], right_array[i], hand_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);
//writeLeftRightServoAngle(left_array[i], right_array[i], true);
//writeServoAngle(SERVO_HAND_ROT_NUM, hand_array[i],true);
delay(time * 1000 / INTERP_INTVLS);
}
} else if (path_type == PATH_LINEAR) {
// we will calculate linear path targets
double x_array[INTERP_INTVLS];
double y_array[INTERP_INTVLS];
double z_array[INTERP_INTVLS];
double hand_array[INTERP_INTVLS];
interpolate(current_x, x, x_array, ease_type);
interpolate(current_y, y, y_array, ease_type);
interpolate(current_z, z, z_array, ease_type);
interpolate(cur_hand, hand_angle, hand_array, ease_type);
for (byte i = 0; i < INTERP_INTVLS; i++)
{
// double rot;
double rot,left, right;
calAngles(x_array[i], y_array[i], z_array[i], rot, left, right);
// Serial.println(rot);
// Serial.println(left);
// Serial.println(right);
// Serial.println();
//writeServoAngle(SERVO_ROT_NUM, rot,true);
// writeServoAngle(SERVO_LEFT_NUM, tgt_left,true);
// writeServoAngle(SERVO_RIGHT_NUM, tgt_right,true);
//writeLeftRightServoAngle(left, right, true);
// if(enable_hand)
//writeServoAngle(SERVO_HAND_ROT_NUM, hand_array[i], true);
writeAngle(rot, left, right, hand_array[i]);
delay(time * 1000 / INTERP_INTVLS);
}
}
}
// set final target position at end of interpolation or "atOnce"
//writeServoAngle(SERVO_ROT_NUM, tgt_rot, true);
// writeServoAngle(SERVO_LEFT_NUM, tgt_left, true);
// 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);
}
