File content as of revision 28:32001ee473e0:

////////////////////////////////////////
//      Rotating Platform             //
//  Arkadiraf@gmail.com - 24/05/2017  //
////////////////////////////////////////

/*
   Parts:
    Nucleo STM32F401RE
    X-NUCLEO-IHM01A1 - 3 Stepper motor controller
*/

/*
 Improvements:
 Move all constant parameters to #define,
 Implement control loop (other than proportional)
 Implement Velocity command saturation with position control (implements max speed limits)
 Implement better stop condition for position control
*/
/*
    Pinout:
    Nucleo STM32F401RE
    PA_5 --> led (DigitalOut)

    PC - Serial 2
    PA_2 (Tx) --> STLINK
    PA_3 (Rx) --> STLINK

    X-NUCLEO-IHM01A1 (http://www.st.com/content/ccc/resource/technical/document/data_brief/59/ff/d0/16/94/ff/49/85/DM00122463.pdf/files/DM00122463.pdf/jcr:content/translations/en.DM00122463.pdf)
    SPI:
    PA_7 (D11) --> mosi
    PA_9 (D12) --> miso
    PA_8 (D13) --> sclk

    Motor 1
    PA_10(D2) --> flag_irq  (DigitalOut)
    PA_9 (D8) --> Standby   (DigitalOut)
    PA_8 (D7) --> MOT1Dir  (DigitalOut)
    PC_7 (D9) --> MOT1Step (PWM)
    PB_6 (D10)--> ssel      (DigitalOut)

    Motor 2
    PA_10(D2) --> flag_irq  (DigitalOut)
    PA_9 (D8) --> Standby   (DigitalOut)
    PB_5 (D4) --> MOT2Dir   (DigitalOut)
    PB_3 (D3) --> MOT2Step  (PWM)
    PB_6 (D10)--> ssel      (DigitalOut)

    Motor 3
    PA_10(D2) --> flag_irq  (DigitalOut)
    PA_9 (D8) --> Standby   (DigitalOut)
    PB_4 (D5) --> MOT3Dir   (DigitalOut)
    PB_10(D6) --> MOT3Step  (PWM)
    PB_6 (D10)--> ssel      (DigitalOut)


*/

///////////////
// Libraries //
///////////////
#include "mbed.h"
#include "BufferedSerial.h"  // solves issues of loosing data. alternative doing it yourself
#include "l6474_class.h" // stepper library


///////////////
// #defines  //
///////////////
#define DEBUG_MSG
#define Motor_Control_Interval 10000 // 100Hz
#define TimeoutCommand 2000 // 2 second (ms units)
#define STEPS2ROTATION 3200.0f // Number of steps for rotation at 16 microstepping

// control loop

#define PROPORTIONAL_GAIN 0.0027777f // 1*1/360; (// gain for rotations/sec

/////////////
// Objects //
/////////////

// X-NUCLEO-IHM01A1
DigitalOut MOT1Dir(D7);
DigitalOut MOT1Step(D9);
DigitalOut MOT2Dir(D4);
DigitalOut MOT2Step(D3);
DigitalOut MOT3Dir(D5);
DigitalOut MOT3Step(D6);

/* Motor Control Component. */
L6474 *motor1;
L6474 *motor2;
L6474 *motor3;

// Led
DigitalOut led(LED1);

// serial
BufferedSerial pc(USBTX, USBRX);

// Define Ticker for Motor Motion Control Ticker
Ticker Platform_Control_Ticker;

// Define Ticker for Steps control
Ticker Motor1_Step_Ticker;
Ticker Motor2_Step_Ticker;
Ticker Motor3_Step_Ticker;

// Timer for clock counter
Timer time_timer;


///////////////
// variables //
///////////////
// Send pos update
bool Pos_Update_Flag=0;

// Driver Flag status, enabled / disabled
bool EN_Stepper_Flag=0;

// flag to indicate control mode; 1 - SPD , 0 - Position
volatile bool SpdPos_Flag=0;

// Motor Speed control
volatile float CMD_Motor_SPD[3]= {0}; // rotations / sec
volatile float Motor_SPD[3]= {0}; // rotations / sec

// Motor Position control
volatile float CMD_Motor_Pos[3]= {0}; // command motor angle in degrees
volatile float Motor_Pos[3]= {0}; // [deg] = Motor_Steps / STEPS2ROTATION *360.0f

// variable to store motor position
volatile int Motor_Steps[3] = {0}; // motor steps performed

// timeout command time stamp
volatile int CMDTimeStamp=0;

///////////////
// Functions //
///////////////

// Incoming Message parser
void Parse_Message(char inbyte);

// Init Platform
void Platform_Init();

// Platform Motion Set
void Platform_Motion_Set(float Set_M1, float Set_M2, float Set_M3);

// Platform Motion Control
void Platform_Motion_Control();

// Motor1 Step Control
void Motor1_Step_Control();

// Motor2 Step Control
void Motor2_Step_Control();

// Motor3 Step Control
void Motor3_Step_Control();

////////////////////////
//  Main Code Setup : //
////////////////////////
int main()
{
    //Initializing SPI bus.
    DevSPI dev_spi(D11, D12, D13);

    //Initializing Motor Control Components.
    motor1 = new L6474(D2, D8, D10, dev_spi);
    motor2 = new L6474(D2, D8, D10, dev_spi);
    motor3 = new L6474(D2, D8, D10, dev_spi);

    // Setup serial
    pc.baud(256000);

    // Init shika shuka
    Platform_Init();

    // initil time timer:
    time_timer.start();

    ///////////////////////
    //  Main Code Loop : //
    ///////////////////////
    while(1) {
        // loop time stamp
        int Timer_TimeStamp_ms=time_timer.read_ms();

        // receive Motor Command
        while (pc.readable()) {
            char InChar=pc.getc();
            pc.printf("%c" ,InChar); // debug check/
            Parse_Message(InChar);
        }//end serial

        // set time out on commad and stop motion
        if (abs(Timer_TimeStamp_ms-CMDTimeStamp)>TimeoutCommand) {
#ifdef DEBUG_MSG
            // pc.printf("Timer_TimeStamp_ms %d CMDTimeStamp %d\r\n", Timer_TimeStamp_ms,CMDTimeStamp);
#endif /* DEBUG_MSG */
            CMDTimeStamp=Timer_TimeStamp_ms;
            motor1->Disable();
            motor2->Disable();
            motor3->Disable();
            CMD_Motor_SPD[0]=0;
            CMD_Motor_SPD[1]=0;
            CMD_Motor_SPD[2]=0;
            Motor_SPD[0]=0;
            Motor_SPD[1]=0;
            Motor_SPD[2]=0;
            EN_Stepper_Flag=0;

            // reset motor position
            Motor_Steps[0]=0;
            Motor_Steps[1]=0;
            Motor_Steps[2]=0;
            CMD_Motor_Pos[0]=0;
            CMD_Motor_Pos[1]=0;
            CMD_Motor_Pos[2]=0;
        } // end timeout
        // send position message
        if (Pos_Update_Flag) {
            Pos_Update_Flag=0;
            int Temp_TimeStamp_ms=time_timer.read_ms();
            static uint32_t MSG_Counter=0;
            MSG_Counter++;
            pc.printf("POS:%d,%d,%d,%d,%d\r\n" ,Temp_TimeStamp_ms,MSG_Counter,Motor_Steps[0],Motor_Steps[1],Motor_Steps[2]); // send motors position
        }// end position update
    }// End Main Loop
}// End Main


///////////////
// Functions //
///////////////
// Incoming Message parser  Format: "$<SPD_M1>,<SPD_M2>,<SPD_YAW>,<PITCH_ANGLE>\r\n" // up to /r/n
void Parse_Message(char inbyte)
{
    static const uint16_t BufferCMDSize=32;
    static const uint16_t BufferCMD_ValuesSize=4;

    static float CMD_Values[BufferCMD_ValuesSize]= {0};
    static char BufferCMD[BufferCMDSize]= {0};
    static uint16_t BufferIndex=0;
    static uint16_t Values_Index=0;

    BufferIndex=BufferIndex%(BufferCMDSize); // simple overflow handler
    Values_Index=Values_Index%(BufferCMD_ValuesSize); // simple overflow handler

    BufferCMD[BufferIndex]=inbyte;
    BufferIndex++;

    // parse incoming message
    if (inbyte=='$') { // start of message
        BufferIndex=0; // initialize to start of parser
        Values_Index=0; // index for values position
    } else if (inbyte==',') { // seperator char
        CMD_Values[Values_Index]=atof(BufferCMD); // input value to buffer values
        BufferIndex=0; // initialize to start of parser
        Values_Index++;
    } else if(inbyte=='\r') { // end of message // parse message
        // Update last value
        CMD_Values[Values_Index]=atof(BufferCMD); // input value to buffer values

        BufferIndex=0; // initialize to start of parser
        Values_Index=0; // reset values index

        // set time stamp on time out commad
        CMDTimeStamp=time_timer.read_ms();

        //0 - Speed Control 1 - Position Control
        SpdPos_Flag=(bool)CMD_Values[0];

        // update command
        Platform_Motion_Set(CMD_Values[1],CMD_Values[2],CMD_Values[3]);

#ifdef DEBUG_MSG
        pc.printf("CMD: %d ,%.3f ,%.3f ,%.3f \r\n" ,SpdPos_Flag,CMD_Values[1],CMD_Values[2],CMD_Values[3]); // debug check/
        //pc.printf("CMD: %.3f ,%.3f \r\n" ,CMD_Values[0],CMD_Values[1]); // debug check/
        led = !led;
#endif /* DEBUG_MSG */

    }//end parser
}// end parser function


// Init shika shuka
void Platform_Init()
{
    //Initializing Motor Control Components.
    if (motor1->Init() != COMPONENT_OK)
        exit(EXIT_FAILURE);
    if (motor2->Init() != COMPONENT_OK)
        exit(EXIT_FAILURE);
    if (motor3->Init() != COMPONENT_OK)
        exit(EXIT_FAILURE);

    /*----- Changing motor setting. -----*/
    /* Setting High Impedance State to update L6474's registers. */
    motor1->SoftHiZ();
    motor2->SoftHiZ();
    motor3->SoftHiZ();
    // Disabling motor
    motor1->Disable();
    motor2->Disable();
    motor3->Disable();
    /* Changing step mode. */
    motor1->SetStepMode(STEP_MODE_1_16);
    motor2->SetStepMode(STEP_MODE_1_16);
    motor3->SetStepMode(STEP_MODE_1_16);

    /* Increasing the torque regulation current. */
    motor1->SetParameter(L6474_TVAL, 1250); // Limit 2.0A
    motor2->SetParameter(L6474_TVAL, 1650); // Limit 1.7A
    motor3->SetParameter(L6474_TVAL, 300);  // Limit 0.28A

    /* Increasing the max threshold overcurrent. */
    motor1->SetParameter(L6474_OCD_TH, L6474_OCD_TH_2625mA);
    motor2->SetParameter(L6474_OCD_TH, L6474_OCD_TH_2625mA);
    motor3->SetParameter(L6474_OCD_TH, L6474_OCD_TH_750mA);

    // Enabling motor
    motor1->Enable();
    motor2->Enable();
    motor3->Enable();

    // Initialize Control Pins
    MOT1Dir.write(0);
    MOT1Step.write(0);
    MOT2Dir.write(0);
    MOT2Step.write(0);
    MOT3Dir.write(0);
    MOT3Step.write(0);

    // Start Moition Control  // need implementation
    Platform_Control_Ticker.attach_us(&Platform_Motion_Control, Motor_Control_Interval);

}// End Init shika shuka

// ShikaShuka Motion Set
void Platform_Motion_Set(float Set_M1, float Set_M2, float Set_M3)
{
    static const float MaxSPDCMD=5.0f;
    static const float DeadZoneCMD=0.0001f;
    static const float MaxAngle=180.0f;

    // Velocity control
    if(SpdPos_Flag) {
        // variable limits: (-MaxSPDCMD>SPD_M>MaxSPDCMD)
        if (Set_M1 > MaxSPDCMD)  Set_M1 = MaxSPDCMD;
        if (Set_M1 < -MaxSPDCMD) Set_M1 = -MaxSPDCMD;
        if (abs(Set_M1)  < DeadZoneCMD)  Set_M1 = 0;

        if (Set_M2 > MaxSPDCMD)  Set_M2 = MaxSPDCMD;
        if (Set_M2 < -MaxSPDCMD) Set_M2 = -MaxSPDCMD;
        if (abs(Set_M2)  < DeadZoneCMD)  Set_M2 = 0;

        if (Set_M3 > MaxSPDCMD)  Set_M3 = MaxSPDCMD;
        if (Set_M3 < -MaxSPDCMD) Set_M3 = -MaxSPDCMD;
        if (abs(Set_M3)  < DeadZoneCMD)  Set_M3 = 0;
        // enable stepper drivers
        if (EN_Stepper_Flag==0) {
            motor1->Enable();
            motor2->Enable();
            motor3->Enable();
            EN_Stepper_Flag=1;
        }
        // update motor speed command
        CMD_Motor_SPD[0]=Set_M1;
        CMD_Motor_SPD[1]=Set_M2;
        CMD_Motor_SPD[2]=Set_M3;

    } else { // end velocity control -  Start position control

        // calculate position based on steps:
        // variable limits: (-MaxSPDCMD>SPD_M>MaxSPDCMD)
        if (Set_M1 > MaxAngle)  Set_M1 = MaxAngle;
        if (Set_M1 < -MaxAngle) Set_M1 = -MaxAngle;

        if (Set_M2 > MaxAngle)  Set_M2 = MaxAngle;
        if (Set_M2 < -MaxAngle) Set_M2 = -MaxAngle;

        if (Set_M3 > MaxAngle)  Set_M3 = MaxAngle;
        if (Set_M3 < -MaxAngle) Set_M3 = -MaxAngle;
        // enable stepper drivers
        if (EN_Stepper_Flag==0) {
            motor1->Enable();
            motor2->Enable();
            motor3->Enable();
            EN_Stepper_Flag=1;
        }
        // update motor speed command
        CMD_Motor_Pos[0]=Set_M1;
        CMD_Motor_Pos[1]=Set_M2;
        CMD_Motor_Pos[2]=Set_M3;

    }// end position control
}// End Platform Motion Set

// Platform Motion Control
void Platform_Motion_Control()
{
    // variable limits: (-100>SPD_M>100)
    static const float MaxSPD=0.5f; // rounds per second
    static const float DeadZone=0.0001f;

    // update max acceleration calculation !!!!!!
    static const float MaxACC=0.5f/(1000000/Motor_Control_Interval); //acceleration set as val/sec here for open loop it is %/sec

    static float SetMotorSPD[3]= {0}; // the actual command set frequency in Hz

    // calculate motor pos:
    Motor_Pos[0]= (((float)Motor_Steps[0]) / STEPS2ROTATION) * 360.0f; // [deg] = Motor_Steps / STEPS2ROTATION *360.0f
    Motor_Pos[1]= (((float)Motor_Steps[1]) / STEPS2ROTATION) * 360.0f; // [deg] = Motor_Steps / STEPS2ROTATION *360.0f
    Motor_Pos[2]= (((float)Motor_Steps[2]) / STEPS2ROTATION) * 360.0f; // [deg] = Motor_Steps / STEPS2ROTATION *360.0f

    // position control
    if (SpdPos_Flag == 0) {
        // implement control loop based on desired position and current position
        // update velocity commands based on position control loop
        CMD_Motor_SPD[0]=(CMD_Motor_Pos[0]-Motor_Pos[0]) * PROPORTIONAL_GAIN;
        CMD_Motor_SPD[1]=(CMD_Motor_Pos[1]-Motor_Pos[1]) * PROPORTIONAL_GAIN;
        CMD_Motor_SPD[2]=(CMD_Motor_Pos[2]-Motor_Pos[2]) * PROPORTIONAL_GAIN;

    }
    //  update velocity commands
    // implement control loop here: (basic control with max acceleration limit)
    if(1) {
        if (abs(CMD_Motor_SPD[0]-Motor_SPD[0])> MaxACC) {
            CMD_Motor_SPD[0]>Motor_SPD[0] ? Motor_SPD[0]+=MaxACC : Motor_SPD[0]-=MaxACC;
        } else {
            Motor_SPD[0]=CMD_Motor_SPD[0];
        }
        if (abs(CMD_Motor_SPD[1]-Motor_SPD[1])> MaxACC) {
            CMD_Motor_SPD[1]>Motor_SPD[1] ? Motor_SPD[1]+=MaxACC : Motor_SPD[1]-=MaxACC;
        } else {
            Motor_SPD[1]=CMD_Motor_SPD[1];
        }
        if (abs(CMD_Motor_SPD[2]-Motor_SPD[2])> MaxACC) {
            CMD_Motor_SPD[2]>Motor_SPD[2] ? Motor_SPD[2]+=MaxACC : Motor_SPD[2]-=MaxACC;
        } else {
            Motor_SPD[2]=CMD_Motor_SPD[2];
        }
    }
    // update driver frequency
    if (1) {
        // Start Moition Control
        // motor 1

        // update driver direction
        if (Motor_SPD[0]>0) {
            MOT1Dir.write(1);
        } else {
            MOT1Dir.write(0);
        }

        // check if SPD is higher than minimum value
        if (abs(Motor_SPD[0])<DeadZone) {
            // disable pulsing, Set speed to zero
            Motor1_Step_Ticker.detach();
            SetMotorSPD[0]=0;

        } else {
            // Set Pulse rate based on pulses per second
            SetMotorSPD[0]=(abs(Motor_SPD[0])*STEPS2ROTATION);
            if (SetMotorSPD[0]>STEPS2ROTATION*MaxSPD) SetMotorSPD[1]=STEPS2ROTATION*MaxSPD; // make sure max speed is implemented
            if (SetMotorSPD[0]<1) SetMotorSPD[0]=1; // make sure minimum frequency
            Motor1_Step_Ticker.attach(&Motor1_Step_Control, (0.5f/(SetMotorSPD[0])));
        }

        // motor 2

        // update driver direction
        if (Motor_SPD[1]>0) {
            MOT2Dir.write(1);
        } else {
            MOT2Dir.write(0);
        }

        // check if SPD is higher than minimum value
        if (abs(Motor_SPD[1])<DeadZone) {
            // disable pulsing, Set speed to zero
            Motor2_Step_Ticker.detach();
            SetMotorSPD[1]=0;

        } else {
            // Set Pulse rate based on pulses per second
            SetMotorSPD[1]=(abs(Motor_SPD[1])*STEPS2ROTATION);
            if (SetMotorSPD[1]>STEPS2ROTATION*MaxSPD) SetMotorSPD[1]=STEPS2ROTATION*MaxSPD; // make sure max speed is implemented
            if (SetMotorSPD[1]<1) SetMotorSPD[1]=1; // make sure minimum frequency
            Motor2_Step_Ticker.attach(&Motor2_Step_Control, (0.5f/(SetMotorSPD[1])));
        }

        // Motor 3
        // update driver direction
        if (Motor_SPD[2]>0) {
            MOT3Dir.write(1);
        } else {
            MOT3Dir.write(0);
        }

        // check if SPD is higher than minimum value
        if (abs(Motor_SPD[2])<DeadZone) {
            // disable pulsing, Set speed to zero
            Motor3_Step_Ticker.detach();
            SetMotorSPD[2]=0;

        } else {
            // Set Pulse rate based on pulses per second
            SetMotorSPD[2]=(abs(Motor_SPD[2])*STEPS2ROTATION);
            if (SetMotorSPD[2]>STEPS2ROTATION*MaxSPD) SetMotorSPD[2]=STEPS2ROTATION*MaxSPD; // make sure max speed is implemented
            if (SetMotorSPD[2]<1) SetMotorSPD[2]=1; // make sure minimum frequency
            Motor3_Step_Ticker.attach(&Motor3_Step_Control, (0.5f/(SetMotorSPD[2])));
        }
    }
#ifdef DEBUG_MSG
    //pc.printf("SPD: %.3f ,%.3f,%.3f \r\n" ,SetMotorSPD[0],SetMotorSPD[1],SetMotorSPD[2]); // debug check/
    led = !led;
#endif /* DEBUG_MSG */
// update position
    Pos_Update_Flag=1;
}// End Platform Motion Control


// Motor 1 Ticker step control
void Motor1_Step_Control()
{
    MOT1Step=!MOT1Step;
    if (MOT1Step) {
        MOT1Dir ? Motor_Steps[0]++ : Motor_Steps[0]--;
    }
}// end motor 1 Step control

// Motor 2 Ticker step control
void Motor2_Step_Control()
{
    MOT2Step=!MOT2Step;
    if (MOT2Step) {
        MOT2Dir ? Motor_Steps[1]++ : Motor_Steps[1]--;
    }
}// end motor 2 Step control

// Motor 3 Ticker step control
void Motor3_Step_Control()
{
    MOT3Step=!MOT3Step;
    if (MOT3Step) {
        MOT3Dir ? Motor_Steps[2]++ : Motor_Steps[2]--;
    }
}// end motor 3 Step control