File content as of revision 7:e9086c72bb22:

#include "function_CAN.h"

// CAN ID Setting Variables
int CID_RX_CMD = 100;
int CID_RX_REF_POSITION = 200;
int CID_RX_REF_TORQUE = 300;
int CID_RX_REF_PRES_DIFF = 400;
int CID_RX_REF_VOUT = 500;
int CID_RX_REF_VALVE_POSITION = 600;

int CID_TX_INFO = 1100;
int CID_TX_POSITION = 1200;
int CID_TX_TORQUE = 1300;
int CID_TX_PRES = 1400;
int CID_TX_VOUT = 1500;
int CID_TX_VALVE_POSITION = 1600;

// Board Information 
int BNO = 0;
int CONTROL_MODE = 0;
double P_GAIN_JOINT_POSITION = 0.0;
double I_GAIN_JOINT_POSITION = 0.0;
double D_GAIN_JOINT_POSITION = 0.0;
double P_GAIN_JOINT_TORQUE = 0.0;
double I_GAIN_JOINT_TORQUE = 0.0;
double D_GAIN_JOINT_TORQUE = 0.0;


void ReadCMD(char CMD)
{
    switch(CMD){
    case CRX_ASK_INFO:
//        CAN_TX_INFO();
        break;
    case CRX_ASK_BNO:
//        CAN_TX_BNO();
        break;
    default:
        break;
    }
}

void CAN_RX_HANDLER()
{
    can.read(msg);
    unsigned int address = msg.id;
    
    if(address==CID_RX_CMD){
        unsigned int CMD = msg.data[0];
        ReadCMD(CMD);
    } else if(address==CID_RX_REF_POSITION) {
        int32_t temp_pos = (int32_t) (msg.data[0] | msg.data[1] << 8 | msg.data[2] << 16 | msg.data[3] << 24);
        int32_t temp_vel = (int32_t) (msg.data[4] | msg.data[5] << 8 | msg.data[6] << 16 | msg.data[7] << 24);
        pos.ref = (double)temp_pos;
        vel.ref = (double)temp_vel;
    } else if(address==CID_RX_REF_TORQUE) {
        int16_t temp_torq = (int16_t) (msg.data[0] | msg.data[1] << 8);
        torq.ref = (double)temp_torq;
    } else if(address==CID_RX_REF_PRES_DIFF) {
//        int16_t temp_presdiff = (int16_t) (msg.data[0] | msg.data[1] << 8);
//        torq.ref = (double)temp_presdiff;   
    } else if(address==CID_RX_REF_VOUT) {
        int16_t temp_PWM = (int16_t) (msg.data[0] | msg.data[1] << 8);
        Vout.ref = (double)temp_PWM;   
    } else if(address==CID_RX_REF_VALVE_POSITION) {
        int16_t temp_cur = (int16_t) (msg.data[0] | msg.data[1] << 8);
        cur.ref = (double)temp_cur; 
    }
        
}

/******************************************************************************
 Information Transmission Functions
*******************************************************************************/
/*
inline void CAN_TX_INFO(void) {
    CANMessage temp_msg;
    
    temp_msg.id = CID_TX_INFO;
    temp_msg.len = 7;
    temp_msg.data[0] = (unsigned char) CTX_SEND_INFO;
    temp_msg.data[1] = (unsigned char) BNO;
    temp_msg.data[2] = (unsigned char) CAN_FREQ;
    temp_msg.data[3] = (unsigned char) (CAN_FREQ >> 8);
    temp_msg.data[4] = (unsigned char) (flag_err[7] << 7 | flag_err[6] << 6 | flag_err[5] << 5 | flag_err[4] << 4 | flag_err[3] << 3 | flag_err[2] << 2 | flag_err[1] << 1 | flag_err[0]);
    temp_msg.data[5] = (unsigned char) CONTROL_MODE;
    temp_msg.data[6] = (unsigned char) OPERATING_MODE;

    can.write(temp_msg);
}

inline void CAN_TX_BNO(void) {
    CANMessage temp_msg;
    
    temp_msg.id = CID_TX_INFO;
    temp_msg.len = 2;
    temp_msg.data[0] = (unsigned char) CTX_SEND_BNO;
    temp_msg.data[1] = (unsigned char) BNO;

    can.write(temp_msg);
}

inline void CAN_TX_OPERATING_MODE(void) {
    CANMessage temp_msg;
    
    temp_msg.id = CID_TX_INFO;
    temp_msg.len = 2;
    temp_msg.data[0] = (unsigned char) CTX_SEND_OPERATING_MODE;
    temp_msg.data[1] = (unsigned char) OPERATING_MODE;

    can.write(temp_msg);
}


inline void CAN_TX_CAN_FREQ(void) {
    CANMessage temp_msg;

    temp_msg.id = CID_TX_INFO;
    temp_msg.len = 3;
    temp_msg.data[0] = (uint8_t) CTX_SEND_CAN_FREQ;
    temp_msg.data[1] = (uint8_t) CAN_FREQ;
    temp_msg.data[2] = (uint8_t) (CAN_FREQ >> 8);

    can.write(temp_msg);
}
*/
inline void CAN_TX_CONTROL_MODE(void) {
    CANMessage temp_msg;

    temp_msg.id = CID_TX_INFO;
    temp_msg.len = 2;
    temp_msg.data[0] = (unsigned char) CTX_SEND_CONTROL_MODE;
    temp_msg.data[1] = (unsigned char) CONTROL_MODE;

    can.write(temp_msg);
}
/*
inline void CAN_TX_JOINT_ENC_DIR(void) {
    CANMessage temp_msg;

    temp_msg.id = CID_TX_INFO;
    temp_msg.len = 3;
    temp_msg.data[0] = (unsigned char) CTX_SEND_JOINT_ENC_DIR;
    temp_msg.data[1] = (unsigned char) DIR_JOINT_ENC;
    temp_msg.data[2] = (unsigned char) (DIR_JOINT_ENC >> 8);

    can.write(temp_msg);
}

inline void CAN_TX_VALVE_DIR(void) {
    CANMessage temp_msg;

    temp_msg.id = CID_TX_INFO;
    temp_msg.len = 3;
    temp_msg.data[0] = (unsigned char) CTX_SEND_VALVE_DIR;
    temp_msg.data[1] = (unsigned char) DIR_VALVE;
    temp_msg.data[2] = (unsigned char) (DIR_VALVE >> 8);

    can.write(temp_msg);
}

inline void CAN_TX_VALVE_ENC_DIR(void) {
    CANMessage temp_msg;

    temp_msg.id = CID_TX_INFO;
    temp_msg.len = 3;
    temp_msg.data[0] = (unsigned char) CTX_SEND_VALVE_ENC_DIR;
    temp_msg.data[1] = (unsigned char) DIR_VALVE_ENC;
    temp_msg.data[2] = (unsigned char) (DIR_VALVE_ENC >> 8);

    can.write(temp_msg);
}

inline void CAN_TX_VOLTAGE_SUPPLY(void) {
    long send_voltage_supply = (long) (SUPPLY_VOLTAGE * 10);

    CANMessage temp_msg;

    temp_msg.id = CID_TX_INFO;
    temp_msg.len = 3;
    temp_msg.data[0] = (unsigned char) CTX_SEND_VOLTAGE_SUPPLY;
    temp_msg.data[1] = (unsigned char) (send_voltage_supply);
    temp_msg.data[2] = (unsigned char) (send_voltage_supply >> 8);

    can.write(temp_msg);
}

inline void CAN_TX_VOLTAGE_VALVE(void) {
    long send_voltage_valve = (long) (VALVE_VOLTAGE_LIMIT * 10);

    CANMessage temp_msg;

    temp_msg.id = CID_TX_INFO;
    temp_msg.len = 3;
    temp_msg.data[0] = (unsigned char) CTX_SEND_VOLTAGE_VALVE;
    temp_msg.data[1] = (unsigned char) send_voltage_valve;
    temp_msg.data[2] = (unsigned char) (send_voltage_valve >> 8);

    can.write(temp_msg);
}
*/

inline void CAN_TX_PID_GAIN(int t_type) {
    // t_type = 0 : valve position control gain
    // t_type = 1 : joint position control gain
    // t_type = 2 : joint torque control gain

    long sendPgain=0, sendIgain=0, sendDgain=0;
    if (t_type == 0) {
//        sendPgain = (long) (P_GAIN_VALVE_POSITION);
//        sendIgain = (long) (I_GAIN_VALVE_POSITION);
//        sendDgain = (long) (D_GAIN_VALVE_POSITION);
    } else if (t_type == 1) {
        sendPgain = (long) (P_GAIN_JOINT_POSITION);
        sendIgain = (long) (I_GAIN_JOINT_POSITION);
        sendDgain = (long) (D_GAIN_JOINT_POSITION);
    } else if (t_type == 2) {
        sendPgain = (long) (P_GAIN_JOINT_TORQUE);
        sendIgain = (long) (I_GAIN_JOINT_TORQUE);
        sendDgain = (long) (D_GAIN_JOINT_TORQUE);
    }

    CANMessage temp_msg;
    
    temp_msg.id = CID_TX_INFO;
    temp_msg.len = 8;
    temp_msg.data[0] = (unsigned char) CTX_SEND_PID_GAIN;
    temp_msg.data[1] = (unsigned char) t_type;
    temp_msg.data[2] = (unsigned char) sendPgain;
    temp_msg.data[3] = (unsigned char) (sendPgain >> 8);
    temp_msg.data[4] = (unsigned char) sendIgain;
    temp_msg.data[5] = (unsigned char) (sendIgain >> 8);
    temp_msg.data[6] = (unsigned char) sendDgain;
    temp_msg.data[7] = (unsigned char) (sendDgain >> 8);

    can.write(temp_msg);
}

/*
inline void CAN_TX_VALVE_DEADZONE(void) {
    CANMessage temp_msg;

    temp_msg.id = CID_TX_INFO;
    temp_msg.len = 7;
    temp_msg.data[0] = (unsigned char) CTX_SEND_VALVE_DEADZONE;
    temp_msg.data[1] = (unsigned char) (int) (VALVE_CENTER);
    temp_msg.data[2] = (unsigned char) ((int) (VALVE_CENTER) >> 8);
    temp_msg.data[3] = (unsigned char) (int) (VALVE_DEADZONE_PLUS);
    temp_msg.data[4] = (unsigned char) ((int) (VALVE_DEADZONE_PLUS) >> 8);
    temp_msg.data[5] = (unsigned char) (int) (VALVE_DEADZONE_MINUS);
    temp_msg.data[6] = (unsigned char) ((int) (VALVE_DEADZONE_MINUS) >> 8);

    can.write(temp_msg);
}

inline void CAN_TX_ENC_PULSE_PER_POSITION(void) {
    CANMessage temp_msg;

    temp_msg.id = CID_TX_INFO;
    temp_msg.len = 3;
    temp_msg.data[0] = (unsigned char) CTX_SEND_ENC_PULSE_PER_POSITION;
    temp_msg.data[1] = (unsigned char) ENC_PULSE_PER_POSITION;
    temp_msg.data[2] = (unsigned char) (ENC_PULSE_PER_POSITION >> 8);

    can.write(temp_msg);
}

inline void CAN_TX_TORQUE_SENSOR_PULSE_PER_TORQUE(void) {
    CANMessage temp_msg;

    temp_msg.id = CID_TX_INFO;
    temp_msg.len = 3;
    temp_msg.data[0] = (unsigned char) CTX_SEND_TORQUE_SENSOR_PULSE_PER_TORQUE;
    temp_msg.data[1] = (unsigned char) TORQUE_SENSOR_PULSE_PER_TORQUE;
    temp_msg.data[2] = (unsigned char) (TORQUE_SENSOR_PULSE_PER_TORQUE >> 8);

    can.write(temp_msg);
}

inline void CAN_TX_PRES_SENSOR_PULSE_PER_PRES(void) {
    CANMessage temp_msg;

    temp_msg.id = CID_TX_INFO;
    temp_msg.len = 5;
    temp_msg.data[0] = (unsigned char) CTX_SEND_PRES_SENSOR_PULSE_PER_BAR;
    temp_msg.data[1] = (unsigned char) (int) (PRES_SENSOR_A_PULSE_PER_BAR * 100.);
    temp_msg.data[2] = (unsigned char) ((int) (PRES_SENSOR_A_PULSE_PER_BAR * 100.) >> 8);
    temp_msg.data[3] = (unsigned char) (int) (PRES_SENSOR_B_PULSE_PER_BAR * 100.);
    temp_msg.data[4] = (unsigned char) ((int) (PRES_SENSOR_B_PULSE_PER_BAR * 100.) >> 8);

    can.write(temp_msg);
}

inline void CAN_TX_HOMEPOS_OFFSET(void) {
    long send_homepos_offset;
    send_homepos_offset = (long) (HOMEPOS_OFFSET);

    CANMessage temp_msg;

    temp_msg.id = CID_TX_INFO;
    temp_msg.len = 3;
    temp_msg.data[0] = (unsigned char) CTX_SEND_HOMEPOS_OFFSET;
    temp_msg.data[1] = (unsigned char) send_homepos_offset;
    temp_msg.data[2] = (unsigned char) (send_homepos_offset >> 8);

    can.write(temp_msg);
}
*/
/******************************************************************************
 Sensor & State Transmission Functions
*******************************************************************************/

inline void CAN_TX_POSITION(long t_pos, long t_vel) {
    CANMessage temp_msg;

    temp_msg.id = CID_TX_POSITION;
    temp_msg.len = 8;
    temp_msg.data[0] = (unsigned char) t_pos;
    temp_msg.data[1] = (unsigned char) (t_pos >> 8);
    temp_msg.data[2] = (unsigned char) (t_pos >> 16);
    temp_msg.data[3] = (unsigned char) (t_pos >> 24);
    temp_msg.data[4] = (unsigned char) t_vel;
    temp_msg.data[5] = (unsigned char) (t_vel >> 8);
    temp_msg.data[6] = (unsigned char) (t_vel >> 16);
    temp_msg.data[7] = (unsigned char) (t_vel >> 24);

    can.write(temp_msg);
}

inline void CAN_TX_TORQUE(int16_t t_torque) {
    CANMessage temp_msg;

    temp_msg.id = CID_TX_TORQUE;
    temp_msg.len = 2;
    temp_msg.data[0] = (unsigned char) t_torque;
    temp_msg.data[1] = (unsigned char) (t_torque >> 8);

    can.write(temp_msg);
}

inline void CAN_TX_PRES(int16_t t_pres_a, int16_t t_pres_b) {
    CANMessage temp_msg;

    temp_msg.id = CID_TX_PRES;
    temp_msg.len = 8;
    temp_msg.data[0] = (unsigned char) t_pres_a;
    temp_msg.data[1] = (unsigned char) (t_pres_a >> 8);
    temp_msg.data[2] = (unsigned char) t_pres_b;
    temp_msg.data[3] = (unsigned char) (t_pres_b >> 8);

    can.write(temp_msg);
}

inline void CAN_TX_PWM(int16_t t_pwm) {
    CANMessage temp_msg;

    temp_msg.id = CID_TX_POSITION;
    temp_msg.len = 8;
    temp_msg.data[0] = (unsigned char) t_pwm;
    temp_msg.data[1] = (unsigned char) (t_pwm >> 8);

    can.write(temp_msg);
}

inline void CAN_TX_VALVE_POSITION(int16_t t_valve_pos) {
    CANMessage temp_msg;

    temp_msg.id = CID_TX_VALVE_POSITION;
    temp_msg.len = 8;
    temp_msg.data[0] = (unsigned char) t_valve_pos;
    temp_msg.data[1] = (unsigned char) (t_valve_pos >> 8);

    can.write(temp_msg);
}