File content as of revision 27:bf0109fb61c3:

#include "mbed.h"
#include "L6206.h"

#define JOINT_SET_SPEED 20

#define JOINT_ID 3


static volatile uint16_t gLastError;
static volatile uint8_t gStep = 0;

int current_pose = 0;
int speed = 0;

L6206_init_t init =
{
    PARALLELING_NONE___1_BIDIR_MOTOR_BRIDGE_A__1_BIDIR_MOTOR_BRIDGE_B,
    {L6206_CONF_PARAM_FREQ_PWM1A, L6206_CONF_PARAM_FREQ_PWM2A, L6206_CONF_PARAM_FREQ_PWM1B, L6206_CONF_PARAM_FREQ_PWM2B},
    {100,100,100,100},
    {FORWARD,BACKWARD,FORWARD,BACKWARD},
    {INACTIVE,INACTIVE,INACTIVE,INACTIVE},
    {FALSE,FALSE}
};

L6206 *motor;

Thread canrxa;

//Utility
InterruptIn button(USER_BUTTON);
DigitalOut led(LED1); //Change?


void motor_error_handler(uint16_t error)
{
  printf("ERROR: Motor Runtime\n\r");
  while(1){}; 
}

void motor_zero()
{
  motor->run(0, BDCMotor::FWD);
  motor->run(1, BDCMotor::FWD);
}

void button_int_handler(unsigned int motorId)
{
    printf("MOTOR SPEED: %d\n\r", motor->get_speed(motorId)); 
    motor_zero();
}

void my_error_handler(uint16_t error)
{
  /* Backup error number */
  gLastError = error;
  
  /* Enter your own code here */
}

void my_flag_irq_handler(void)
{
  /* Code to be customised */
  /************************/
  /* Get the state of bridge A */
  uint16_t bridgeState  = motor->get_bridge_status(0);
  
  if (bridgeState == 0) {
    if ((motor->get_device_state(0) != INACTIVE)||
        (motor->get_device_state(1) != INACTIVE)) {
      /* Bridge A was disabling due to overcurrent or over temperature */
      /* When at least on of its  motor was running */
        my_error_handler(0XBAD0);
    }
  }
  
  /* Get the state of bridge B */
  bridgeState  = motor->get_bridge_status(1);
  
  if (bridgeState == 0)  {
    if ((motor->get_device_state(2) != INACTIVE)||
        (motor->get_device_state(3) != INACTIVE)) {
      /* Bridge A was disabling due to overcurrent or over temperature */
      /* When at least on of its  motor was running */
        my_error_handler(0XBAD1);
    }
  }  
}

// CAN, to revise
CAN can1(PB_8, PB_9);     // RX, TX

CANMessage messageIn;
CANMessage messageOut;

void canrx()
{
  while(1)
  {    
    if(can1.read(messageIn))
    {
      printf("READ!\n\r");
      
      if(messageIn.id == ((JOINT_SET_SPEED << 8) + JOINT_ID))
      {
          speed = 0;
          speed = (messageIn.data[0] << 24) | (messageIn.data[1] << 16) | (messageIn.data[2] << 8) | (messageIn.data[3]);
          
          motor->set_speed(0, speed);
          (speed > 0) ? motor->run(0, BDCMotor::BWD) : motor->run(0, BDCMotor::FWD);
          
          printf("CAN: mess %d\n\r", speed);
      }
    }
    
    int speed = 5000;
    
    messageOut.data[0] = speed >> 24;
   messageOut.data[1] = speed >> 16;
   messageOut.data[2] = speed >> 8;
   messageOut.data[3] = speed;
   
   messageOut.id = (3 << 8) + 2;
    int status = can1.write(messageOut);
    
    printf("STATUS: %d\n\r", status);
    
    wait(0.01);
  }
}


/* Main ----------------------------------------------------------------------*/
int main()
{
  can1.frequency(125000);
  messageIn.format=CANExtended;
  messageOut.format=CANExtended;
  
  
  // Motor Initialization 
  
#ifdef TARGET_STM32F429
    motor = new L6206(D2, A4, PB_4, PC_7, PA_15, PB_3);
#else
    motor = new L6206(D2, A4, D5, D4, A0, A1);
#endif
 
  if (motor->init(&init) != COMPONENT_OK) 
  {
    printf("ERROR: vvMotor Init\n\r");
    exit(EXIT_FAILURE);
  }

  motor->attach_flag_interrupt(my_flag_irq_handler);
  motor->attach_error_handler(my_error_handler);

  
  printf("DONE: Motor Init\n\r");
  
  // CAN Initialization
  
  canrxa.start(canrx);
  
  printf("DONE: CAN Init\n\r");
  
  //motor->set_speed(1, 50);
  //motor->run(1, BDCMotor::FWD);
  
  printf("Running!\n\r");
  
  while(true)
  {
    wait(1);
  }
}