Diff: main.cpp

Revision:

0:ad32f7e53ed6

Child:

1:fd0fbabd0c8f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Jan 26 10:22:39 2016 +0000
@@ -0,0 +1,198 @@
+////////////////////////////////////////
+// Control Stepper motor using matlab //
+// Arkadiraf@gmail.com - 24/1/2016    //
+////////////////////////////////////////
+/*
+   Parts:
+    Nucleo STM32F401RE
+    X-Nucleo-IHM01A1 Stepper Driver shield     
+    
+*/
+
+// Libraries:
+#include "mbed.h"
+#include "BufferedSerial.h"  // solves issues of loosing data. alternative doing it yourself
+#include "l6474_class.h" // stepper library
+
+/////////////
+// Objects //
+////////////
+
+// create serial object
+//Serial pc(SERIAL_TX, SERIAL_RX);
+BufferedSerial pc(USBTX, USBRX);
+
+// Pinouts:
+DigitalOut myled(LED1);
+
+// Stepper Pins
+DigitalOut StepDir(D7);
+DigitalOut StepPWM(D9);
+
+// Define Stepper Ticker
+Ticker StepperTicker;
+
+// Define Ticker
+Ticker SensorSampleTicker;
+
+// define millis ticker
+volatile uint32_t MillisValue = 0;
+Ticker MillisTicket;
+
+///////////////
+// variables //
+///////////////
+char BufferCMD[64]={0};
+uint16_t BufferIndex=0;
+volatile bool Sensor_Sample_Flag=0;
+#define Sample_Interval 10000 // interval in us, (10ms, 100Hz)
+float CMDValue=0; // CMD at which speed to run
+char inbyte=' ';
+
+// Stepper Variables:
+#define STEPS2ROTATION 3200.0f // 200 steps rotation 16 microsteps
+/* Initialization parameters. */
+// not sure if it acctually uses it
+L6474_InitTypeDef init =
+{
+    50,                              /* Acceleration rate in step/s2. Range: (0..+inf). */
+    50,                              /* Deceleration rate in step/s2. Range: (0..+inf). */
+    3500,                             /* Maximum speed in step/s. Range: (30..10000]. */
+    30,                              /* Minimum speed in step/s. Range: [30..10000). */
+    500,                              /* Torque regulation current in mA. Range: 31.25mA to 4000mA. */
+    L6474_OCD_TH_1500mA,              /* Overcurrent threshold (OCD_TH register). */
+    L6474_CONFIG_OC_SD_ENABLE,        /* Overcurrent shutwdown (OC_SD field of CONFIG register). */
+    L6474_CONFIG_EN_TQREG_TVAL_USED,  /* Torque regulation method (EN_TQREG field of CONFIG register). */
+    L6474_STEP_SEL_1_16,              /* Step selection (STEP_SEL field of STEP_MODE register). */
+    L6474_SYNC_SEL_1_2,               /* Sync selection (SYNC_SEL field of STEP_MODE register). */
+    L6474_FAST_STEP_12us,             /* Fall time value (T_FAST field of T_FAST register). Range: 2us to 32us. */
+    L6474_TOFF_FAST_8us,              /* Maximum fast decay time (T_OFF field of T_FAST register). Range: 2us to 32us. */
+    3,                                /* Minimum ON time in us (TON_MIN register). Range: 0.5us to 64us. */
+    21,                               /* Minimum OFF time in us (TOFF_MIN register). Range: 0.5us to 64us. */
+    L6474_CONFIG_TOFF_044us,          /* Target Swicthing Period (field TOFF of CONFIG register). */
+    L6474_CONFIG_SR_320V_us,          /* Slew rate (POW_SR field of CONFIG register). */
+    L6474_CONFIG_INT_16MHZ,           /* Clock setting (OSC_CLK_SEL field of CONFIG register). */
+    L6474_ALARM_EN_OVERCURRENT |
+    L6474_ALARM_EN_THERMAL_SHUTDOWN |
+    L6474_ALARM_EN_THERMAL_WARNING |
+    L6474_ALARM_EN_UNDERVOLTAGE |
+    L6474_ALARM_EN_SW_TURN_ON |
+    L6474_ALARM_EN_WRONG_NPERF_CMD    /* Alarm (ALARM_EN register). */
+};
+
+
+/* Motor Control Component. */
+L6474 *StepperMotor;
+
+// variable to store stepper position
+volatile int32_t StepperPos=0;
+
+
+////////////////
+// Functions: //
+////////////////
+// Ticker function
+void Stepper_PWM() {
+    StepPWM=!StepPWM;
+    if (StepPWM==1){ //If change to high, step performed Update Stepper Position
+     if (StepDir==1){ 
+         StepperPos++;
+     }else{
+        StepperPos--;
+     }
+    }
+}
+
+// Ticker function
+void Sensor_Sample() {
+    Sensor_Sample_Flag=1;
+}
+
+// millis ticker // its roughly gets the job done, so hell why not... (Better solution is to use SysTick)
+void MillisCounter ()
+{
+    MillisValue++;
+}
+
+// Main Code:
+int main() {
+    // Setup:
+    pc.baud(921600); 
+    
+    //Attach ticker object:   
+    SensorSampleTicker.attach_us(&Sensor_Sample, Sample_Interval); // create 10Khz Ticker
+    
+    // Attach millis ticker:
+    MillisTicket.attach_us(&MillisCounter, 1000); // create 1Khz Ticker (1ms increment) 
+    
+    // Initialize Stepper:
+        /* Initializing SPI bus. */
+    DevSPI dev_spi(D11, D12, D13);
+
+    /* Initializing Motor Control Component. */
+    //StepperMotor = new L6474(D2, D8, D7, D9, D10, dev_spi);
+    StepperMotor = new L6474(D2, D8, D3, D4, D10, dev_spi); // initialization with different Step pin and Dir Pin, Interference with external control
+    if (StepperMotor->Init() != COMPONENT_OK)
+        exit(EXIT_FAILURE);
+    
+    /*----- Changing motor setting. -----*/
+    
+    /* Setting High Impedance State to update L6474's registers. */
+    StepperMotor->SoftHiZ();
+    // Disabling motor
+    StepperMotor->Disable();
+    /* Changing step mode. */
+    StepperMotor->SetStepMode(STEP_MODE_1_16);
+    /* Increasing the torque regulation current to 500mA. */
+    StepperMotor->SetParameter(L6474_TVAL, 500);
+    
+    // Enabling motor
+    StepperMotor->Enable();
+    
+    /* Waiting 1 second. */
+    wait_ms(1000);
+    
+    ////////////
+    // Loop : //
+    ////////////
+    while(1){ 
+    
+        // receive Motor Command
+        while (pc.readable()) {
+            inbyte=pc.getc();
+            //pc.printf("%c" ,inbyte); // debug check/
+            BufferCMD[BufferIndex]=inbyte;
+            BufferIndex++;
+            // parse incoming message format: "$<value>\r\n"    
+            if (inbyte=='$'){ // start of message
+                BufferIndex=0; // initialize to start of parser   
+            }else if(inbyte=='\r'){ // end of message
+               CMDValue=atof(BufferCMD);
+               BufferIndex=0;   
+               //pc.printf("CMD: %f \r\n" ,CMDValue); // debug check/ 
+               //pc.printf("CMD: %s \r\n" ,BufferCMD); // debug check/
+               
+               /* Update Stepper Commad */
+               if (CMDValue>0){
+                    StepDir=1;
+                }else{
+                    StepDir=0;
+                    CMDValue=-CMDValue;
+                }
+               if (CMDValue<1.0f){ // Stops
+                  StepperTicker.detach(); 
+               }else{
+                   StepperTicker.attach(&Stepper_PWM,2.0f/CMDValue); // Set Stepper Freq, 2.0f/Freq : 2 calls to ticker object performs a step
+               }
+     
+            }//end parser  
+        }//end serial
+        
+        if(Sensor_Sample_Flag){ // sample data and stream out.
+            Sensor_Sample_Flag=0;
+            /* Stream Data */
+            pc.printf("VAL:%d,%d\r\n" ,MillisValue,StepperPos); // Stream data
+        }// End sampling and stream data
+    }// end loop
+}// end main
+