//
// Created by 24871 on 2024/10/2.
//

#ifndef CUSTOMER_CONTROLLER_KINEMATICS_H
#define CUSTOMER_CONTROLLER_KINEMATICS_H

//连杆偏移
#define LINK_OFFSET1 0.076
#define LINK_OFFSET2 0.116
#define LINK_OFFSET3 0
#define LINK_OFFSET4 0.1295

//连杆长度
#define LINK_LENGTH1 0
#define LINK_LENGTH2 0
#define LINK_LENGTH3 0
#define LINK_LENGTH4 0.036

//连杆扭角
#define LINK_TWIST1 -1.570796
#define LINK_TWIST2 1.570796
#define LINK_TWIST3 -1.570796
#define LINK_TWIST4 0

typedef struct
{
    double offset;      //d
    double length;       //a
    double twist;    //alpha
    double angle;  //theta
    double angle_set;
    double angle_set2;
    double transformation_matrix[4][4];
} Link_Parameter_s;


void Kinematic_Init();
void Kinematic_Calculate();

#endif //CUSTOMER_CONTROLLER_KINEMATICS_H