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