6족보행로봇 제작과정 및 원리설명(2)

leg_IK 함수

void leg_IK(int leg_number, float X, float Y, float Z)
{
    //compute target femur-to-toe (L3) length
    L0 = sqrt(sq(X) + sq(Y)) - COXA_LENGTH;
    L3 = sqrt(sq(L0) + sq(Z));

    //process only if reach is within possible range (not too long or too short!)
    if ((L3 < (TIBIA_LENGTH + FEMUR_LENGTH)) && (L3 > (TIBIA_LENGTH - FEMUR_LENGTH)))
    {
        //compute tibia angle
        phi_tibia = acos((sq(FEMUR_LENGTH) + sq(TIBIA_LENGTH) - sq(L3)) / (2 * FEMUR_LENGTH * TIBIA_LENGTH));
        theta_tibia = phi_tibia * RAD_TO_DEG - 23.0 + TIBIA_CAL[leg_number];
        theta_tibia = constrain(theta_tibia, 0.0, 180.0);

        //compute femur angle
        gamma_femur = atan2(Z, L0);
        phi_femur = acos((sq(FEMUR_LENGTH) + sq(L3) - sq(TIBIA_LENGTH)) / (2 * FEMUR_LENGTH * L3));
        theta_femur = (phi_femur + gamma_femur) * RAD_TO_DEG + 14.0 + 90.0 + FEMUR_CAL[leg_number];
        theta_femur = constrain(theta_femur, 0.0, 180.0);

        //compute coxa angle
        theta_coxa = atan2(X, Y) * RAD_TO_DEG + COXA_CAL[leg_number];

        //output to the appropriate leg
        switch (leg_number)
        {
        case 0:
            if (leg1_IK_control == true)                       //flag for IK or manual control of leg
            {
                theta_coxa = theta_coxa + 45.0;                 //compensate for leg mounting
                theta_coxa = constrain(theta_coxa, 0.0, 180.0);
                coxa1_servo.write(int(theta_coxa));
                femur1_servo.write(int(theta_femur));
                tibia1_servo.write(int(theta_tibia));
            }
            break;
            
        // case 1,2,3,4,5 생략
        
        }
    }
}

 

앞에서 유도했던 역기구학 계산 식을 통해 X,Y,Z 입력값이 주어지면 움직여야하는 관절각을 계산하는 함수입니다.

위 함수에서 case를 나눈 이유는 로봇의 구조상 world frame(로봇의 정중앙에 있는 좌표계)을 기준으로 다리가 다 회전되어있기 때문입니다.

 

 

tripod_gait 함수

void tripod_gait()
{
    //if commands more than deadband then process
    if ((abs(commandedX) > 15) || (abs(commandedY) > 15) || (abs(commandedR) > 15) || (tick > 0))
    {
        compute_strides();
        numTicks = round(duration / FRAME_TIME_MS / 2.0); //total ticks divided into the two cases
        for (leg_num = 0; leg_num < 6; leg_num++)
        {
            compute_amplitudes();
            switch (tripod_case[leg_num])
            {
            case 1:                               //move foot forward (raise and lower)
                current_X[leg_num] = HOME_X[leg_num] - amplitudeX * cos(M_PI * tick / numTicks);
                current_Y[leg_num] = HOME_Y[leg_num] - amplitudeY * cos(M_PI * tick / numTicks);
                current_Z[leg_num] = HOME_Z[leg_num] + abs(amplitudeZ) * sin(M_PI * tick / numTicks);
                if (tick >= numTicks - 1) tripod_case[leg_num] = 2;
                break;
            case 2:                               //move foot back (on the ground)
                current_X[leg_num] = HOME_X[leg_num] + amplitudeX * cos(M_PI * tick / numTicks);
                current_Y[leg_num] = HOME_Y[leg_num] + amplitudeY * cos(M_PI * tick / numTicks);
                current_Z[leg_num] = HOME_Z[leg_num];
                if (tick >= numTicks - 1) tripod_case[leg_num] = 1;
                break;
            }
        }
        //increment tick
        if (tick < numTicks - 1) tick++;
        else tick = 0;
    }
}

보행궤적을 만드는 함수입니다. 큰 그림을 설명드리면, cos, sin함수를 이용해 그 위의 점을 여러 개 찍고 순서대로 이동함으로써 보행궤적을 그립니다. 

처음에numTicks를 계산하는 데 이는 얼마만큼 많은 점을 찍을 것이냐를 결정하는 변수입니다.

duration값은 compute_strides함수에서 정의하는 데 normal mode는 1080입니다.(Slow mode = 3240, 많은 점을 찍어 천천히 이동) 그리고FRAME_TIME_MS는 20인데, 이는 서보모터가 위치를 옮기는 데 걸리는 시간이 20ms가 걸리기 때문에 이렇게 설정한 것입니다. 그리고 제 생각에 2를 나눈 이유는 보행이 swing phase(발이 공중에 있는 구간)와 stance phase(발이 지면에 닿아있는 구간)으로 2구간으로 나뉘어져있기 때문인 것 같습니다.

 

개미의 보행

다리가 6개인 개미의 보행사진입니다. 하나의 다리가 앞으로 가있을 때 옆에 다리는 뒤로 가있습니다.

따라서

case 1current_X, current_Y변수에서는 -를 쓴거고, HOME_X[leg_num] - amplitudeX * cos(M_PI * tick / numTicks)

case2 current_X, current_Y변수에서는 +를 쓴 것입니다. HOME_X[leg_num] + amplitudeX * cos(M_PI * tick / numTicks)

 

 

---

PS3 컨트롤러를 이용해 로봇 조종하기

원래는 USB shield를 아두이노 장착하고 usb dongle을 끼운다음 통신할려고 했는데(이렇게 되면 아두이노에서 바로 정보를 받기 때문에 간편함) usb dongle이 호환이 안되는 건지 작동이 안돼서 포기하고, esp32를 활용하는 방법으로 바꾸었습니다. 

 

아래에 esp32관련 문서를 참고하였습니다.

 

notify함수

void notify() {


    //왼쪽 스틱 값 반환
    if (abs(Ps3.event.analog_changed.stick.lx) + abs(Ps3.event.analog_changed.stick.ly) > 2) {
        left_x = Ps3.data.analog.stick.lx;
        left_y = Ps3.data.analog.stick.ly;
        Serial1.print("TRANS/"); Serial.print(left_x); Serial.print("/"); Serial.print(left_y); Serial.print("/");
    }
    //오른쪽 스틱 값 반환
    if (abs(Ps3.event.analog_changed.stick.rx) + abs(Ps3.event.analog_changed.stick.ry) > 2) {
        right_x = Ps3.data.analog.stick.rx;
        Serial1.print("ROTATE/"); Serial.print(right_x); Serial.print("/");
    }

    if (Ps3.event.button_down.select) Serial1.print("SEL/");
    if (Ps3.event.button_down.start) Serial1.print("START/");
    if (Ps3.event.button_down.ps) Serial1.print("Ps/");

    if (Ps3.event.button_down.triangle) Serial1.print("TRI/");
    if (Ps3.event.button_down.circle) Serial1.print("CIR/");
    if (Ps3.event.button_down.cross) Serial1.print("CRO/");
    if (Ps3.event.button_down.square) Serial1.print("SQU/");

    if (Ps3.event.button_down.up) Serial1.print("UP/");
    if (Ps3.event.button_down.right) Serial1.print("RIGHT/");
    if (Ps3.event.button_down.down) Serial1.print("DOWN/");
    if (Ps3.event.button_down.left) Serial1.print("LEFT/");
}

notify함수는 라이브러리 제작자가 제공해주는 함수입니다. Ps3컨트롤러의 아날로그 스틱의 값을 읽어주고 왼쪽 아날로그 스틱이 병진운동 조종스틱이므로 TRANS/라는 구분자를 삽입하였고, 오른쪽 아날로그 스틱은 회전운동 조종스틱이므로 ROTATE/라는 구분자를 삽입하였습니다. 그리고 각각의 값마다 /를 삽입하여 값을 구분하도록 하였습니다.

위 코드를 보시다시피 나머지 버튼들도 명령어와 /를 합쳐 처리하였습니다. 

 

여기서 Serial1은 아두이노와 esp32간의 직렬통신을 위해 설정한 것입니다. setup함수에서 보실 수 있습니다.

void setup() {
    Serial.begin(115200); //필요없는 데 디버깅을 위해서
    Serial1.begin(115200, SERIAL_8N1, RX, TX); //아두이노와 통신하기 위한 설정
    Ps3.attach(notify);
    Ps3.attachOnConnect(onConnect);
    Ps3.begin("70:b8:f6:44:03:9e"); // esp32의 MAC주소를 넣으시오.
}

 

 

---

<참고자료>

https://github.com/jvpernis/esp32-ps3

GitHub - jvpernis/esp32-ps3: Control your ESP32 projects with a PS3 controller!