File content as of revision 0:a043c98470ae:

#include "mbed.h"
#include "leg_message.h"
#include "math_ops.h"

// Master CAN ID ///
#define CAN_ID 0x0

/// Value Limits ///
#define P_MIN -12.5f
#define P_MAX 12.5f
#define V_MIN -65.0f
#define V_MAX 65.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

/// Joint Soft Stops ///
#define A_LIM_P 1.5f
#define A_LIM_N -1.5f
#define H_LIM_P 5.0f
#define H_LIM_N -5.0f
#define K_LIM_P 0.2f
#define K_LIM_N 7.7f
#define KP_SOFTSTOP 100.0f
#define KD_SOFTSTOP 0.4f;

DigitalOut led(PC_5);

Serial       pc(PA_2, PA_3);
CAN          can1(PB_12, PB_13, 1000000);  // CAN Rx pin name, CAN Tx pin name //CAN5 on board
CAN          can2(PA_11, PA_12, 1000000);  // CAN Rx pin name, CAN Tx pin name //CAN2 on board
CAN          can3(PA_8, PA_15, 1000000);  // CAN Rx pin name, CAN Tx pin name //CAN4 on board

CANMessage   rxMsg1, rxMsg2;
CANMessage   txMsg1, txMsg2;
CANMessage   q1_can, q2_can, q3_can;    //TX Messages
int                     ledState;
Ticker                  sendCAN;
int                     counter = 0;
volatile bool           msgAvailable = false;
Ticker loop;

leg_state l1_state, l2_state;;
leg_control l1_control, l2_control;

/// 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, joint_control joint){
     
     /// limit data to be within bounds ///
     float p_des = fminf(fmaxf(P_MIN, joint.p_des), P_MAX);                    
     float v_des = fminf(fmaxf(V_MIN, joint.v_des), V_MAX);
     float kp = fminf(fmaxf(KP_MIN, joint.kp), KP_MAX);
     float kd = fminf(fmaxf(KD_MIN, joint.kd), KD_MAX);
     float t_ff = fminf(fmaxf(T_MIN, joint.t_ff), T_MAX);
     /// convert floats to unsigned ints ///
     uint16_t p_int = float_to_uint(p_des, P_MIN, P_MAX, 16);            
     uint16_t v_int = float_to_uint(v_des, V_MIN, V_MAX, 12);
     uint16_t kp_int = float_to_uint(kp, KP_MIN, KP_MAX, 12);
     uint16_t kd_int = float_to_uint(kd, KD_MIN, KD_MAX, 12);
     uint16_t 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;
     }
     
/// CAN Reply Packet Structure ///
/// 16 bit position, between -4*pi and 4*pi
/// 12 bit velocity, between -30 and + 30 rad/s
/// 12 bit current, between -40 and 40;
/// CAN Packet is 5 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], current[11-8]]
/// 4: [current[7-0]]

void unpack_reply(CANMessage msg, leg_state *leg){
    /// unpack ints from can buffer ///
    uint16_t id = msg.data[0];
    uint16_t p_int = (msg.data[1]<<8)|msg.data[2];
    uint16_t v_int = (msg.data[3]<<4)|(msg.data[4]>>4);
    uint16_t i_int = ((msg.data[4]&0xF)<<8)|msg.data[5];
    /// convert uints to floats ///
    float p = uint_to_float(p_int, P_MIN, P_MAX, 16);
    float v = uint_to_float(v_int, V_MIN, V_MAX, 12);
    float t = uint_to_float(i_int, -T_MAX, T_MAX, 12);
    
    if(id==1){
        leg->a.p = p;
        leg->a.v = v;
        leg->a.t = t;
        }
    else if(id==2){
        leg->h.p = p;
        leg->h.v = v;
        leg->h.t = t;
        }
    else if(id==3){
        leg->k.p = p;
        leg->k.v = v;
        leg->k.t = t;
        }
    } 

void rxISR1() {
    can1.read(rxMsg1);                    // read message into Rx message storage
    unpack_reply(rxMsg1, &l1_state);
}
void rxISR2(){
    can2.read(rxMsg2);
    unpack_reply(rxMsg2, &l2_state);
}

void WriteAll(){
    //toggle = 1;
    int w1 = can1.write(q1_can);
    pc.printf("CAN1 Write: %d\n", w1);
    wait(.00002);
    int w2 = can2.write(q2_can);
    pc.printf("CAN2 Write: %d\n", w2);
    wait(.00002);
    int w3 = can3.write(q3_can);
    pc.printf("CAN3 Write: %d\n", w3);
    wait(.00002);
    //toggle = 0;
    }
    
void Random(CANMessage *msg){
    msg->data[0] = 0xF1;
    msg->data[1] = 0xF2;
    msg->data[2] = 0xF3;
    msg->data[3] = 0xF4;
    msg->data[4] = 0xF5;
    msg->data[5] = 0xF6;
    msg->data[6] = 0xF7;
    msg->data[7] = 0xF8;
    WriteAll();
    }
    
void Random2(CANMessage *msg){
    msg->data[0] = 0xF8;
    msg->data[1] = 0xF7;
    msg->data[2] = 0xF6;
    msg->data[3] = 0xF5;
    msg->data[4] = 0xF4;
    msg->data[5] = 0xF3;
    msg->data[6] = 0xF2;
    msg->data[7] = 0xF1;
    WriteAll();
    }

void Random3(CANMessage *msg){
    msg->data[0] = 0xF7;
    msg->data[1] = 0xF8;
    msg->data[2] = 0xF7;
    msg->data[3] = 0xF8;
    msg->data[4] = 0xF7;
    msg->data[5] = 0xF8;
    msg->data[6] = 0xF7;
    msg->data[7] = 0xF8;
    WriteAll();
    }

int main() {
    //setup the PC baud rate
    pc.baud(115200);

    //setup the CAN buffers
    can1.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); //set up can filter
    can2.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); //set up can filter
    can3.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); //set up can filter

    //set random messages
    Random(&q1_can);
    Random2(&q2_can);
    Random3(&q3_can);
    //write these
    while(1) {
        WriteAll(); 
        wait(.25);
        //pc.printf("Published New...\n");   
        
        //IF WE WANT
        /*
        can2.read(rxMsg2);
        unpack_reply(rxMsg2, &l2_state);
        can1.read(rxMsg1);                    // read message into Rx message storage
        unpack_reply(rxMsg1, &l1_state);
        wait_us(10);
        */
    }
}