File content as of revision 1:6cca05643958:

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

#define Rx_Buff_Dim 12

#define VREFA_PWM_PIN D3
#define VREFB_PWM_PIN D9
#define BAUDRATE 9600
#define JOINT 2

l6208_init_t init =
{
  8100,            //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)
  8100,            //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)
};

// Motor Control
L6208 *motor;
DigitalIn end0(PA_5);   // endstop
Serial s_rx(PC_10, PC_11); // comunication usart
Serial Pc_Stat(PA_2, PA_3);// status usart
Timer timeOuter;

char dataRxBuffer[Rx_Buff_Dim];  
volatile int rxBufferPtr;

float pose, current_pose;
int speed, current_speed;

void motor_error_handler(uint16_t error);   
void zero();
void fmotor();
void runCommand();
void serialrx();

bool posMode = false;

int main(){
    s_rx.baud(BAUDRATE);
    Pc_Stat.baud(115200);
    
    // Motor Initialization
    motor = new L6208(D2, D8, D7, D4, D5, D6, VREFA_PWM_PIN, VREFB_PWM_PIN);
    motor->set_step_mode(StepperMotor::STEP_MODE_1_16);
    if (motor->init(&init) != COMPONENT_OK)
    {
      Pc_Stat.printf("ERROR: vvMotor Init\n\r");
      exit(EXIT_FAILURE);
    }
    
    motor->attach_error_handler(&motor_error_handler);
        
    printf("DONE: Motor Init\n\r");
    
    Pc_Stat.printf("Running!\n\r");
        
    motor->set_home();

    s_rx.printf("19 1\r\n");

    timeOuter.start();
    
    while(true)
    {
      serialrx();
      if (timeOuter.read_ms() > 2000) {
          current_speed = 0;
          timeOuter.reset();
          motor->hard_stop();
          Pc_Stat.printf("Command timeout!!\n");
      }
      //wait (0.001);
      //if (!posMode) fmotor();
      
    }
}
  
void motor_error_handler(uint16_t error){
  Pc_Stat.printf("ERROR: Motor Runtime\n\r");
}  
  
void zero(){
  Pc_Stat.printf("Zeroing (%d)... ", (int) end0);
  //motor->run(StepperMotor::BWD);
  
  int osc_pos = 10000;
    
  if (!end0)  {
      motor->hard_stop();
      motor->set_home();

      motor->go_to(-8000);
      while(motor->get_position() != -8000);
  }

  motor->set_home();
  
  for (int i = 0; i < 4; i++) {
      motor->go_to(osc_pos);
      while(end0 && motor->get_position() != osc_pos);
      if (!end0) {
          motor->hard_stop();
          motor->set_home();
          osc_pos = 5000 * (osc_pos > 0 ? 1 : -1);
          motor->go_to(osc_pos);
          while(motor->get_position() != osc_pos);
          motor->set_home();
          Pc_Stat.printf("OK!\n");
          return;
      } else { 
          osc_pos *= -2;
      }
  }

  Pc_Stat.printf("ZERO FAILURE!\n");
}  

void fmotor(int speed)
{ 
    unsigned int sp = abs(speed*80);
    
    if (sp) {
        motor->set_max_speed(sp);
        
        if(speed > 0){
            motor->run(StepperMotor::FWD );
        } 
        else if (speed < 0){
            motor->run(StepperMotor::BWD );
        }
        
        //motor->run(current_speed >= 0 ? 
          //  StepperMotor::FWD : StepperMotor::BWD);
    } else {
        motor->hard_stop();
        current_pose = motor->get_position();
        motor->go_to(current_pose);
    }
    wait(0.001);
}

void runCommand() {
    int joint, spd;
    
    current_speed = 0;
    timeOuter.reset();
          
    sscanf(dataRxBuffer, "%d %d", &joint, &spd);
    Pc_Stat.printf("joint: %d, value: %d\n", joint, spd);
    posMode = false;
    
    switch(joint){
        case 10:
            zero();
            break;
        case 11:
            posMode = true;
            Pc_Stat.printf("Moving(position)... \n");
            motor->set_max_speed(8000);
            motor->go_to(spd);
            Pc_Stat.printf("Done!\n");
            break;
        case 12:
            Pc_Stat.printf("Moving(velocity)... \n");
            fmotor(spd);
            s_rx.printf("15 %d\n",motor->get_position());
            break;
        case 13:
            posMode = true;
            motor->hard_stop();
            Pc_Stat.printf("STOP.\n");
            break;           
        default:
            Pc_Stat.printf("Unknown index %d - data: %d\n", joint, spd);
            break;
    }
}

void serialrx()
{  
    while(s_rx.readable()) 
    {
      char c = s_rx.getc();
      //Pc_Stat.printf("%c",c);
      
      if (c == '\n' || rxBufferPtr >= Rx_Buff_Dim - 1) {
          dataRxBuffer[rxBufferPtr] = '\0';
          runCommand();
          rxBufferPtr = 0;
          
      } else {
          dataRxBuffer[rxBufferPtr++] = c;
      }
    }
}