File content as of revision 6:95dc3761f64a:

#define CAN_ID 0x0
#include "mbed.h"
#include "math_ops.h"
#include "MotorModule.h"

typedef union _data {
  float f;
  char  s[4];
} myData;


Serial       pc(PA_2, PA_3);                // Serial port to the computer
CAN          can(PB_8, PB_9, 1000000);      // CAN Rx pin name, CAN Tx pin name

myData  dat1;
myData  dat2;
myData  dat3;
myData  dat4;

Ticker loop_ctrl;                                // Control loop interrupt handler
Ticker loop_msg;

#define DT_ctrl             .001f             // Control loop period
#define DT_msg              .01f             // msg loop period    
#define N_MOTORS            1               // Number of motors on the can bus

MotorStruct motors[N_MOTORS];               // Create a list of the motors attached

/* Communication functions.  Do not touch */
void onMsgReceived();
void init_motors(int ids[N_MOTORS]);

int loop_counter = 0;
int newprint = 1;
int nskip = 5;  
int skip_counter = 0;

float t = 0.0;
float A=  0.5;
float f = 1.0;
double mycur = 0.2;
bool flagCtrl = true;

void printnew(){
    newprint = 1;
}

void control(){ 
    t = DT_ctrl*loop_counter;  //time in seconds
    if (flagCtrl) {
        // if control is ON
        if (t<1) {
            motors[0].control.p_des = 0;
            motors[0].control.v_des = 0;
            motors[0].control.kp = 0;
            motors[0].control.kd = 0;
        }
        if (1<=t<20){
            motors[0].control.i_ff = mycur;
            motors[0].control.kd = 0;
            motors[0].control.kp = 0;
        }
        for(int i = 0; i<N_MOTORS; i++) { //send msg to driver
            pack_cmd(&motors[i]);
            can.write(motors[i].txMsg);
        }
    } else {
        // if control is OFF
        motors[0].control.i_ff = 0.0f;  //all set to zero (no control)
        motors[0].control.kp = 0.0f;
        motors[0].control.kd = 0.0f;
        
        loop_counter = 0.0f;            //re-zero loop timing 
        
        for(int i = 0; i<N_MOTORS; i++) { //send msg to driver
            pack_cmd(&motors[i]);
            can.write(motors[i].txMsg);
        }
    }
    loop_counter++;     // increase loop counter
}

int main() 
{
    /* Setup & Initialization */
    pc.baud(921600);                            // Set baud rate for communication over USB serial
    can.attach(&onMsgReceived);                 // attach 'CAN receive-complete' interrupt handler
    can.filter(CAN_ID , 0xFFF, CANStandard, 0); // Set up can filter so it interrups only for messages with ID CAN_ID

    
    int ids[N_MOTORS] = {1};                 // List of motor CAN ID's
    init_motors(ids);                           // Initialize the list of motors
    
    enable_motor(&motors[0], &can);             // Enable motors
    //enable_motor(&motors[1], &can);
    
    zero_motor(&motors[0], &can);               //Zero motors
    //zero_motor(&motors[1], &can);
    wait(1);                                    // Wait 1 second
    
    //disable_motor(&motors[0], &can);            // Disable first motor
    //disable_motor(&motors[1], &can); 
    
    /* Interrupt Enables */
    loop_ctrl.attach(&control, DT_ctrl);                 // Start running the contorl interrupt at 1/DT Hz
    loop_msg.attach(&printnew, DT_msg);    
    
    while(1) {
        if (pc.readable()) {
            char c = pc.getc();
            if(c == 's') {
                flagCtrl = false;
                printf("s");
            }
            if(c == 'g') {
                flagCtrl = true;
                printf("g");
            }
            if(c == 'e') {
                mycur = mycur +0.1;
            }
            if(c == 'r') {
                mycur = mycur - 0.1;
            }

        }
        if(newprint == 1){
            printf("%f \t%f \t%f \r\n", t, motors[0].state.position, motors[0].state.current);
            //pc.putc(motors[0].state.position);
            //pc.write(&motors[0].state.position);
            //dat1.f = motors[0].state.position;
            //pc.printf("%c%c%c%c", dat1.s[0], dat1.s[1], dat1.s[2], dat1.s[3]);
            newprint = 0;
        }
    }
}
    
/* low-level communication functoins below.  Do not touch */

 void onMsgReceived() 
/* This interrupt gets called when a CAN message with ID CAN_ID shows up */
{
    CANMessage   rxMsg;
    rxMsg.len = 6;
    can.read(rxMsg);                    // read message into Rx message storage
    int id = rxMsg.data[0];
    for (int i = 0; i< N_MOTORS; i++)
    {
        if(motors[i].control.id == id)
        {
            memcpy(&motors[i].rxMsg, &rxMsg, sizeof(motors[i].rxMsg));
            unpack_reply(&motors[i]);
        }
    }
}

void init_motors(int ids[N_MOTORS])
/* Initialize buffer lengths and IDs of the motors in the list */
{
    for(int i = 0; i<N_MOTORS; i++)
    {
        motors[i].txMsg.len = 8;
        motors[i].rxMsg.len = 6;
        motors[i].control.id = ids[i];
        motors[i].txMsg.id = ids[i];
        motors[i].control.p_des = 0;
        motors[i].control.v_des = 0;
        motors[i].control.kp = 0;
        motors[i].control.kd = 0;
        motors[i].control.i_ff = 0;
    }
}