File content as of revision 31:5d6a97adae07:

#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)
};
 
Thread cantxa(osPriorityNormal);
Thread canrxa(osPriorityNormal);
 
// Utility
//InterruptIn button(USER_BUTTON);
DigitalOut led(LED1);
 
// Motor Control
L6208 *motor;
 
InterruptIn end1(USER_BUTTON, PullUp);
DigitalIn end0(PA_5, 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 zero()
{
    printf("zero");
    motor->run(StepperMotor::BWD);
    while(!end0){
        }
    motor->hard_stop();
    motor->set_home();
    motor->go_to(0);
    printf("END0: Pressed\n\rPOSITION: %d\n\r", motor->get_position());
}
    
    
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 end1_int_handler()
{
 // motor->hard_stop();
 
  motor->run(StepperMotor::FWD);
 
  printf("END1: Pressed\n\rPOSITION: %d\n\r", motor->get_position());
}
 
 
// CAN
CAN can1(PB_8, PB_9);     // RX, TX
 
CANMessage messageIn;
CANMessage messageOut;
 
void cantx ()
{
    while(1)
    {
    
    int _pose;
    messageOut.format = CANExtended;
    messageOut.id=gen_can_id(JOINT_CURRENT_POSITION, BASE);
    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)
  {
   // printf("receive\t\n");
    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)
      {
        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);
 
 
  end1.rise(&end1_int_handler);
 
  printf("DONE: Motor Init\n\r");
 
  // CAN Initialization
    
    
  canrxa.start(canrx_ISR);
  cantxa.start(cantx_ISR);
  
  printf("DONE: CAN Init\n\r");
 
 
 
  printf("Running!\n\r");
  
  zero();
  
  while(true)
  {
    wait(1000);
  }
}