Code: Select all
#include <Servo.h>
#include <math.h>
#include <FlexiTimer2.h> //타이머
#include <SerialCommand.h>
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
SerialCommand SCmd;
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
//좌표값을 받기 위한 구조체
struct Point
{
float x; //x좌표
float y; //y좌표
int mark; //꼬리 상하모터 제어를 위한 그리기 진행상황
};
//BANNED 3개 서보모터
Servo tailUpDownServo; //꼬리 상하모터
Servo tailServo1; //꼬리 첫번째 좌우모터
Servo tailServo2; //꼬리 두번째 좌우모터
//BANNED 3개 모터 핀번호 설정
int tailUpDownPin = 4; //꼬리 상하모터
int tailServoPin1 = 5; //꼬리 첫번째 좌우모터
int tailServoPin2 = 6; //꼬리 두번째 좌우모터
//초음파 센서 핀번호 설정
#define ECHO 7
#define TRIG 8
//로봇의 사이즈
const float length_a = 55; //다리 가운데 프레임 길이
const float length_b = 77.5; //다리 끝 프레임 길이
const float length_c = 27.5; //다리 몸체쪽 프레임 길이
const float length_side = 71; //몸체 왼쪽,오른쪽 다리사이 거리
const float length_tail1 = 60; //꼬리 첫번째 프레임 길이
const float length_tail2 = 61; //꼬리 두번째 프레임 길이
//움직임을 위한 BANNED
const float z_default = -50, z_up = -30, z_boot = -28; //z축 기준과 움직일 BANNED
const float x_default = 62, x_offset = 0; //x축 기준과 움직일 BANNED
const float y_start = 0, y_step = 40; //y축 기준과 움직일 BANNED
//움직임을 위한 BANNED
volatile float site_now[4][3]; //실제 다리의 위치
volatile float site_expect[4][3]; //움직일 예상 다리의 위치
float temp_speed[4][3]; //움직이기 전에 재계산될 각 축의 속도
float move_speed; //움직이는 속도 BANNED
float speed_multiple = 1; //움직일 속도의 BANNED
float spot_turn_speed = 4; //회전 속도
float leg_move_speed = 8; //다리 속도
float body_move_speed = 3; //몸체 속도
const float stand_seat_speed = 1; //앉았다 일어나는 속도
volatile int rest_counter; //+1/0.02s, 자동으로 휴식
const float KEEP = 255; //움직이지 않기위한 BANNED
const float pi = 3.1415926; //파이
//회전을 위한 BANNED///////////////////////////////////////////////////////////////////////
//현재 다리와 움직일 예상 다리의 길이들을 구한 뒤 제2코사인 법칙을 이용하여 움직일 각도를 구하기
//제2코사인 법칙 : 각 변의 길이가 a, b, c 일때 사잇각의 각도 구하기
//cos(alpha) = (a^2 + b^2 - c^2)/2ab
const float temp_a = sqrt(pow(2 * x_default + length_side, 2) + pow(y_step, 2));
const float temp_b = 2 * (y_start + y_step) + length_side;
const float temp_c = sqrt(pow(2 * x_default + length_side, 2) + pow(2 * y_start + y_step + length_side, 2));
const float temp_alpha = acos((pow(temp_a, 2) + pow(temp_b, 2) - pow(temp_c, 2)) / 2 / temp_a / temp_b);
//위의 값들을 이용해서 회전할때 움직일 범위구하기
const float turn_x1 = (temp_a - length_side) / 2;
const float turn_y1 = y_start + y_step / 2;
const float turn_x0 = turn_x1 - temp_b * cos(temp_alpha);
const float turn_y0 = temp_b * sin(temp_alpha) - turn_y1 - length_side;
///////////////////////////////////////////////////////////////////////////////////////
//그리기 관련 BANNED
Point ptArray[70]; //Point형식을 저장하기 위한 배열
int count = 0; //Point배열 보관 카운트
float theta1; //꼬리 첫번째 좌우모터 각도
float theta2; //꼬리 두번째 좌우모터 각도
float up_down_theta; //꼬리 상하모터 각도
//자동주행모드 on/off
bool autoSet = true;
void setup()
{
Serial.begin(9600); //시리얼 통신
Serial.println("Starts initialization");
//명령 등록
SCmd.addCommand("m", moving_action_cmd);
SCmd.addCommand("p", painting_action_cmd);
SCmd.addCommand("q", start_painting_cmd);
SCmd.addCommand("s", sp_control);
SCmd.addCommand("a", auto_drive_cmd);
//올바른 명령이 들어오지 않았을때 BANNED 호출
SCmd.setDefaultHandler(unrecognized);
pwm.begin();
pwm.setPWMFreq(66); // Analog servos run at ~60 Hz updates
yield();
//몸체 초기값 설정
set_site(0, x_default - x_offset, y_start + y_step, z_boot);
set_site(1, x_default - x_offset, y_start + y_step, z_boot);
set_site(2, x_default + x_offset, y_start, z_boot);
set_site(3, x_default + x_offset, y_start, z_boot);
for (int i = 0; i < 4; i++)
{
for (int j = 0; j < 3; j++)
{
site_now[i][j] = site_expect[i][j];
}
}
//초음파센서 핀등록
pinMode(TRIG, OUTPUT);
pinMode(ECHO, INPUT);
//타이머 시작
FlexiTimer2::set(1, servo_service);
FlexiTimer2::start();
Serial.println("Servo service started");
//서보모터 연결
servo_attach();
Serial.println("Servos initialized");
Serial.println("Initialization Complete");
}
void servo_attach(void)
{
//꼬리 모터 연결
tailServo1.attach(tailServoPin1);
tailServo2.attach(tailServoPin2);
tailUpDownServo.attach(tailUpDownPin);
pinMode(tailServoPin1, OUTPUT);
pinMode(tailServoPin2, OUTPUT);
pinMode(tailUpDownPin, OUTPUT);
//꼬리 모터 초기값 설정
tailServo1.write(90);
tailServo2.write(0);
tailUpDownServo.write(5);
}
void loop()
{
SCmd.readSerial();
}
//속도 제어
void sp_control()
{
char* arg;
int tempsp;
arg=SCmd.next();
tempsp= atoi(arg);
if(tempsp==1)
{
leg_move_speed =3;
body_move_speed=1;
spot_turn_speed = 1;
}
else if(tempsp==2)
{
leg_move_speed =5;
body_move_speed=2;
spot_turn_speed = 2;
}
else if(tempsp==3)
{
leg_move_speed =7;
body_move_speed=3;
spot_turn_speed = 3;
}
else if(tempsp==4)
{
leg_move_speed = 9;
body_move_speed = 4;
spot_turn_speed = 4;
}
else if(tempsp==5)
{
leg_move_speed = 11;
body_move_speed = 5;
spot_turn_speed = 5;
}
}
// m 0 1: stand
// m 0 0: sit
// m 1 x: forward x step
// m 2 x: back x step
// m 3 x: right turn x step
// m 4 x: left turn x step
#define M_STAND_SIT 0
#define M_FORWARD 1
#define M_BACKWARD 2
#define M_LEFT 3
#define M_RIGHT 4
void moving_action_cmd(void)
{
char *arg;
int action_mode, n_step;
Serial.println("Action:");
arg = SCmd.next();
action_mode = atoi(arg);
Serial.print("action_mode : ");
Serial.println(action_mode);
arg = SCmd.next();
n_step = atoi(arg);
Serial.print("n_step : ");
Serial.println(n_step);
switch (action_mode)
{
case M_FORWARD:
Serial.println("Step forward");
if (!is_stand())
stand();
step_forward(n_step);
break;
case M_BACKWARD:
Serial.println("Step back");
if (!is_stand())
stand();
step_back(n_step);
break;
case M_LEFT:
Serial.println("Turn left");
if (!is_stand())
stand();
turn_left(n_step);
break;
case M_RIGHT:
Serial.println("Turn right");
if (!is_stand())
stand();
turn_right(n_step);
break;
case M_STAND_SIT:
Serial.println("1:up,0:dn");
if (n_step)
stand();
else
sit();
break;
default:
Serial.println("Error");
break;
}
}
//받은 좌표들을 저장
void painting_action_cmd(void)
{
String arg;
arg = SCmd.next(); //x좌표 받기
ptArray[count].x = (arg).toFloat();
Serial.println(ptArray[count].x);
arg = SCmd.next(); //y좌표 받기
ptArray[count].y = (arg).toFloat();
Serial.println(ptArray[count].y);
arg = SCmd.next(); //mark 받기
if(arg == "s")
{
ptArray[count].mark = 1;
Serial.println(ptArray[count].mark);
count++;
}
else if(arg == "i")
{
ptArray[count].mark = 0;
Serial.println(ptArray[count].mark);
count++;
}
else if(arg == "f")
{
ptArray[count].mark = -1;
Serial.println(ptArray[count].mark);
count++;
}
}
//저장된 좌표를 이용해서 그리기 시작
void start_painting_cmd(void)
{
float temp_th1 = 90; //꼬리 첫번째 좌우모터 각도를 계산하기 위한 임시 BANNED
float temp_th2 = 0; //꼬리 두번째 좌우모터 각도를 계산하기 위한 임시 BANNED
Serial.println("start_painting");
delay(1000);
for (int i = 0; i < count; ++i)
{
//BANNED 공식 이용
//원하는 지점의 좌표값을 알고있을 때 두개의 좌우모터의 각도를 계산하는 공식
temp_th2 = 2 * atan(sqrt(((length_tail1 + length_tail2)*(length_tail1 + length_tail2) - (ptArray[i].x * ptArray[i].x + ptArray[i].y * ptArray[i].y)) / ((ptArray[i].x * ptArray[i].x) + (ptArray[i].y * ptArray[i].y) - (length_tail1 - length_tail2)*(length_tail1 - length_tail2))));
temp_th1 = atan2(ptArray[i].y, ptArray[i].x) - atan(length_tail2*sin(temp_th2) / (length_tail1 + length_tail2*cos(temp_th2)));
//호도법을 60분법으로 변경
theta1 = temp_th1 * 180 / PI;
theta2 = temp_th2 * 180 / PI;
//모터 움직이기
tailServo2.write(theta2);
delay(10);
tailServo1.write(theta1);
delay(10);
if (ptArray[i].mark == 1) //그리기의 시작일때 꼬리 상하모터 내리기
{
delay(500);
up_down_theta = 0;
tailUpDownServo.write(up_down_theta);
delay(500);
}
else if (ptArray[i].mark == -1) //그리기의 종료일때 꼬리 상하모터 올리기
{
delay(500);
up_down_theta = 5;
tailUpDownServo.write(up_down_theta);
delay(500);
}
delay(200);
}
//그리기가 끝난 후 초기화
for (int i = 0; i < count; ++i) //배열 초기화
{
Point pt;
ptArray[i] = pt;
}
count = 0; //카운트 초기화
theta1 = 90;
tailServo1.write(theta1); //원래 초기 각도로
delay(10);
theta2 = 0;
tailServo2.write(theta2); //원래 초기 각도로
delay(10);
}
//자동 주행 모드
void auto_drive_cmd(void)
{
int distance = 0;
while(autoSet)
{
Serial.println("auto_driving_start");
char* temp;
digitalWrite(TRIG,HIGH); //초음파 출력
delayMicroseconds(10);
digitalWrite(TRIG,LOW); //받기 전에 끄기
distance = pulseIn(ECHO,HIGH)/58.2; //거리 받아오기
Serial.println(distance);
delay(200);
if(distance <= 15) //장애물의 거리가 15cm 보다 가까울때
{
step_back(4);
delay(500);
turn_right(4);
delay(500);
autoSet==false;
}
step_forward(2);
delay(100);
}
}
//설정과 다른 명령을 입력했을때 호출
void unrecognized(const char *command) {
Serial.println("No Match Command!");
}
//앉았는지 안앉았는지 확인
bool is_stand(void)
{
if (site_now[0][2] == z_default)
return true;
else
return false;
}
void sit(void)
{
move_speed = stand_seat_speed;
for (int leg = 0; leg < 4; leg++)
{
set_site(leg, KEEP, KEEP, z_boot);
}
wait_all_reach();
}
void stand(void)
{
move_speed = stand_seat_speed;
for (int leg = 0; leg < 4; leg++)
{
set_site(leg, KEEP, KEEP, z_default);
}
wait_all_reach();
}
void turn_left(unsigned int step)
{
move_speed = spot_turn_speed;
while (step-- > 0)
{
if (site_now[3][1] == y_start)
{
//leg 3&1 move
set_site(3, x_default + x_offset, y_start, z_up);
wait_all_reach();
set_site(0, turn_x1 - x_offset, turn_y1, z_default);
set_site(1, turn_x0 - x_offset, turn_y0, z_default);
set_site(2, turn_x1 + x_offset, turn_y1, z_default);
set_site(3, turn_x0 + x_offset, turn_y0, z_up);
wait_all_reach();
set_site(3, turn_x0 + x_offset, turn_y0, z_default);
wait_all_reach();
set_site(0, turn_x1 + x_offset, turn_y1, z_default);
set_site(1, turn_x0 + x_offset, turn_y0, z_default);
set_site(2, turn_x1 - x_offset, turn_y1, z_default);
set_site(3, turn_x0 - x_offset, turn_y0, z_default);
wait_all_reach();
set_site(1, turn_x0 + x_offset, turn_y0, z_up);
wait_all_reach();
set_site(0, x_default + x_offset, y_start, z_default);
set_site(1, x_default + x_offset, y_start, z_up);
set_site(2, x_default - x_offset, y_start + y_step, z_default);
set_site(3, x_default - x_offset, y_start + y_step, z_default);
wait_all_reach();
set_site(1, x_default + x_offset, y_start, z_default);
wait_all_reach();
}
else
{
//leg 0&2 move
set_site(0, x_default + x_offset, y_start, z_up);
wait_all_reach();
set_site(0, turn_x0 + x_offset, turn_y0, z_up);
set_site(1, turn_x1 + x_offset, turn_y1, z_default);
set_site(2, turn_x0 - x_offset, turn_y0, z_default);
set_site(3, turn_x1 - x_offset, turn_y1, z_default);
wait_all_reach();
set_site(0, turn_x0 + x_offset, turn_y0, z_default);
wait_all_reach();
set_site(0, turn_x0 - x_offset, turn_y0, z_default);
set_site(1, turn_x1 - x_offset, turn_y1, z_default);
set_site(2, turn_x0 + x_offset, turn_y0, z_default);
set_site(3, turn_x1 + x_offset, turn_y1, z_default);
wait_all_reach();
set_site(2, turn_x0 + x_offset, turn_y0, z_up);
wait_all_reach();
set_site(0, x_default - x_offset, y_start + y_step, z_default);
set_site(1, x_default - x_offset, y_start + y_step, z_default);
set_site(2, x_default + x_offset, y_start, z_up);
set_site(3, x_default + x_offset, y_start, z_default);
wait_all_reach();
set_site(2, x_default + x_offset, y_start, z_default);
wait_all_reach();
}
}
}
void turn_right(unsigned int step)
{
move_speed = spot_turn_speed;
while (step-- > 0)
{
if (site_now[2][1] == y_start)
{
//leg 2&0 move
set_site(2, x_default + x_offset, y_start, z_up);
wait_all_reach();
set_site(0, turn_x0 - x_offset, turn_y0, z_default);
set_site(1, turn_x1 - x_offset, turn_y1, z_default);
set_site(2, turn_x0 + x_offset, turn_y0, z_up);
set_site(3, turn_x1 + x_offset, turn_y1, z_default);
wait_all_reach();
set_site(2, turn_x0 + x_offset, turn_y0, z_default);
wait_all_reach();
set_site(0, turn_x0 + x_offset, turn_y0, z_default);
set_site(1, turn_x1 + x_offset, turn_y1, z_default);
set_site(2, turn_x0 - x_offset, turn_y0, z_default);
set_site(3, turn_x1 - x_offset, turn_y1, z_default);
wait_all_reach();
set_site(0, turn_x0 + x_offset, turn_y0, z_up);
wait_all_reach();
set_site(0, x_default + x_offset, y_start, z_up);
set_site(1, x_default + x_offset, y_start, z_default);
set_site(2, x_default - x_offset, y_start + y_step, z_default);
set_site(3, x_default - x_offset, y_start + y_step, z_default);
wait_all_reach();
set_site(0, x_default + x_offset, y_start, z_default);
wait_all_reach();
}
else
{
//leg 1&3 move
set_site(1, x_default + x_offset, y_start, z_up);
wait_all_reach();
set_site(0, turn_x1 + x_offset, turn_y1, z_default);
set_site(1, turn_x0 + x_offset, turn_y0, z_up);
set_site(2, turn_x1 - x_offset, turn_y1, z_default);
set_site(3, turn_x0 - x_offset, turn_y0, z_default);
wait_all_reach();
set_site(1, turn_x0 + x_offset, turn_y0, z_default);
wait_all_reach();
set_site(0, turn_x1 - x_offset, turn_y1, z_default);
set_site(1, turn_x0 - x_offset, turn_y0, z_default);
set_site(2, turn_x1 + x_offset, turn_y1, z_default);
set_site(3, turn_x0 + x_offset, turn_y0, z_default);
wait_all_reach();
set_site(3, turn_x0 + x_offset, turn_y0, z_up);
wait_all_reach();
set_site(0, x_default - x_offset, y_start + y_step, z_default);
set_site(1, x_default - x_offset, y_start + y_step, z_default);
set_site(2, x_default + x_offset, y_start, z_default);
set_site(3, x_default + x_offset, y_start, z_up);
wait_all_reach();
set_site(3, x_default + x_offset, y_start, z_default);
wait_all_reach();
}
}
}
void step_forward(unsigned int step)
{
move_speed = leg_move_speed;
while (step-- > 0)
{
if (site_now[2][1] == y_start)
{
//leg 2&1 move
set_site(2, x_default + x_offset, y_start, z_up);
wait_all_reach();
set_site(2, x_default + x_offset, y_start + 2 * y_step, z_up);
wait_all_reach();
set_site(2, x_default + x_offset, y_start + 2 * y_step, z_default);
wait_all_reach();
move_speed = body_move_speed;
set_site(0, x_default + x_offset, y_start, z_default);
set_site(1, x_default + x_offset, y_start + 2 * y_step, z_default);
set_site(2, x_default - x_offset, y_start + y_step, z_default);
set_site(3, x_default - x_offset, y_start + y_step, z_default);
wait_all_reach();
move_speed = leg_move_speed;
set_site(1, x_default + x_offset, y_start + 2 * y_step, z_up);
wait_all_reach();
set_site(1, x_default + x_offset, y_start, z_up);
wait_all_reach();
set_site(1, x_default + x_offset, y_start, z_default);
wait_all_reach();
}
else
{
//leg 0&3 move
set_site(0, x_default + x_offset, y_start, z_up);
wait_all_reach();
set_site(0, x_default + x_offset, y_start + 2 * y_step, z_up);
wait_all_reach();
set_site(0, x_default + x_offset, y_start + 2 * y_step, z_default);
wait_all_reach();
move_speed = body_move_speed;
set_site(0, x_default - x_offset, y_start + y_step, z_default);
set_site(1, x_default - x_offset, y_start + y_step, z_default);
set_site(2, x_default + x_offset, y_start, z_default);
set_site(3, x_default + x_offset, y_start + 2 * y_step, z_default);
wait_all_reach();
move_speed = leg_move_speed;
set_site(3, x_default + x_offset, y_start + 2 * y_step, z_up);
wait_all_reach();
set_site(3, x_default + x_offset, y_start, z_up);
wait_all_reach();
set_site(3, x_default + x_offset, y_start, z_default);
wait_all_reach();
}
}
}
void step_back(unsigned int step)
{
move_speed = leg_move_speed;
while (step-- > 0)
{
if (site_now[3][1] == y_start)
{
//leg 3&0 move
set_site(3, x_default + x_offset, y_start, z_up);
wait_all_reach();
set_site(3, x_default + x_offset, y_start + 2 * y_step, z_up);
wait_all_reach();
set_site(3, x_default + x_offset, y_start + 2 * y_step, z_default);
wait_all_reach();
move_speed = body_move_speed;
set_site(0, x_default + x_offset, y_start + 2 * y_step, z_default);
set_site(1, x_default + x_offset, y_start, z_default);
set_site(2, x_default - x_offset, y_start + y_step, z_default);
set_site(3, x_default - x_offset, y_start + y_step, z_default);
wait_all_reach();
move_speed = leg_move_speed;
set_site(0, x_default + x_offset, y_start + 2 * y_step, z_up);
wait_all_reach();
set_site(0, x_default + x_offset, y_start, z_up);
wait_all_reach();
set_site(0, x_default + x_offset, y_start, z_default);
wait_all_reach();
}
else
{
//leg 1&2 move
set_site(1, x_default + x_offset, y_start, z_up);
wait_all_reach();
set_site(1, x_default + x_offset, y_start + 2 * y_step, z_up);
wait_all_reach();
set_site(1, x_default + x_offset, y_start + 2 * y_step, z_default);
wait_all_reach();
move_speed = body_move_speed;
set_site(0, x_default - x_offset, y_start + y_step, z_default);
set_site(1, x_default - x_offset, y_start + y_step, z_default);
set_site(2, x_default + x_offset, y_start + 2 * y_step, z_default);
set_site(3, x_default + x_offset, y_start, z_default);
wait_all_reach();
move_speed = leg_move_speed;
set_site(2, x_default + x_offset, y_start + 2 * y_step, z_up);
wait_all_reach();
set_site(2, x_default + x_offset, y_start, z_up);
wait_all_reach();
set_site(2, x_default + x_offset, y_start, z_default);
wait_all_reach();
}
}
}
void servo_service(void)
{
//BANNED 인터럽트 허가 받기
sei(); //인터럽트란? 컴퓨터 작동 중에 예기치 않은 문제가 발생한 경우라도 업무 처리가 계속될 BANNED 있도록 하는 컴퓨터 운영체계의 한 기능.
static float alpha, beta, gamma;
for (int i = 0; i < 4; i++)
{
for (int j = 0; j < 3; j++)
{
if (abs(site_now[i][j] - site_expect[i][j]) >= abs(temp_speed[i][j])) //실제 위치와 예상 위치의 차이가 축의 임시속도보다 크다면
site_now[i][j] += temp_speed[i][j]; //추가시켜서 늘려준다
else
site_now[i][j] = site_expect[i][j]; //아니라면 도착했으므로 예상 위치를 실제 위치에 넣는다
}
cartesian_to_polar(alpha, beta, gamma, site_now[i][0], site_now[i][1], site_now[i][2]);
polar_to_servo(i, alpha, beta, gamma);
}
rest_counter++;
}
void set_site(int leg, float x, float y, float z)
{
float length_x = 0, length_y = 0, length_z = 0;
if (x != KEEP)
length_x = x - site_now[leg][0]; //입력받은 움직일 위치에서 실제 위치를 뺀다.
if (y != KEEP)
length_y = y - site_now[leg][1];
if (z != KEEP)
length_z = z - site_now[leg][2];
float length = sqrt(pow(length_x, 2) + pow(length_y, 2) + pow(length_z, 2)); //원점에서 3차원 공간상 입력받은 가야할 좌표의 거리
temp_speed[leg][0] = length_x / length * move_speed * speed_multiple; //거리에 따라 속도가 비례
temp_speed[leg][1] = length_y / length * move_speed * speed_multiple;
temp_speed[leg][2] = length_z / length * move_speed * speed_multiple;
if (x != KEEP)
site_expect[leg][0] = x; //입력받은 움직일 위치는 예상 위치에 넣는다.
if (y != KEEP)
site_expect[leg][1] = y;
if (z != KEEP)
site_expect[leg][2] = z;
}
void wait_reach(int leg)
{
while (1) //가야할 예상 위치와 실제 위치가 같아질때까지 무한루프를 돌며 빠져나오지 않는다.
if (site_now[leg][0] == site_expect[leg][0])
if (site_now[leg][1] == site_expect[leg][1])
if (site_now[leg][2] == site_expect[leg][2])
break;
}
void wait_all_reach(void)
{
for (int i = 0; i < 4; i++)
wait_reach(i);
}
void cartesian_to_polar(volatile float &alpha, volatile float &beta, volatile float &gamma, volatile float x, volatile float y, volatile float z)
{
//calculate w-z degree
float v, w;
w = (x >= 0 ? 1 : -1) * (sqrt(pow(x, 2) + pow(y, 2)));
v = w - length_c;
alpha = atan2(z, v) + acos((pow(length_a, 2) - pow(length_b, 2) + pow(v, 2) + pow(z, 2)) / 2 / length_a / sqrt(pow(v, 2) + pow(z, 2)));
beta = acos((pow(length_a, 2) + pow(length_b, 2) - pow(v, 2) - pow(z, 2)) / 2 / length_a / length_b);
//calculate x-y-z degree
gamma = (w >= 0) ? atan2(y, x) : atan2(-y, -x);
//trans degree pi->180
alpha = alpha / pi * 180;
beta = beta / pi * 180;
gamma = gamma / pi * 180;
}
void polar_to_servo(int leg, float alpha, float beta, float gamma)
{
if (leg == 0)
{
alpha = 90 - alpha;
beta = beta;
gamma += 90;
}
else if (leg == 1)
{
alpha += 90;
beta = 180 - beta;
gamma = 90 - gamma;
}
else if (leg == 2)
{
alpha += 90;
beta = 180 - beta;
gamma = 90 - gamma;
}
else if (leg == 3)
{
alpha = 90 - alpha;
beta = beta;
gamma += 90;
}
Serial.println("leg:");
Serial.println(leg);
Serial.println("alpha:");
Serial.println(alpha);
Serial.println("beta:");
Serial.println(beta);
Serial.println("gamma:");
Serial.println(gamma);
//다리 서보모터 움직이기
float alphalen = map(alpha,0,180,150,600);
float betalen = map(beta,0,180,150,600);
float gammalen = map(gamma,0,180,150,600);
Serial.println("alphalen:");
Serial.println(alphalen);
Serial.println("betalen:");
Serial.println(betalen);
Serial.println("gammalen:");
Serial.println(gammalen);
if(leg == 0 )
{
pwm.setPWM(0,0,alphalen); //servo motor 2
pwm.setPWM(1,0,betalen); //3
pwm.setPWM(2,0,gammalen); //4
}
else if (leg==1)
{
pwm.setPWM(3,0,alphalen); //5
pwm.setPWM(4,0,betalen); //6
pwm.setPWM(5,0,gammalen); //7
}
else if (leg==2)
{
pwm.setPWM(6,0,alphalen); //8
pwm.setPWM(7,0,betalen); //9
pwm.setPWM(8,0,gammalen); //10
}
else if (leg==3)
{
pwm.setPWM(9,0,alphalen); //11
pwm.setPWM(10,0,betalen); //12
pwm.setPWM(11,0,gammalen); //13
}
}