uint8_t init_body(int direction, double length){ double move_x = -length*cos(degree_to_rad(direction))/2.0; double move_y = length*sin(degree_to_rad(direction))/2.0; int i,a; int y_multi = 1; uint16_t max = 0; for(i = 1; i <= 6; ++i){ a = current_step%10; if(current_step%10 == 5){ current_step = 0; a = 0; } if(a != 0 && (i == 1 || i == 4 || i == 5)){ a = (current_step + 5)%10; } y_multi = (i % 2 == 0 ? 1 : -1); max = get_max_2(ik(step_multi[a][0]*move_x, y_multi*step_multi[a][1]*move_y, step_multi[a][2]*LIFT_HEIGHT, i), max); } if(current_step % 10 != 0) current_step++; SERVO_ACTION; robot_delay2(max); return current_step%10; }
void rotate_body(int direction, double length){ double move_x = -length*cos(degree_to_rad(direction))/2.0; double move_y = length*sin(degree_to_rad(direction))/2.0; double move_x1 = -length*cos(degree_to_rad(180 - direction))/2.0; double move_y1 = length*sin(degree_to_rad(180 - direction))/2.0; int i,a; int y_multi = 1; uint16_t max = 0; for(i = 1; i <= 6; ++i){ a = current_step%10; if(i == 1 || i == 4 || i == 5){ a = (current_step + 5)%10; } y_multi = (i % 2 == 0 ? 1 : -1); if(i % 2 == 1){ max = get_max_2(ik(step_multi[a][0]*move_x, y_multi*step_multi[a][1]*move_y, step_multi[a][2]*LIFT_HEIGHT, i), max); }else if(i % 2 == 0){ max = get_max_2(ik(step_multi[a][0]*move_x1, y_multi*step_multi[a][1]*move_y1, step_multi[a][2]*LIFT_HEIGHT, i), max); } } current_step++; SERVO_ACTION; robot_delay2(max); }
CSUtils::ENUPoint CSUtils::ccsa_to_enu(const CCSAPoint& _point, const Angle& _eps, const Angle& _beta) { double eps = degree_to_rad(_eps); double beta = degree_to_rad(_beta); Eigen::Matrix3d C_1 = Eigen::Matrix3d::Zero(); C_1(0, 0) = cos(beta); C_1(0, 2) = -sin(beta); C_1(1, 1) = 1.0; C_1(2, 0) = sin(beta); C_1(2, 2) = cos(beta); Eigen::Matrix3d C_2 = Eigen::Matrix3d::Zero(); C_2(0, 0) = cos(eps); C_2(0, 1) = sin(eps); C_2(1, 0) = -sin(eps); C_2(1, 1) = cos(eps); C_2(2, 2) = 1.0; Eigen::Vector3d ret = (C_2 * C_1).transpose() * Eigen::Vector3d(_point.x, _point.y, _point.z); return CSUtils::ENUPoint(ret(0), ret(1), ret(2)); }