File content as of revision 24:37f139e067b2:

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

#define MAX_MOTOR (2)

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

int current_pose = 0;
int speed = 0;

L6206_init_t init =
{
    L6206_CONF_PARAM_PARALLE_BRIDGES,
    {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,FORWARD,BACKWARD,FORWARD},
    {INACTIVE,INACTIVE,INACTIVE,INACTIVE},
    {FALSE,FALSE}
};

L6206 *motor;
InterruptIn my_button_irq(USER_BUTTON); /* User button on Nucleo board */
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);
    }
  }  
}
void end0_int_handler(unsigned int motorId)
{
  printf("END0: Pressed\n\rSPEED: %d\n\r", motor->get_speed(motorId));
}

void end1_int_handler()
{
  motor->hard_stop(0);
  motor->hard_stop(1); //or hard_hiz(); for disabling the bridge?

   motor->run(0, BDCMotor::BWD);
   motor->run(1, BDCMotor::BWD);

  printf("END1: Pressed\n\r");
}



// CAN, to revise
CAN can1(PB_12, PB_13);     // RX, TX

CANMessage messageIn;
CANMessage messageOut;


int filter = can1.filter(0x010, 0x4FF, CANStandard);

void canrx()
{
  while(1)
  {    
    if(can1.read(messageIn,filter)&& ((messageIn.id>>8 == 20) && (messageIn.id & 0x00FF==6))) //Primo motore
    {
      speed=messageIn.data[0]; //Messaggio da 0 a 255, devo sottrrarre 127 e imporre velocità con segno
      float speedMap=(speed-127)/127*100;
      printf("CAN: mess %d\n\r", speedMap, "\% ");
      
      //CAN MESSAGE WITH SPEED TO REVISE
      //Ci sarebbe anche il set speed.
      if (speedMap == 0) 
      {
        motor->set_speed(0,0); //There's no soft stop. could it work like this?
        //current_speed= motor->get_speed(0); We could do lie this?
        //motor->go_to(current_pose); 
      }
      else if (speedMap>0)
        {
            motor->run(0,BDCMotor::FWD);
            motor->set_speed(0,(unsigned int) speedMap);
        }
      else if (speedMap<0)
      {
        motor->run(0,BDCMotor::BWD);
        motor->set_speed(0,(unsigned int) -speedMap);
      }
      else 
      {
        motor->set_speed(0,0); //Riportare errore? NO
      }
     }
     else if(can1.read(messageIn,filter)&& ((messageIn.id>>8 == 20) && (messageIn.id & 0x00FF==7))) //Secondo motore
    {
      speed=messageIn.data[0]; //Messaggio da 0 a 255, devo sottrrarre 127 e imporre velocità con segno
      float speedMap=(speed-127)/127*100;
      printf("CAN: mess %d\n\r", speedMap, "\% ");
      
      //CAN MESSAGE WITH SPEED TO REVISE
      //Ci sarebbe anche il set speed.
      if (speedMap == 0) 
      {
        motor->set_speed(1,0); //There's no soft stop. could it work like this?
        //current_speed= motor->get_speed(0); We could do lie this?
        //motor->go_to(current_pose); 
      }
      else if (speedMap>0)
        {
            motor->run(1,BDCMotor::FWD);
            motor->set_speed(1,(unsigned int) speedMap);
        }
      else if (speedMap<0)
      {
        motor->run(1,BDCMotor::BWD);
        motor->set_speed(1,(unsigned int) -speedMap);
      }
      else 
      {
        motor->set_speed(0,0); //Riportare errore? NO
      }
     }
}
//Aggiungere un get speed,FATTO  ci sarebbe anche il set_speed 
        //Dentro la libreria controllare se c'è controllo di Duty cycle FATTO
        //motor->go_to(current_pose);

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

 
  
  button.rise(&button_int_handler);
  
  printf("DONE: Motor Init\n\r");
  
  // CAN Initialization
  
  canrxa.start(canrx);
  
  printf("DONE: CAN Init\n\r");
  

  
  printf("Running!\n\r");
  
  while(true)
  {
    wait(1000);
  }
}