File content as of revision 32:03c98e297a4a:

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

#define VREFA_PWM_PIN D3
#define VREFB_PWM_PIN D9
#define SEND_FREQUENCY 10 //Hz
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)
};
// CAN

CAN can1(PA_11, PA_12);     // RX, TX

CANMessage messageIn;
CANMessage messageOut;

Thread cantxa(osPriorityNormal);
Thread canrxa(osPriorityNormal);

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

float pose, current_pose;
float speed, current_speed;

void set_zero()
{
  printf("set zero\t\n");
  motor->hard_stop();
  motor->set_home();
  motor->go_to(0);
}
void zero()
{
    printf("zero");
    motor->run(StepperMotor::FWD);
    
}
    
    
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;
}
double to_rad(double angle)
{
  return angle*0.0174533;
}
double angle_deparse (long int pose, float offset)
{
  offset = offset * 0.00872664625;
  double angle = pose *0.000487012987; //do something 0,0004791666667
  angle = (angle - offset);
  return angle;
}
void motor_error_handler(uint16_t error)
{
  printf("ERROR: Motor Runtime\n\r");

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

  motor->run(StepperMotor::BWD);

  printf("END0: Pressed\n\rPOSITION: %d\n\r", motor->get_position());
}
void end0_int_handler_fall()
{
  motor->hard_stop();
  int position = motor->get_position();
  motor->go_to(position);

  
}

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

  motor->run(StepperMotor::FWD);

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



void cantx ()
{
    while(1)
    {
    
    int _pose;
    messageOut.format = CANExtended;
    messageOut.id=gen_can_id(JOINT_CURRENT_POSITION, SHOULDER);
    pose=angle_deparse(motor->get_position(), 0);
    _pose=pose*100;
    messageOut.data[3]=_pose;
    messageOut.data[2]=_pose >>8;
    messageOut.data[1]=_pose >>16;
    messageOut.data[0]=_pose >>24;

    int status = can1.write(messageOut);
    led=status;
    printf("CAN send CURRENT POSITION Joint status %d : pose %f\t\n",status, pose);
    }
    



}
void cantx_ISR()
{
    cantx();
    osDelay(1/SEND_FREQUENCY*1000);
}


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


      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,SHOULDER))
    {
      if((messageIn.data[3] + (messageIn.data[2] << 8) + (messageIn.data[1] << 16) + (messageIn.data[0] << 24))==1)
      {
        zero();
        motor->wait_while_active();
      }
    }
    
  }
}
void canrx_ISR()
{
    canrx();
    osDelay(10);
}

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

int main()
{
  
  can1.frequency(125000);
   
  // 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)
  {
    printf("ERROR: vvMotor Init\n\r");
    exit(EXIT_FAILURE);
  }

  motor->attach_error_handler(&motor_error_handler);


  end0.rise(&end0_int_handler);
  end0.fall(&end0_int_handler_fall);
  end1.rise(&end1_int_handler);
  end1.fall(&set_zero);

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

  // CAN Initialization
   // zero();
    
    
  canrxa.start(canrx_ISR);
  //cantxa.start(cantx_ISR);
  
  printf("DONE: CAN Init\n\r");
  


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