File content as of revision 29:f888a2394027:

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

#define VREFA_PWM_PIN D3
#define VREFB_PWM_PIN D9

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;
Thread cantxa;

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

// Motor Control
L6208 *motor;

InterruptIn end0(PA_5, PullUp);
InterruptIn end1(PA_6, PullUp);
InterruptIn enc(PC_12, PullUp);

typedef enum
{
  JOINT_SET_SPEED = 20,
  JOINT_SET_POSITION,
  JOINT_CURRENT_POSITION,
  JOINT_CURRENT_SPEED,
  JOINT_STATUS,
  JOINT_ERROR,
  JOINT_TORQUE,
  JOINT_MAXTORQUE,
  JOINT_ZERO,
}CAN_COMMANDS;

typedef enum
{
  BASE=1,
  SHOULDER,
  ELBOW,
  WRIST1,
  WRIST2,
  WRIST3,
  END_EFFECTOR,
  CAMERA1,
  CAMERA2,
}JOINT;

long int pose, current_pose;
int speed, current_speed;

uint32_t gen_can_id(CAN_COMMANDS message_id, JOINT can_id)
{
  uint32_t id = (uint32_t)can_id;     // LSB byte is the controller id.
  id |= (uint32_t)message_id << 8;  // Next lowest byte is the packet id.
  id |= 0x80000000;              // Send in Extended Frame Format.
  return id;
}

int angle_deparse (long int pose, float offset)
{
  offset = offset * 0.00872664625;
  float angle = pose * 0.00000425 ; //do something
  angle = (angle - offset) * 100;
  return (int)angle;
}
void motor_error_handler(uint16_t error)
{
  printf("ERROR: Motor Runtime\n\r");

}

void end0_int_handler()
{
  motor->hard_stop();

  motor->run(StepperMotor::FWD);

  printf("END0: Pressed\n\rPOSITION: %d\n\r", motor->get_position());


}

void end1_int_handler()
{
  motor->hard_stop();

  motor->run(StepperMotor::BWD);

  printf("END1: Pressed\n\rPOSITION: %d\n\r", motor->get_position());
}

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 cantx ()
{
    messageOut.format = CANExtended;
    messageOut.id=gen_can_id(JOINT_CURRENT_POSITION, BASE);
    pose=angle_deparse(motor->get_position(), 0);

    messageOut.data[3]=pose;
    messageOut.data[2]=pose >>8;
    messageOut.data[1]=pose >>16;
    messageOut.data[0]=pose >>24;

    int status = can1.write(messageOut);
    printf("CAN send CURRENT POSITION Joint status %d : pose %d",status, pose);

    wait(1);



}



void canrx()
{
  while(1)
  {
    if(can1.read(messageIn)&&messageIn.id==gen_can_id(JOINT_SET_SPEED,BASE))
    {
      speed=messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24);
      printf("CAN: mess %d\n\r", speed);
      current_speed=speed-100;


      if (current_speed>0)
      {
        motor->set_max_speed(current_speed*80);
        motor->run(StepperMotor::FWD);
      }
      else if (current_speed<0)
      {
        motor->set_max_speed(current_speed*80);
        motor->run(StepperMotor::BWD);
      }

      else
      {
        motor->soft_stop();
        current_pose= motor->get_position();
        motor->go_to(current_pose);
      }
    }
    
    if(can1.read(messageIn)&&messageIn.id==gen_can_id(JOINT_ZERO,BASE))
    {
      if((messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24))==1)
      {
        motor->hard_stop();
        motor->set_max_speed(5000);
        motor->run(StepperMotor::BWD);
      }
    }
    
  }
}


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

int main()
{
  can1.frequency(125000);

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



  motor->set_step_mode(StepperMotor::STEP_MODE_1_16);
  printf("DONE: Motor Init\n\r");

  // CAN Initialization

  canrxa.start(canrx);
  cantxa.start(cantx);

  printf("DONE: CAN Init\n\r");



  printf("Running!\n\r");

  while(true)
  {
    wait(1000);
  }
}