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