Servo control more than 16

Post test messages here

Moderators: adafruit_support_bill, adafruit

Please be positive and constructive with your questions and comments.
User avatar
MOOOOON
 
Posts: 15
Joined: Fri Jan 22, 2016 8:48 pm

Re: Servo control more than 16

Post by MOOOOON »

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

That is what I'm using on arduino nano with 12 servos...
Last edited by adafruit_support_bill on Sun Jan 31, 2016 9:59 pm, edited 1 time in total.
Reason: please use the </> button when submitting code. press </>, then paste your code between the [code] [/code] tags.

User avatar
adafruit_support_bill
 
Posts: 88093
Joined: Sat Feb 07, 2009 10:11 am

Re: Servo control more than 16

Post by adafruit_support_bill »

But the robot moves super slowly
What functions are slow? You use a lot of calls to delay() in your code.

User avatar
MOOOOON
 
Posts: 15
Joined: Fri Jan 22, 2016 8:48 pm

Re: Servo control more than 16

Post by MOOOOON »

Actually, this code calls servo_service function so every seconde it makes event that servos move

I think moving slowly reason is flexitimer2 but I don't know how could I fix it
this code calculates and make the serovs moving

User avatar
MOOOOON
 
Posts: 15
Joined: Fri Jan 22, 2016 8:48 pm

Re: Servo control more than 16

Post by MOOOOON »

So timer makes to servo moving from setup part it code working after calculate pulslength

There is no delay function that one is only for the other that code is not using now

User avatar
adafruit_support_bill
 
Posts: 88093
Joined: Sat Feb 07, 2009 10:11 am

Re: Servo control more than 16

Post by adafruit_support_bill »

So timer makes to servo moving from setup part it code working after calculate pulslength
I don't understand what you mean by that.

User avatar
MOOOOON
 
Posts: 15
Joined: Fri Jan 22, 2016 8:48 pm

Re: Servo control more than 16

Post by MOOOOON »

I meant servos are moved by flexitimer

Anyway serovs move slowly and there is no delay function

Without the driver code it moves much faster I'll show you the video...

User avatar
MOOOOON
 
Posts: 15
Joined: Fri Jan 22, 2016 8:48 pm

Re: Servo control more than 16

Post by MOOOOON »

The video is too big for uploding here could I know eamil address

User avatar
adafruit_support_bill
 
Posts: 88093
Joined: Sat Feb 07, 2009 10:11 am

Re: Servo control more than 16

Post by adafruit_support_bill »

Code: Select all

  FlexiTimer2::set(1, servo_service);
You are calling servo_service every millisecond.

Code: Select all

  pwm.setPWMFreq(66);  // Analog servos run at ~60 Hz updates
But your servo update time is only every 15 milliseconds.

User avatar
MOOOOON
 
Posts: 15
Joined: Fri Jan 22, 2016 8:48 pm

Re: Servo control more than 16

Post by MOOOOON »

How do I fix it then ?

How do I control it I tried to change it
That function setpwmfreq, but if I change it the servos setting is so wired should I fix the value in setpwmfreq ??

But how ?

User avatar
adafruit_support_bill
 
Posts: 88093
Joined: Sat Feb 07, 2009 10:11 am

Re: Servo control more than 16

Post by adafruit_support_bill »

Unless you are using newer digital servos, the update frequency should be in the 50-60Hz range.

it does not make sense to do all your calculations 15 times for every actual update. You should increase your timer period to save processor time.
FlexiTimer2::set(1, servo_service);
The first parameter is the number of milliseconds.

Locked
Please be positive and constructive with your questions and comments.

Return to “Test Message Forum (closed)”