encode.cpp

#include <cstdio>

#define P_MIN   (-12.5f)
#define P_MAX   (12.5f)
#define V_MIN   (-45.0f)
#define V_MAX   (45.0f)
#define KP_MIN  (0.0f)
#define KP_MAX  (500.0f)
#define KD_MIN  (0.0f)
#define KD_MAX  (5.0f)
#define T_MIN   (-18.0f)
#define T_MAX   (18.0f)
#define I_MAX   (40.0f)

using namespace std;

inline
float max(float const x, float const y)
{
    return x > y ? x : y;
}

inline
float min(float const x, float const y)
{
    return x > y ? y : x;
}

inline
int float_to_uint(float const x, float const x_min, float const x_max, int const bits)
    // Converts a float to an unsigned int, given range and number of bits
{
    float const span = x_max - x_min;
    float const offset = x_min;
    return static_cast<int>((x - offset) * static_cast<float>((1 << bits) - 1) / span);
}

inline
float uint_to_float(int const x_int, float const x_min, float const x_max, int const bits)
    // converts unsigned int to float, given range and number of bits
{
    float const span = x_max - x_min;
    float const offset = x_min;
    return (static_cast<float>(x_int)) * span / static_cast<float>((1 << bits) - 1) + offset;
}

#if 0
float fmaxf(float x, float y){
    /// Returns maximum of x, y ///
    return (((x)>(y))?(x):(y));
}
float fminf(float x, float y){
    /// Returns minimum of x, y ///
    return (((x)<(y))?(x):(y));
}
/// CAN Command Packet Structure ///
/// 16 bit position command, between -4*pi and 4*pi
/// 12 bit velocity command, between -30 and + 30 rad/s
/// 12 bit kp, between 0 and 500 N-m/rad
/// 12 bit kd, between 0 and 100 N-m*s/rad
/// 12 bit feed forward torque, between -18 and 18 N-m
/// CAN Packet is 8 8-bit words
/// Formatted as follows. For each quantity, bit 0 is LSB
/// 0: [position[15-8]] 1: [position[7-0]] 2: [velocity[11-4]] 3: [velocity[3-0], kp[11-8]] 4: [kp[7-0]] 5: [kd[11-4]] 6: [kd[3-0], torque[11-8]] 7: [torque[7-0]]
void pack_cmd(CANMessage *msg, float p_des, float v_des, float kp, float kd, float t_ff) {
    /// limit data to be within bounds ///
    p_des = fminf(fmaxf(P_MIN, p_des), P_MAX);                    
    v_des = fminf(fmaxf(V_MIN, v_des), V_MAX);
    kp = fminf(fmaxf(KP_MIN, kp), KP_MAX);
    kd = fminf(fmaxf(KD_MIN, kd), KD_MAX);
    t_ff = fminf(fmaxf(T_MIN, t_ff), T_MAX);
    /// convert floats to unsigned ints ///
    int p_int  = float_to_uint(p_des, P_MIN, P_MAX, 16);            
    int v_int  = float_to_uint(v_des, V_MIN, V_MAX, 12);
    int kp_int = float_to_uint(kp, KP_MIN, KP_MAX, 12);
    int kd_int = float_to_uint(kd, KD_MIN, KD_MAX, 12);
    int t_int  = float_to_uint(t_ff, T_MIN, T_MAX, 12);
    /// pack ints into the can buffer ///
    msg->data[0] = p_int>>8;                                       
    msg->data[1] = p_int&0xFF;
    msg->data[2] = v_int>>4;
    msg->data[3] = ((v_int&0xF)<<4)|(kp_int>>8);
    msg->data[4] = kp_int&0xFF;
    msg->data[5] = kd_int>>4;
    msg->data[6] = ((kd_int&0xF)<<4)|(t_int>>8);
    msg->data[7] = t_int&0xff;
}
#endif

void pack_cmd(unsigned char (*msg)[8], float p_des, float v_des, float kp, float kd, float t_ff)
{
    p_des = min(max(P_MIN, p_des), P_MAX);                    
    v_des = min(max(V_MIN, v_des), V_MAX);
    kp = min(max(KP_MIN, kp), KP_MAX);
    kd = min(max(KD_MIN, kd), KD_MAX);
    t_ff = min(max(T_MIN, t_ff), T_MAX);
    int const p_int  = float_to_uint(p_des, P_MIN, P_MAX, 16);
    int const v_int  = float_to_uint(v_des, V_MIN, V_MAX, 12);
    int const kp_int = float_to_uint(kp, KP_MIN, KP_MAX, 12);
    int const kd_int = float_to_uint(kd, KD_MIN, KD_MAX, 12);
    int const t_int  = float_to_uint(t_ff, T_MIN, T_MAX, 12);
    (*msg)[0] = p_int>>8;
    (*msg)[1] = p_int&0xFF;
    (*msg)[2] = v_int>>4;
    (*msg)[3] = ((v_int&0xF)<<4)|(kp_int>>8);
    (*msg)[4]= kp_int&0xFF;
    (*msg)[5] = kd_int>>4;
    (*msg)[6] = ((kd_int&0xF)<<4)|(t_int>>8);
    (*msg)[7] = t_int&0xff;
}

void encode(unsigned char (*msg)[8], int p_int, int v_int, int kp_int, int kd_int, int t_int)
{
    (*msg)[0] = p_int>>8;
    (*msg)[1] = p_int&0xFF;
    (*msg)[2] = v_int>>4;
    (*msg)[3] = ((v_int&0xF)<<4)|(kp_int>>8);
    (*msg)[4] = kp_int&0xFF;
    (*msg)[5] = kd_int>>4;
    (*msg)[6] = ((kd_int&0xF)<<4)|(t_int>>8);
    (*msg)[7] = t_int&0xFF;
}

int main(void)
{
    unsigned char const data[3][8] = {
        {0x7C, 0xA5, 0x96, 0xB0, 0x7A, 0x99, 0x97, 0xFF},
        {0x7C, 0xED, 0x96, 0xB0, 0x7A, 0x99, 0x95, 0xC6},
        {0x7F, 0xBA, 0x7F, 0xF0, 0x39, 0x00, 0x07, 0x8D},
    };
    unsigned char buf[3][8];
    unsigned int p_int[3], v_int[3], kp_int[3], kd_int[3], t_int[3], i, j;
    float p[3], v[3], kp[3], kd[3], t_ff[3];

    // Decoding the encoded data
    for (i = 0; i < 3; i++) {
        p_int[i] = (data[i][0] << 8) | (data[i][1]);
        v_int[i] = (data[i][2] << 4) | (data[i][3] >> 4);
        kp_int[i] = ((data[i][3] & 0x0F) << 8) | (data[i][4]);
        kd_int[i] = (data[i][5] << 4) | (data[i][6] >> 4);
        t_int[i] = ((data[i][6] & 0x0F) << 8) | (data[i][7]);
    }

    // Printing the decoded int-values
    for (i = 0; i < 3; i++) {
        printf("%d %d %d %d %d\n", p_int[i], v_int[i], kp_int[i], kd_int[i], t_int[i]);
    }
    // 31909 2411 122 2457 2047
    // 31981 2411 122 2457 1478
    // 32698 2047 57 0 1933

    // Encoding the decoded int-values
    for (i = 0; i < 3; i++) {
        encode(&buf[i], p_int[i], v_int[i], kp_int[i], kd_int[i], t_int[i]);
    }

    // Checking them well-decoded
    for (i = 0; i < 3; i++) {
        printf("%2X %2X %2X %2X %2X %2X %2X %2X\n", buf[i][0], buf[i][1], buf[i][2], buf[i][3], buf[i][4], buf[i][5], buf[i][6], buf[i][7]);
    }
    // 7C A5 96 B0 7A 99 97 FF
    // 7C ED 96 B0 7A 99 95 C6
    // 7F BA 7F F0 39  0  7 8D

    // Calculating the original float-values
    for (i = 0; i < 3; i++) {
        p[i] = uint_to_float(p_int[i], P_MIN, P_MAX, 16);
        v[i] = uint_to_float(v_int[i], V_MIN, V_MAX, 12);
        kp[i] = uint_to_float(kp_int[i], KP_MIN, KP_MAX, 12);
        kd[i] = uint_to_float(kd_int[i], KD_MIN, KD_MAX, 12);
        t_ff[i] = uint_to_float(t_int[i], T_MIN, T_MAX, 12);
    }

    // Printing the original float-values
    for (i = 0; i < 3; i++) {
        printf("%f %f %f %f %f\n", p[i], v[i], kp[i], kd[i], t_ff[i]);
    }
    // -0.327497 7.989010 14.896214 3.000000 -0.004396
    // -0.300031 7.989010 14.896214 3.000000 -5.006594
    // -0.026512 -0.010990 6.959707 0.000000 -1.006594
    
    // Packing them
    for (i = 0; i < 3; i++) {
        pack_cmd(&buf[i], p[i], v[i], kp[i], kd[i], t_ff[i]);
    }
    
    // Checking well-packed
    for (i = 0; i < 3; i++) {
        for (j = 0; j < 8; j++) {
            printf("%2X ", buf[i][j]);
        }
        printf("\n");
    }
    // 7C A5 96 B0 7A 99 97 FF
    // 7C ED 96 B0 7A 99 95 C6 
    // 7F BA 7F F0 39  0  7 8D

    return 0;
}