File content as of revision 2:36a254d3dbf3:

#define CAN_ID 0x0

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



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

Ticker loop;                                // Control loop interrupt handler
int loop_counter = 0;
#define DT                  .01             // Control loop period

#define N_MOTORS             2              // 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]);
/*                                       */

void control()
{
    /* Your control loop goes here.  */
    /* Update torques, position/velocity setpoints, etc */
    motors[0].control.v_des = 5.0f*sin(.01f*loop_counter);
    motors[0].control.kd = .5f;
    /*                              */
    
    for(int i = 0; i<N_MOTORS; i++)
    {
        pack_cmd(&motors[i]);
        can.write(motors[i].txMsg);
    }
    
    
    printf("%f\n\r", motors[0].state.velocity);           // This will print to the computer.  Usefull for debugging
    loop_counter++;     // Increment loop counter
}


int main() 
{
    
    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, 2};                 // List of motor CAN ID's
    init_motors(ids);                           // Initialize the list of motors
    
    enable_motor(&motors[0], &can);             // Enable first motor
    wait(1);                                    // Wait 1 second
    disable_motor(&motors[0], &can);            // Disable first motor
    
    loop.attach(&control, DT);                 // Start running the contorl interrupt at 1/DT Hz
        
    while(1) 
    {
        // Usuallly nothing should run here.  Instead run control in the interrupt.
    }
        
}
    

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