File content as of revision 23:1bcf834fb859:

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

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

/*
    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 "FastPWM.h" // high frequency pwm with good resolution
#include "l6474_class.h" // stepper library


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


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

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

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

// Motor Position control
volatile float CMD_Motor_Pos[3]= {0};
volatile float Motor_Pos[3]= {0};

// 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_SPD_M1, float Set_SPD_M2, float Set_SPD_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, D7, D9, D10, dev_spi);
    motor2 = new L6474(D2, D8, D4, D3, D10, dev_spi);
    motor3 = new L6474(D2, D8, D5, D6, D10, dev_spi);  
    
    // Setup serial
    pc.baud(57600);

    // 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();  
        static int ADC_Read_time=0;
        
        // receive Motor Command
        while (InMSG.readable()) {
            char InChar=InMSG.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;
           PitchMotor->Disable();
           RollMotor->Disable();
           CMD_Motor_SPD[0]=0;
           CMD_Motor_SPD[1]=0;
           Motor_SPD[0]=0;
           Motor_SPD[1]=0;
           EN_Stepper_Flag=0;
        }
          
        // read ADC value
        if (abs(Timer_TimeStamp_ms-ADC_Read_time)>VccReadDelay) {
           ADC_Read_time=Timer_TimeStamp_ms;
           // LPF on value
           float ReadVoltage=Vcc_11.read();
           ReadVoltage=ReadVoltage* 3.3f*11.0f; // 1/11 voltage divider
           
           static float dt_VCC_Read=((float)VccReadDelay)/1000.0f;
           static float Alpha_LPF=dt_VCC_Read/(1.0f+dt_VCC_Read) ; // α := dt / (RC + dt)
           
           // LPF: y[i] := y[i-1] + α * (x[i] - y[i-1])   
           VCC_Voltage = VCC_Voltage + Alpha_LPF * (ReadVoltage - VCC_Voltage);
           // disable motion if voltage too low
           if (VCC_Voltage<VCC_Thresh) {
                PitchMotor->Disable();
                RollMotor->Disable();
                Motor_SPD[0]=0;
                Motor_SPD[1]=0;
                EN_Stepper_Flag=0;
            }
           
#ifdef DEBUG_MSG
            //pc.printf("CMD: %.3f ,%.3f ,%.3f ,%.3f \r\n" ,CMD_Values[0],CMD_Values[1],CMD_Values[2],CMD_Values[3]); // debug check/
            //pc.printf("VCC = %.2f LPF_V %.2f V \r\n", VCC_Voltage,ReadVoltage);
            
#endif /* DEBUG_MSG */
        }
    }// 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();
        // update command
        ShikaShuka_Motion_Set(CMD_Values[2],CMD_Values[3]);
        
        // send out the remaining message for the brushed controller
        OutMSG.printf("$%.3f,%.3f\r\n",CMD_Values[0],CMD_Values[1]);
        
#ifdef DEBUG_MSG
        pc.printf("CMD: %.3f ,%.3f ,%.3f ,%.3f \r\n" ,CMD_Values[0],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, 250);  // 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
    Platform_Control_Ticker.attach_us(&ShikaShuka_Motion_Control, ShikaShuka_Control_Interval); 
    
}// End Init shika shuka
    
// ShikaShuka Motion Set
void ShikaShuka_Motion_Set(float Set_SPD_M1, float Set_SPD_M2) 
{
    static const float MaxSPDCMD=5.0f;
    static const float DeadZoneCMD=0.001f;

    // variable limits: (-MaxSPDCMD>SPD_M>MaxSPDCMD)
    if (Set_SPD_M1 > MaxSPDCMD)  Set_SPD_M1 = MaxSPDCMD;
    if (Set_SPD_M1 < -MaxSPDCMD) Set_SPD_M1 = -MaxSPDCMD;
    if (abs(Set_SPD_M1)  < DeadZoneCMD)  Set_SPD_M1 = 0;
    
    if (Set_SPD_M2 > MaxSPDCMD)  Set_SPD_M2 = MaxSPDCMD;
    if (Set_SPD_M2 < -MaxSPDCMD) Set_SPD_M2 = -MaxSPDCMD;
    if (abs(Set_SPD_M2)  < DeadZoneCMD)  Set_SPD_M2 = 0;
    // verify voltage level:
    if (VCC_Voltage>VCC_Thresh) {
        // enable stepper drivers
        if (EN_Stepper_Flag==0){
            PitchMotor->Enable();
            RollMotor->Enable();
            EN_Stepper_Flag=1;
        }
        // update motor speed command
        CMD_Motor_SPD[0]=Set_SPD_M1;
        CMD_Motor_SPD[1]=Set_SPD_M2;
    } else {
        // disable motion if voltage too low
        if (EN_Stepper_Flag==1){
            PitchMotor->Disable();
            RollMotor->Disable();
            Motor_SPD[0]=0;
            Motor_SPD[1]=0;
            EN_Stepper_Flag=0;
        }
        CMD_Motor_SPD[0]=0;
        CMD_Motor_SPD[1]=0;
    }
}// End ShikaShuka Motion Set

// ShikaShuka Motion Control
void ShikaShuka_Motion_Control()
{
    // variable limits: (-100>SPD_M>100)
    static const float MaxSPD=5.0f;
    static const float DeadZone=0.001f;
    static const float MaxACC=0.5f/(1000000/ShikaShuka_Control_Interval); //acceleration set as val/sec here for open loop it is %/sec
    
    static float SetMotorSPDPWM[2]= {0}; // the actual command set frequency in Hz

    // 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];
        }
    } 
    // update driver PWM frequency
    if (1) {
        // motor 1
        if (abs(Motor_SPD[0])<DeadZone) {
            SetMotorSPDPWM[0]=1;
            PitchStep.write(0); // disable pulsing, euqal to stop
        } else {
            // Set Pulse rate based on pulses per second  
            SetMotorSPDPWM[0]=(abs(Motor_SPD[0])*STEPS2ROTATION); // rotation per second, to pulses with duty cycle of 0.5 
            PitchStep.write(0.5f); // enable pulsing
        }
        // update driver PWM based on direction
        if (SetMotorSPDPWM[0]>STEPS2ROTATION*MaxSPD) SetMotorSPDPWM[0]=STEPS2ROTATION*MaxSPD; // make sure pwm command is trimmed
        if (SetMotorSPDPWM[0]<1) SetMotorSPDPWM[0]=1; // make sure pwm command is trimmed
        
        if (Motor_SPD[0]>0) {
            PitchDir.write(1); 
        } else {
            PitchDir.write(0);            
        }
        PitchStep.period(1.0f/(SetMotorSPDPWM[0]));
        
        // motor 2
        if (abs(Motor_SPD[1])<DeadZone) {
            SetMotorSPDPWM[1]=1;
            RollStep.write(0); // disable pulsing, euqal to stop
        } else {
            // Set Pulse rate based on pulses per second  
            SetMotorSPDPWM[1]=(abs(Motor_SPD[1])*STEPS2ROTATION); // rotation per second, to pulses with duty cycle of 0.5 
            RollStep.write(0.5f); // enable pulsing
        }
        // update driver PWM based on direction
        if (SetMotorSPDPWM[1]>STEPS2ROTATION*MaxSPD) SetMotorSPDPWM[1]=STEPS2ROTATION*MaxSPD; // make sure pwm command is trimmed
        if (SetMotorSPDPWM[1]<1) SetMotorSPDPWM[1]=1; // make sure pwm command is trimmed
        
        if (Motor_SPD[1]>0) {
            RollDir.write(1); 
        } else {
            RollDir.write(0);            
        }
        RollStep.period(1.0f/(SetMotorSPDPWM[1]));
    }
    //pc.printf("CMD: %.3f ,%.3f \r\n" ,SetMotorSPDPWM[0],SetMotorSPDPWM[1]); // debug check/
}// End ShikaShuka Motion Control