File content as of revision 6:f51261d3fe8f:

#define CAN_ID 0x0

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


Serial       pc(PA_2, PA_3);
CAN          can(PB_8, PB_9, 1000000);      // CAN Rx pin name, CAN Tx pin name
CANMessage   rxMsg;
CANMessage   txMsg;
int                     ledState;
Timer                   timer;
Ticker                  sendCAN;
int                     counter = 0;
volatile bool           msgAvailable = false;
Ticker loop;

 float theta1, theta2, dtheta1, dtheta2;

float command_f[5];
float data_f[3];
DigitalOut led(PC_5);
/// Value Limits ///
/*
#define P_MIN -95.5f
 #define P_MAX 95.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 I_MIN -18.0f
 #define I_MAX 18.0f
 */
//#define P_MIN -12.5f
//#define P_MAX 12.5f
 #define P_MIN 0.04f
 #define P_MAX 6.24f
 //#define V_MIN -45.0f
 //#define V_MAX 45.0f
 #define V_MIN -15.0f
 #define V_MAX 15.0f
 #define KP_MIN 0.0f
 #define KP_MAX 500.0f
 #define KD_MIN 0.0f
 #define KD_MAX 5.0f
 //#define I_MIN -18.0f
 //#define I_MAX 18.0f
  #define I_MIN -50.0f
 #define I_MAX 50.0f
/// 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]]

union f_b
{
float f;
uint8_t bytes[4];
};

f_b p_act;
f_b v_act;
f_b i_act;
int can_i;

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(I_MIN, t_ff), I_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, I_MIN, I_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){
    /// unpack ints from can buffer ///
    int id = msg.data[0];
    int p_int = (msg.data[1]<<8)|msg.data[2];
    int v_int = (msg.data[3]<<4)|(msg.data[4]>>4);
    int i_int = ((msg.data[4]&0xF)<<8)|msg.data[5];
    /// convert ints 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 i = uint_to_float(i_int, -I_MAX, I_MAX, 12);
    //printf("%d  %f\n\r", id, p);
    
    can_i = id;
    p_act.f = p;
    v_act.f = v;
    i_act.f = i;
    
    
    //printf("%d  %.3f   %.3f   %.3f\n\r", id, p, v, i);
    } 
    
 void onMsgReceived() {
    can.read(rxMsg);                    // read message into Rx message storage
    unpack_reply(rxMsg);
}

void enable_motor(int id)
{
    txMsg.data[0] = 0xFF;
    txMsg.data[1] = 0xFF;
    txMsg.data[2] = 0xFF;
    txMsg.data[3] = 0xFF;
    txMsg.data[4] = 0xFF;
    txMsg.data[5] = 0xFF;
    txMsg.data[6] = 0xFF;
    txMsg.data[7] = 0xFC;  
    can.write(txMsg); 
}
void disable_motor(int id)
{
    txMsg.data[0] = 0xFF;
    txMsg.data[1] = 0xFF;
    txMsg.data[2] = 0xFF;
    txMsg.data[3] = 0xFF;
    txMsg.data[4] = 0xFF;
    txMsg.data[5] = 0xFF;
    txMsg.data[6] = 0xFF;
    txMsg.data[7] = 0xFD;  
    can.write(txMsg); 
}

void sendCMD(){
    pack_cmd(&txMsg, 0, 0, 0, 0, -0.1f); 
    //pack_cmd(&txMsg2, theta1, dtheta1, 10, .1, 0);
    //pack_cmd(&txMsg2, 0, 0, 0, 0, 1.0);
    //pack_cmd(&txMsg1, 0, 0, 0, 0, 1.0);
    //can.write(txMsg2);
    wait(.0003);        // Give motor 1 time to respond.
    can.write(txMsg);
    //txMsg1.id = txMsg1.id+1;
    }
    
void serial_isr(void)
{
    int b1 = pc.getc();
    if(b1==0xFF){
        int n = 9;
        int bytes[n];
        bytes[0] = b1;
        for(int i = 1; i<n; i++)
        {
            bytes[i] = pc.getc();
        }
        if(((bytes[0]==0xFF) & (bytes[1]==0xFF) & (bytes[2]==0xFF) & (bytes[3]==0xFF) & (bytes[4]==0xFF) & (bytes[5]==0xFF) & (bytes[6]==0xFF) & (bytes[7]==0xFC)))
            {enable_motor(bytes[8]);}
        else if(((bytes[0]==0xFF) & (bytes[1]==0xFF) & (bytes[2]==0xFF) & (bytes[3]==0xFF) & (bytes[4]==0xFF) & (bytes[5]==0xFF) & (bytes[6]==0xFF) & (bytes[7]==0xFD)))
            {disable_motor(bytes[8]);}
    }
    else
    {
        int n = 21;
        int bytes[n];
        bytes[0] = b1;
    
        for(int i = 1; i<n; i++)
        {
            bytes[i] = pc.getc();
        }
    
        
        f_b p_des;
        f_b v_des;
        f_b kp;
        f_b kd;
        f_b i_ff;
        
        
        for(int i = 0; i<4; i++)
        {
            p_des.bytes[i] = bytes[1+i];
            v_des.bytes[i] = bytes[5+i];
            kp.bytes[i] = bytes[9+i];
            kd.bytes[i] = bytes[13+i];
            i_ff.bytes[i] = bytes[17+i];
        }
        
        while(pc.readable()){char flush = pc.getc();}
        
        command_f[0] = p_des.f;
        command_f[1] = v_des.f;
        command_f[2] = kp.f;
        command_f[3] = kd.f;
        command_f[4] = i_ff.f;
        
        txMsg.id = bytes[0];
        pack_cmd(&txMsg, p_des.f, v_des.f, kp.f, kd.f, i_ff.f);
        can.write(txMsg);
        wait_us(10);
        int tx_bytes[13];
        tx_bytes[0] = can_i;
        for(int i = 0; i<4; i++)
        {
         tx_bytes[1+i] = p_act.bytes[i];
         tx_bytes[5+i] = v_act.bytes[i];
         tx_bytes[9+i] = i_act.bytes[i];   
        }
        
        for(int i = 0; i<13; i++)
        {
            pc.putc(tx_bytes[i]);
        }

    }
    
    
}
    
int can_packet[8] = {1, 2, 3, 4, 5, 6, 7, 8};
int main() {
    pc.baud(921600);
    pc.attach(&serial_isr);
    can.attach(&onMsgReceived);                 // attach 'CAN receive-complete' interrupt handler
    can.filter(CAN_ID , 0xFFF, CANStandard, 0);
    //printf("%d\n\r", 1 << 18);
    int count = 0;
    txMsg.len = 8;                         //transmit 8 bytes
    rxMsg.len = 6;                          //receive 5 bytes
    //loop.attach(&sendCMD, .001);
    txMsg.id = 0x1;                        //1st motor ID
    pack_cmd(&txMsg, 0, 0, 0, 0, 0);       //Start out by sending all 0's
    timer.start();
    
    while(1) {
        //printf("%d\n\r", txMsg.data[0]);
        wait(0.5);
        led = !led;
        }
        
    }