File content as of revision 4:e8c1b3f8fc6c:

//counter for misc purposes
int counter3 = 0;
                                                                                    //GO THROUGH AND RE-CHECK ALL THE VARIABLES, STRUCT NAMES, SIZES, BUFFERS + ETC!!!
                                                                                    //ALSO GO THROUGH THE COMMENTS TO SEE IF THEY NEED CHANGING

#include "mbed.h"
#include "math_ops.h"
#include <cstring>
#include "leg_message.h"

// length of receive/transmit buffers
#define RX_LEN 98                                                                   //CHECK THESE BUFFER LENGHTS
#define TX_LEN 98                                                                   //CHECK THESE BUFFER LENGHTS

// length of outgoing/incoming messages
#define DATA_LEN 44                                                                 //CHECK THESE BUFFER LENGHTS
#define CMD_LEN  98                                                                 //CHECK THESE BUFFER LENGHTS

// 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;

#define ENABLE_CMD 0xFFFF
#define DISABLE_CMD 0x1F1F

spi_data_t spi_data; // data from spine to up
spi_command_t spi_command; // data from up to spine

// spi buffers
uint16_t rx_buff[RX_LEN];
uint16_t tx_buff[TX_LEN];

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
CAN          can2(PA_11, PA_12, 1000000);  // CAN Rx pin name, CAN Tx pin name
CAN          can3(PA_8, PA_15, 1000000);  // CAN Rx pin name, CAN Tx pin name //CAN4 on board

CANMessage   rxMsg1, rxMsg2, rxMsg3;
CANMessage   txMsg1, txMsg2, txMsg3;
CANMessage   q11_can, q21_can, q31_can, q12_can, q22_can, q32_can, q13_can, q23_can, q33_can;    //TX Messages
int                     ledState;
Ticker                  sendCAN;
int                     counter = 0;
volatile bool           msgAvailable = false;
Ticker loop;

int spi_enabled = 0;
InterruptIn cs(PA_4);
DigitalIn estop(PB_15);
//SPISlave spi(PA_7, PA_6, PA_5, PA_4);


grouped_act_state g1_state, g2_state, g3_state;
grouped_act_control q1_control, q2_control, q3_control;

uint16_t x = 0;
uint16_t x2 = 0;
uint16_t count = 0;
uint16_t counter2 = 0;                                                              //SEE IF WE NEED TO UPDATE THESE TO ADD COUNTER3 AND X3

int control_mode = 1;
int is_standing = 0;                                                                //SEE IF WE STILL NEED THE STANDING THING
int enabled = 0;

// generates fake spi data from spi command
void test_control();                                                                //MAY NEED TO GET RID OF THIS?
void 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, grouped_act_state * group){
    /// 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){
        group->a.p = p;
        group->a.v = v;
        group->a.t = t;
        }
    else if(id==2){
        group->h.p = p;
        group->h.v = v;
        group->h.t = t;
        }
    else if(id==3){
        group->k.p = p;
        group->k.v = v;
        group->k.t = t;
        }
    } 

 void rxISR1() {
    can1.read(rxMsg1);                    // read message into Rx message storage
    unpack_reply(rxMsg1, &g1_state);
}
void rxISR2(){
    can2.read(rxMsg2);
    unpack_reply(rxMsg2, &g2_state);
    }
void rxISR3(){
    can3.read(rxMsg3);
    unpack_reply(rxMsg3, &g3_state);
    }
    
void PackAll(){
    pack_cmd(&q11_can, q1_control.a); 
    pack_cmd(&q21_can, q1_control.h); 
    pack_cmd(&q31_can, q1_control.k); 
    pack_cmd(&q12_can, q2_control.a); 
    pack_cmd(&q22_can, q2_control.h); 
    pack_cmd(&q32_can, q2_control.k); 
    pack_cmd(&q13_can, q3_control.a); 
    pack_cmd(&q23_can, q3_control.h); 
    pack_cmd(&q33_can, q3_control.k); 
    
    }
void WriteAll(){
    //toggle = 1;
    can1.write(q11_can);
    wait(.00002);
    can2.write(q12_can);
    wait(.00002);
    can3.write(q13_can);
    wait(.00002);
    can1.write(q21_can);
    wait(.00002);
    can2.write(q22_can);
    wait(.00002);
    can3.write(q23_can);
    wait(.00002);
    can1.write(q31_can);
    wait(.00002);
    can2.write(q32_can);
    wait(.00002);
    can3.write(q33_can);
    wait(.00002);
    //toggle = 0;
    }

void sendCMD(){
    counter ++;

    PackAll();

    if(counter>100){
        pc.printf("%.3f %.3f %.3f   %.3f %.3f %.3f   %.3f %.3f %.3f\n\r", g1_state.a.p, g1_state.h.p, g1_state.k.p, g2_state.a.p, g2_state.h.p, g2_state.k.p, g3_state.a.p, g3_state.h.p, g3_state.k.p);
        counter = 0 ;
        }
    
    WriteAll();
    }



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

void EnterMotorMode(CANMessage * msg){
    msg->data[0] = 0xFF;
    msg->data[1] = 0xFF;
    msg->data[2] = 0xFF;
    msg->data[3] = 0xFF;
    msg->data[4] = 0xFF;
    msg->data[5] = 0xFF;
    msg->data[6] = 0xFF;
    msg->data[7] = 0xFC;
    WriteAll();
    }
    
void ExitMotorMode(CANMessage * msg){
    msg->data[0] = 0xFF;
    msg->data[1] = 0xFF;
    msg->data[2] = 0xFF;
    msg->data[3] = 0xFF;
    msg->data[4] = 0xFF;
    msg->data[5] = 0xFF;
    msg->data[6] = 0xFF;
    msg->data[7] = 0xFD;
    WriteAll();
    }
void serial_isr(){
     /// handle keyboard commands from the serial terminal ///
     while(pc.readable()){
        char c = pc.getc();
        //led = !led;
        switch(c){
            case(27):
                //loop.detach();
                pc.printf("\n\r exiting motor mode \n\r");
                ExitMotorMode(&q11_can);
                ExitMotorMode(&q21_can);
                ExitMotorMode(&q31_can);
                ExitMotorMode(&q12_can);
                ExitMotorMode(&q22_can);
                ExitMotorMode(&q32_can);
                ExitMotorMode(&q13_can);
                ExitMotorMode(&q23_can);
                ExitMotorMode(&q33_can);
                enabled = 0;
                break;
            case('m'):
                pc.printf("\n\r entering motor mode \n\r");
                EnterMotorMode(&q11_can);
                EnterMotorMode(&q21_can);
                EnterMotorMode(&q31_can);
                EnterMotorMode(&q12_can);
                EnterMotorMode(&q22_can);
                EnterMotorMode(&q32_can);
                EnterMotorMode(&q13_can);
                EnterMotorMode(&q23_can);
                EnterMotorMode(&q33_can);
                wait(.5);
                enabled = 1;
                //loop.attach(&sendCMD, .001);
                break;
            case('s'):
                pc.printf("\n\r standing \n\r");
                counter2 = 0;
                is_standing = 1;
                //stand();
                break;
            case('z'):
                pc.printf("\n\r zeroing \n\r");
                Zero(&q11_can);
                Zero(&q21_can);
                Zero(&q31_can);
                Zero(&q12_can);
                Zero(&q22_can);
                Zero(&q32_can);
                Zero(&q13_can);
                Zero(&q23_can);
                Zero(&q33_can);
                break;
            }
        }
        WriteAll();
        
    }
    
uint32_t xor_checksum(uint32_t* data, size_t len)
{
    uint32_t t = 0;
    for(int i = 0; i < len; i++)   
        t = t ^ data[i];
    return t;
}



void print_SPI_command() {
    pc.printf("SPI MESSAGE RECIEVED:\n");
    
    pc.printf("MOTOR 1 Q: %f\n", spi_command.q_des_1s[0]);
    pc.printf("MOTOR 1 Qd: %f\n", spi_command.qd_des_1s[0]);
    pc.printf("MOTOR 1 Kp: %f\n", spi_command.kp_1s[0]);
    pc.printf("MOTOR 1 Kd: %f\n", spi_command.kd_1s[0]);
    pc.printf("MOTOR 1 T_FF: %f\n", spi_command.tau_1s_ff[0]);
    
    pc.printf("MOTOR 2 Q: %f\n", spi_command.q_des_2s[0]);
    pc.printf("MOTOR 2 Qd: %f\n", spi_command.qd_des_2s[0]);
    pc.printf("MOTOR 2 Kp: %f\n", spi_command.kp_2s[0]);
    pc.printf("MOTOR 2 Kd: %f\n", spi_command.kd_2s[0]);
    pc.printf("MOTOR 2 T_FF: %f\n", spi_command.tau_2s_ff[0]);
    
    pc.printf("MOTOR 3 Q: %f\n", spi_command.q_des_3s[0]);
    pc.printf("MOTOR 3 Qd: %f\n", spi_command.qd_des_3s[0]);
    pc.printf("MOTOR 3 Kp: %f\n", spi_command.kp_3s[0]);
    pc.printf("MOTOR 3 Kd: %f\n", spi_command.kd_3s[0]);
    pc.printf("MOTOR 3 T_FF: %f\n", spi_command.tau_3s_ff[0]);
    
    pc.printf("MOTOR 4 Q: %f\n", spi_command.q_des_1s[1]);
    pc.printf("MOTOR 4 Qd: %f\n", spi_command.qd_des_1s[1]);
    pc.printf("MOTOR 4 Kp: %f\n", spi_command.kp_1s[1]);
    pc.printf("MOTOR 4 Kd: %f\n", spi_command.kd_1s[1]);
    pc.printf("MOTOR 4 T_FF: %f\n", spi_command.tau_1s_ff[1]);
    
    pc.printf("MOTOR 5 Q: %f\n", spi_command.q_des_2s[1]);
    pc.printf("MOTOR 5 Qd: %f\n", spi_command.qd_des_2s[1]);
    pc.printf("MOTOR 5 Kp: %f\n", spi_command.kp_2s[1]);
    pc.printf("MOTOR 5 Kd: %f\n", spi_command.kd_2s[1]);
    pc.printf("MOTOR 5 T_FF: %f\n", spi_command.tau_2s_ff[1]);
    
    pc.printf("MOTOR 6 Q: %f\n", spi_command.q_des_3s[1]);
    pc.printf("MOTOR 6 Qd: %f\n", spi_command.qd_des_3s[1]);
    pc.printf("MOTOR 6 Kp: %f\n", spi_command.kp_3s[1]);
    pc.printf("MOTOR 6 Kd: %f\n", spi_command.kd_3s[1]);
    pc.printf("MOTOR 6 T_FF: %f\n", spi_command.tau_3s_ff[1]);
    
    pc.printf("MOTOR 7 Q: %f\n", spi_command.q_des_1s[2]);
    pc.printf("MOTOR 7 Qd: %f\n", spi_command.qd_des_1s[2]);
    pc.printf("MOTOR 7 Kp: %f\n", spi_command.kp_1s[2]);
    pc.printf("MOTOR 7 Kd: %f\n", spi_command.kd_1s[2]);
    pc.printf("MOTOR 7 T_FF: %f\n", spi_command.tau_1s_ff[2]);
    
    pc.printf("MOTOR 8 Q: %f\n", spi_command.q_des_2s[2]);
    pc.printf("MOTOR 8 Qd: %f\n", spi_command.qd_des_2s[2]);
    pc.printf("MOTOR 8 Kp: %f\n", spi_command.kp_2s[2]);
    pc.printf("MOTOR 8 Kd: %f\n", spi_command.kd_2s[2]);
    pc.printf("MOTOR 8 T_FF: %f\n", spi_command.tau_2s_ff[2]);
    
    pc.printf("MOTOR 9 Q: %f\n", spi_command.q_des_3s[2]);
    pc.printf("MOTOR 9 Qd: %f\n", spi_command.qd_des_3s[2]);
    pc.printf("MOTOR 9 Kp: %f\n", spi_command.kp_3s[2]);
    pc.printf("MOTOR 9 Kd: %f\n", spi_command.kd_3s[2]);
    pc.printf("MOTOR 9 T_FF: %f\n", spi_command.tau_3s_ff[2]);
    
}





void spi_isr(void)
{
    //pc.printf("CS ACTIVE\n");
    GPIOC->ODR |= (1 << 8);
    GPIOC->ODR &= ~(1 << 8);
    int bytecount = 0;
    SPI1->DR = tx_buff[0];
    while(cs == 0) {
        if(SPI1->SR&0x1) {
            rx_buff[bytecount] = SPI1->DR;
            bytecount++;
            if(bytecount<TX_LEN) {
                SPI1->DR = tx_buff[bytecount];
            }
        }
    }
    //pc.printf("RECIEVED: %d BYTES\n", bytecount);
    
    //pc.printf("HERE'S A SPI COMMAND:\n");
    // after reading, save into spi_command
    // should probably check checksum first!
    uint32_t calc_checksum = xor_checksum((uint32_t*)rx_buff,32);
    for(int i = 0; i < CMD_LEN; i++)
    {
        ((uint16_t*)(&spi_command))[i] = rx_buff[i];
        //pc.printf("WORD %d RECIEVED: %d\n", i, rx_buff[i]);
    }
    //print_SPI_command();
    
    // run control, which fills in tx_buff for the next iteration
    /*
    if(calc_checksum != spi_command.checksum){
        spi_data.flags[1] = 0xdead;
        pc.printf("FAILED CHECKSUM\n");
        pc.printf("ACTUAL: %d\n", calc_checksum);
        pc.printf("CURRENT: %d\n", spi_command.checksum);}
    */
        
    //test_control();
    //spi_data.q_abad[0] = 12.0f;
    control();
    PackAll();
    WriteAll();


    //for (int i = 0; i<TX_LEN; i++) {
     //   tx_buff[i] = 2*rx_buff[i];
    //}
//    for (int i=0; i<TX_LEN; i++) {
//        //printf("%d ", rx_buff[i]);
//    }
    //printf("\n\r");
}

int softstop_joint(joint_state state, joint_control * control, float limit_p, float limit_n){
    if((state.p)>=limit_p){
        //control->p_des = limit_p;
        control->v_des = 0.0f;
        control->kp = 0;
        control->kd = KD_SOFTSTOP;
        control->t_ff += KP_SOFTSTOP*(limit_p - state.p);
        return 1;
    }
    else if((state.p)<=limit_n){
        //control->p_des = limit_n;
        control->v_des = 0.0f;
        control->kp = 0;
        control->kd = KD_SOFTSTOP;
        control->t_ff += KP_SOFTSTOP*(limit_n - state.p);
        return 1;
    }
    return 0;
    
    }
    
    
void control()
{
    
    if(((spi_command.flags[0]&0x1)==1)  && (enabled==0)){
        enabled = 1;
        //BUS ONE
        EnterMotorMode(&q11_can);
        can1.write(q11_can);
        EnterMotorMode(&q21_can);
        can1.write(q21_can);
        EnterMotorMode(&q31_can);
        can1.write(q31_can);
        //BUS TWO
        EnterMotorMode(&q12_can);
        can2.write(q12_can);
        EnterMotorMode(&q22_can);
        can2.write(q22_can);
        EnterMotorMode(&q32_can);
        can2.write(q32_can);
        //BUS THREE
        EnterMotorMode(&q13_can);
        can3.write(q13_can);
        EnterMotorMode(&q23_can);
        can3.write(q23_can);
        EnterMotorMode(&q33_can);
        can3.write(q33_can);
        pc.printf("e\n\r");
        return;
    }
    else if((((spi_command.flags[0]&0x1))==0)  && (enabled==1)){
        enabled = 0;
        //BUS ONE
        ExitMotorMode(&q11_can);
        can1.write(q11_can);
        ExitMotorMode(&q21_can);
        can1.write(q21_can);
        ExitMotorMode(&q31_can);
        can1.write(q31_can);
        //BUS TWO
        ExitMotorMode(&q12_can);
        can2.write(q12_can);
        ExitMotorMode(&q22_can);
        can2.write(q22_can);
        ExitMotorMode(&q32_can);
        can2.write(q32_can);
        //BUS THREE
        ExitMotorMode(&q13_can);
        can3.write(q13_can);
        ExitMotorMode(&q23_can);
        can3.write(q23_can);
        ExitMotorMode(&q33_can);
        can3.write(q33_can);
        pc.printf("x\n\r");
        return;
        }
    
    spi_data.q_1s[0] = g1_state.a.p;
    spi_data.q_2s[0] = g1_state.h.p;
    spi_data.q_3s[0] = g1_state.k.p;
    spi_data.qd_1s[0] = g1_state.a.v;
    spi_data.qd_2s[0] = g1_state.h.v;
    spi_data.qd_3s[0] = g1_state.k.v;
    
    spi_data.q_1s[1] = g1_state.a.p;
    spi_data.q_2s[1] = g1_state.h.p;
    spi_data.q_3s[1] = g1_state.k.p;
    spi_data.qd_1s[1] = g1_state.a.v;
    spi_data.qd_2s[1] = g1_state.h.v;
    spi_data.qd_3s[1] = g1_state.k.v;
    
    spi_data.q_1s[2] = g1_state.a.p;
    spi_data.q_2s[2] = g1_state.h.p;
    spi_data.q_3s[2] = g1_state.k.p;
    spi_data.qd_1s[2] = g1_state.a.v;
    spi_data.qd_2s[2] = g1_state.h.v;
    spi_data.qd_3s[2] = g1_state.k.v;
    
    
    
    if(estop==0){
        //printf("estopped!!!!\n\r");
        memset(&q1_control, 0, sizeof(q1_control));
        memset(&q2_control, 0, sizeof(q2_control));
        memset(&q3_control, 0, sizeof(q3_control));
        spi_data.flags[0] = 0xdead;
        spi_data.flags[1] = 0xdead;
        spi_data.flags[2] = 0xdead;
        led = 1;
        }
    
    else{
        led = 0;
        
        memset(&q1_control, 0, sizeof(q1_control));
        memset(&q2_control, 0, sizeof(q2_control));
        memset(&q3_control, 0, sizeof(q3_control));
        
        
        q1_control.a.p_des = spi_command.q_des_1s[0];
        q1_control.a.v_des  = spi_command.qd_des_1s[0];
        q1_control.a.kp = spi_command.kp_1s[0];
        q1_control.a.kd = spi_command.kd_1s[0];
        q1_control.a.t_ff = spi_command.tau_1s_ff[0];
        
        q1_control.h.p_des = spi_command.q_des_2s[0];
        q1_control.h.v_des  = spi_command.qd_des_2s[0];
        q1_control.h.kp = spi_command.kp_2s[0];
        q1_control.h.kd = spi_command.kd_2s[0];
        q1_control.h.t_ff = spi_command.tau_2s_ff[0];
        
        q1_control.k.p_des = spi_command.q_des_3s[0];
        q1_control.k.v_des  = spi_command.qd_des_3s[0];
        q1_control.k.kp = spi_command.kp_3s[0];
        q1_control.k.kd = spi_command.kd_3s[0];
        q1_control.k.t_ff = spi_command.tau_3s_ff[0];
        
        q2_control.a.p_des = spi_command.q_des_1s[1];
        q2_control.a.v_des  = spi_command.qd_des_1s[1];
        q2_control.a.kp = spi_command.kp_1s[1];
        q2_control.a.kd = spi_command.kd_1s[1];
        q2_control.a.t_ff = spi_command.tau_1s_ff[1];
        
        q2_control.h.p_des = spi_command.q_des_2s[1];
        q2_control.h.v_des  = spi_command.qd_des_2s[1];
        q2_control.h.kp = spi_command.kp_2s[1];
        q2_control.h.kd = spi_command.kd_2s[1];
        q2_control.h.t_ff = spi_command.tau_2s_ff[1];
        
        q2_control.k.p_des = spi_command.q_des_3s[1];
        q2_control.k.v_des  = spi_command.qd_des_3s[1];
        q2_control.k.kp = spi_command.kp_3s[1];
        q2_control.k.kd = spi_command.kd_3s[1];
        q2_control.k.t_ff = spi_command.tau_3s_ff[1];
        
        q3_control.a.p_des = spi_command.q_des_1s[2];
        q3_control.a.v_des  = spi_command.qd_des_1s[2];
        q3_control.a.kp = spi_command.kp_1s[2];
        q3_control.a.kd = spi_command.kd_1s[2];
        q3_control.a.t_ff = spi_command.tau_1s_ff[2];
        
        q3_control.h.p_des = spi_command.q_des_2s[2];
        q3_control.h.v_des  = spi_command.qd_des_2s[2];
        q3_control.h.kp = spi_command.kp_2s[2];
        q3_control.h.kd = spi_command.kd_2s[2];
        q3_control.h.t_ff = spi_command.tau_2s_ff[2];
        
        q3_control.k.p_des = spi_command.q_des_3s[2];
        q3_control.k.v_des  = spi_command.qd_des_3s[2];
        q3_control.k.kp = spi_command.kp_3s[2];
        q3_control.k.kd = spi_command.kd_3s[2];
        q3_control.k.t_ff = spi_command.tau_3s_ff[2];
        
        
        spi_data.flags[0] = 0;
        spi_data.flags[1] = 0;
        spi_data.flags[2] = 0;
        spi_data.flags[0] |= softstop_joint(g1_state.a, &q1_control.a, A_LIM_P, A_LIM_N);
        spi_data.flags[0] |= (softstop_joint(g1_state.h, &q1_control.h, H_LIM_P, H_LIM_N))<<1;
        //spi_data.flags[0] |= (softstop_joint(g1_state.k, &q1_control.k, K_LIM_P, K_LIM_N))<<2;
        spi_data.flags[1] |= softstop_joint(g2_state.a, &q2_control.a, A_LIM_P, A_LIM_N);
        spi_data.flags[1] |= (softstop_joint(g2_state.h, &q2_control.h, H_LIM_P, H_LIM_N))<<1;
        //spi_data.flags[1] |= (softstop_joint(g2_state.k, &q2_control.k, K_LIM_P, K_LIM_N))<<2;
        spi_data.flags[2] |= softstop_joint(g3_state.a, &q3_control.a, A_LIM_P, A_LIM_N);
        spi_data.flags[2] |= (softstop_joint(g3_state.h, &q3_control.h, H_LIM_P, H_LIM_N))<<1;
        //spi_data.flags[2] |= (softstop_joint(g3_state.k, &q3_control.k, K_LIM_P, K_LIM_N))<<2;
        
        //spi_data.flags[0] = 0xbeef;
        //spi_data.flags[1] = 0xbeef;
        //PackAll();
        //WriteAll();
    }
    spi_data.checksum = xor_checksum((uint32_t*)&spi_data,14);
    for(int i = 0; i < DATA_LEN; i++){
        tx_buff[i] = ((uint16_t*)(&spi_data))[i];}
    
}
    

void test_control()
{
    for(int i = 0; i < 3; i++)
    {
        spi_data.q_1s[i] = spi_command.q_des_1s[i] + 1.f;
        spi_data.q_2s[i] = spi_command.q_des_2s[i] + 1.f;
        spi_data.q_3s[i]  = spi_command.q_des_3s[i]  + 1.f;
        
        spi_data.qd_1s[i] = spi_command.qd_des_1s[i] + 1.f;
        spi_data.qd_2s[i] = spi_command.qd_des_2s[i] + 1.f;
        spi_data.qd_3s[i]  = spi_command.qd_des_3s[i]  + 1.f;
    }
    
    spi_data.flags[0] = 0xdead;
    //spi_data.flags[1] = 0xbeef;
    
    // only do first 56 bytes of message.
    spi_data.checksum = xor_checksum((uint32_t*)&spi_data,14);
    
    for(int i = 0; i < DATA_LEN; i++)
        tx_buff[i] = ((uint16_t*)(&spi_data))[i];
}

void init_spi(void){
    SPISlave *spi = new SPISlave(PA_7, PA_6, PA_5, PA_4);
    spi->format(16, 0);
    spi->frequency(12000000);
    spi->reply(0x0);
    cs.fall(&spi_isr);
    pc.printf("done\n\r");
}
    
int main() {
    wait(1);
    //led = 1;
    pc.baud(115200);                                                                //MAYBE CHANGE THIS IF NEEDED
    pc.attach(&serial_isr);
    estop.mode(PullUp);
    //spi.format(16, 0);
    //spi.frequency(1000000);
    //spi.reply(0x0);
    //cs.fall(&spi_isr);

    //can1.frequency(1000000);                     // set bit rate to 1Mbps
    //can1.attach(&rxISR1);                 // attach 'CAN receive-complete' interrupt handler
    can1.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); //set up can filter
    //can2.frequency(1000000);                     // set bit rate to 1Mbps
    //can2.attach(&rxISR2);                 // attach 'CAN receive-complete' interrupt handler
    can2.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); //set up can filter
    can3.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); //set up can filter

    memset(&tx_buff, 0, TX_LEN * sizeof(uint16_t));
    memset(&spi_data, 0, sizeof(spi_data_t));
    memset(&spi_command,0,sizeof(spi_command_t));
    

    NVIC_SetPriority(TIM5_IRQn, 1);
    //NVIC_SetPriority(CAN1_RX0_IRQn, 3);
    //NVIC_SetPriority(CAN2_RX0_IRQn, 3);
    
    pc.printf("\n\r SPIne\n\r");
    //printf("%d\n\r", RX_ID << 18);
    
    q11_can.len = 8;                         //transmit 8 bytes
    q21_can.len = 8;                         //transmit 8 bytes
    q31_can.len = 8;
    q12_can.len = 8;                         //transmit 8 bytes
    q22_can.len = 8;                         //transmit 8 bytes
    q32_can.len = 8;
    q13_can.len = 8;                         //transmit 8 bytes
    q23_can.len = 8;                         //transmit 8 bytes
    q33_can.len = 8;
    rxMsg1.len = 6;                          //receive 6 bytes
    rxMsg2.len = 6;
    rxMsg3.len = 6;

    q11_can.id = 0x1;
    q21_can.id = 0x2;
    q31_can.id = 0x3;
    q12_can.id = 0x1;
    q22_can.id = 0x2;
    q32_can.id = 0x3;
    q13_can.id = 0x1;
    q23_can.id = 0x2;
    q33_can.id = 0x3;    

    pack_cmd(&q11_can, q1_control.a); 
    pack_cmd(&q12_can, q2_control.a); 
    pack_cmd(&q13_can, q3_control.a); 
    pack_cmd(&q21_can, q1_control.h); 
    pack_cmd(&q22_can, q2_control.h);
    pack_cmd(&q23_can, q3_control.h); 
    pack_cmd(&q31_can, q1_control.k); 
    pack_cmd(&q32_can, q2_control.k);
    pack_cmd(&q33_can, q3_control.k); 
    WriteAll();
    
    //just debugging things
    pc.printf("SETUP VARS ALL DONE\n");


    // SPI doesn't work if enabled while the CS pin is pulled low
    // Wait for CS to not be low, then enable SPI
    if(!spi_enabled){ 
        while((spi_enabled==0) && (cs.read() ==0)){pc.printf("waiting for CS Pin\n"); wait_us(10);}
        init_spi();
        spi_enabled = 1;
        pc.printf("SPI ENABLED AND READY\n");
        }
        
    //spi_command=set the thing here... 
            
    while(1) {
        //pc.printf("test, of SPINE\r\n");
        counter++;
        can2.read(rxMsg2);
        unpack_reply(rxMsg2, &g2_state);
        can1.read(rxMsg1);                    // read message into Rx message storage
        unpack_reply(rxMsg1, &g1_state);
        can3.read(rxMsg3);                    // read message into Rx message storage
        unpack_reply(rxMsg3, &g3_state);
        wait_us(10);
        
        //print heatbeat (always will print message 0)
        /*
        if (counter3 == 100000) {                                                 //for debugging purposes
            pc.printf("HEARTBEAT\n");
            counter3 = 0;
        }
        counter3++;
        */
        }

}