File content as of revision 27:275ba9c137c9:

#include "mbed.h"
#include "L6208.h"

#define VREFA_PWM_PIN D3
#define VREFB_PWM_PIN D9

#define JOINT_SET_SPEED 20

#define JOINT_ID 2

l6208_init_t init =
{
  8000,            //Acceleration rate in step/s^2 or (1/16)th step/s^2 for microstep modes
  80,              //Acceleration current torque in % (from 0 to 100)
  8000,            //Deceleration rate in step/s^2 or (1/16)th step/s^2 for microstep modes
  80,              //Deceleration current torque in % (from 0 to 100)
  8000,            //Running speed in step/s or (1/16)th step/s for microstep modes
  80,              //Running current torque in % (from 0 to 100)
  40,              //Holding current torque in % (from 0 to 100)
  STEP_MODE_1_16,  //Step mode via enum motorStepMode_t
  FAST_DECAY,      //Decay mode via enum motorDecayMode_t
  0,               //Dwelling time in ms
  FALSE,           //Automatic HIZ STOP
  100000           //VREFA and VREFB PWM frequency (Hz)
};

Thread canrxa;

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

// Motor Control
L6208 *motor;

InterruptIn end0(PC_4, PullUp);
InterruptIn end1(PC_8, PullUp);
InterruptIn enc(PC_12, PullUp);

int32_t speed = 0;

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

void motor_zero()
{
    motor->run(StepperMotor::FWD);
}

void button_int_handler()
{
    printf("POSITION: %d\n\r", motor->get_position());
    motor_zero();
}

void end0_int_handler()
{
    motor->run(StepperMotor::BWD);
    printf("END0: Pressed\n\rPOSITION: %d\n\r", motor->get_position());
}

void end1_int_handler()
{
    motor->hard_stop();
    motor->run(StepperMotor::FWD);

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

void motor_set_home()
{
  motor->hard_stop();
  motor->set_home();
  motor->go_to(0);
}

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

CANMessage messageIn;
CANMessage messageOut;

void canrx()
{
  while(1)
  {    
    if(can1.read(messageIn))
    {
      printf("received\r\n");
      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_max_speed(speed);
          (speed > 0) ? motor->run(StepperMotor::BWD) : motor->run(StepperMotor::FWD);
          
          printf("CAN: mess %d\n\r", speed);
      }
    }
    
    wait(0.01);
  }
}


/* Main ----------------------------------------------------------------------*/

int main()
{
  can1.frequency(125000);
  messageIn.format=CANExtended;
  
  // Motor Initialization 
  motor = new L6208(D2, D8, D7, D4, D5, D6, VREFA_PWM_PIN, VREFB_PWM_PIN);
  
  if (motor->init(&init) != COMPONENT_OK)
  {
    printf("ERROR: vvMotor Init\n\r");
    exit(EXIT_FAILURE);
  }

  motor->attach_error_handler(&motor_error_handler);
  
  end0.rise(&end0_int_handler);
  end1.rise(&end1_int_handler);
  end1.fall(&motor_set_home);
  
  button.rise(&button_int_handler);
  
  motor->set_step_mode(StepperMotor::STEP_MODE_1_16);
  printf("DONE: Motor Init\n\r");
  
  // CAN Initialization
  
  canrxa.start(canrx);
  
  printf("DONE: CAN Init\n\r");
  
  printf("Running!\n\r");
  
  // DEBUG
  //motor->set_max_speed(8000);
  //motor->run(StepperMotor::FWD);
  
  while(true)
  {
    wait(1);
  }
}