File content as of revision 34:4be6353f0186:

#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);
        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))
        {
            led = !led;
            
            printf("Read!\n\r");
            
            if(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  : %f\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);
                }
            }
            else if(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);
    }
}