Diff: main.cpp

Revision:

0:9ebe3081c641

Child:

4:0e50964ff610

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu May 26 15:54:22 2016 +0000
@@ -0,0 +1,313 @@
+/**
+ ******************************************************************************
+ * @file    main.cpp
+ * @author  IPC Rennes
+ * @version V1.0.0
+ * @date    May 26th, 2016
+ * @brief   mbed simple application for the STMicroelectronics X-NUCLEO-IHM06A1
+ *          Motor Control Expansion Board: control of 1 motor.
+ ******************************************************************************
+ * @attention
+ *
+ * <h2><center>&copy; COPYRIGHT(c) 2016 STMicroelectronics</center></h2>
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ *   1. Redistributions of source code must retain the above copyright notice,
+ *      this list of conditions and the following disclaimer.
+ *   2. Redistributions in binary form must reproduce the above copyright notice,
+ *      this list of conditions and the following disclaimer in the documentation
+ *      and/or other materials provided with the distribution.
+ *   3. Neither the name of STMicroelectronics nor the names of its contributors
+ *      may be used to endorse or promote products derived from this software
+ *      without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ ******************************************************************************
+ */
+
+/* Includes ------------------------------------------------------------------*/
+
+/* mbed specific header files. */
+#include "mbed.h"
+
+/* Component specific header files. */
+#include "stspin220_class.h"
+
+/* Definitions ---------------------------------------------------------------*/
+#ifdef TARGET_NUCLEO_F302R8
+#define PWM_REF_PIN D11 /* HW mandatory patch: bridge manually D9 with D10 */
+#else
+#define PWM_REF_PIN D9
+#endif
+/* Uncomment the line below to enable the step clock monitoring duration */
+//#define STEP_CLOCK_HANDLER_DURATION_MONITORING
+#ifdef STEP_CLOCK_HANDLER_DURATION_MONITORING
+#define MONITORING_PIN D15
+#endif
+
+/* Variables -----------------------------------------------------------------*/
+
+/* Initialization parameters of the motor connected to the expansion board. */
+Stspin220_Init_t initDeviceParameters =
+{
+  10000,           //Acceleration rate in pulse/s2 (must be greater than 0)
+  10000,           //Deceleration rate in pulse/s2 (must be greater than 0)
+  10000,           //Running speed in pulse/s (8 pulse/s < Maximum speed <= 10 000 pulse/s )
+  400,             //Minimum speed in pulse/s (8 pulse/s <= Minimum speed < 10 000 pulse/s)
+  20,              //Acceleration current torque in % (from 0 to 100)
+  15,              //Deceleration current torque in % (from 0 to 100)
+  10,              //Running current torque in % (from 0 to 100)
+  25,              //Holding current torque in % (from 0 to 100)
+  TRUE,            //Torque boost speed enable
+  200,             //Torque boost speed threshold in fullstep/s
+  STEP_MODE_1_32,  //Step mode via enum motorStepMode_t  
+  HIZ_MODE,        //Automatic HIZ STOP
+  100000           //REF frequency (Hz)
+};
+
+/* Motor Control Component. */
+STSPIN220 *motor;
+
+/* Functions -----------------------------------------------------------------*/
+
+/**
+ * @brief  This is an example of user handler for the flag interrupt.
+ * @param  None
+ * @retval None
+ * @note   If needed, implement it, and then attach and enable it:
+ *           + motor->AttachFlagIRQ(&myFlagIRQHandler);
+ *           + motor->EnableFlagIRQ();
+ *         To disable it:
+ *           + motor->DisbleFlagIRQ();
+ */
+void myFlagIRQHandler(void)
+{
+  printf("    WARNING: \"FLAG\" interrupt triggered:\r\n");
+  motor->Disable();
+  printf("    Motor disabled.\r\n\n");
+}
+
+/**
+ * @brief  This is an example of error handler.
+ * @param[in] error Number of the error
+ * @retval None
+ * @note   If needed, implement it, and then attach it:
+ *           + motor->AttachErrorHandler(&myErrorHandler);
+ */
+void myErrorHandler(uint16_t error)
+{
+  /* Printing to the console. */
+  printf("Error %d detected\r\n\n", error);
+  
+  /* Infinite loop */
+  while(1)
+  {
+  }    
+}
+
+/* Main ----------------------------------------------------------------------*/
+
+int main()
+{
+  /* Printing to the console. */
+  printf("STARTING MAIN PROGRAM\r\n");
+  printf("    Reminder:\r\n");
+  printf("    The position, speed, acceleration and deceleration units\r\n");
+  printf("    are in agreement to the step mode.\r\n");
+  printf("    In example if the step mode is 1/32th,\r\n");
+  printf("    the position is in 1/32th step,\r\n");
+  printf("    the speed is in 1/32th step/s,\r\n");
+  printf("    the acceleration and the deceleration are in 1/32th step/s^2.\r\n");
+    
+//----- Initialization 
+  /* Initializing Motor Control Component. */
+#ifdef STEP_CLOCK_HANDLER_DURATION_MONITORING
+  motor = new STSPIN220(D2, D8, D7, D5, D10, D3, PWM_REF_PIN, MONITORING_PIN);
+#else
+  motor = new STSPIN220(D2, D8, D7, D5, D10, D3, PWM_REF_PIN);
+#endif
+
+  if (motor->Init(&initDeviceParameters) != COMPONENT_OK) exit(EXIT_FAILURE);
+
+  /* Attaching and enabling an interrupt handler. */
+  motor->AttachFlagIRQ(&myFlagIRQHandler);
+  motor->EnableFlagIRQ();
+    
+  /* Attaching an error handler */
+  motor->AttachErrorHandler(&myErrorHandler);
+
+  /* Printing to the console. */
+  printf("Motor Control Application Example for 1 Motor\r\n");
+  
+//----- Get the step mode after initialization 
+  StepperMotor::step_mode_t myStepMode = motor->GetStepMode();
+
+//----- Run the motor BACKWARD
+  printf("--> Running the motor backward.\r\n");
+  motor->Run(StepperMotor::BWD);
+  
+  while (motor->GetStatus()!=STEADY)
+  {
+    /* Print reached speed to the console in step/s or microsteps/s */
+    printf("    Reached Speed: %d step/s.\r\n", motor->GetSpeed());
+    wait_ms(50);    
+  }
+  printf("    Reached Speed: %d step/s.\r\n", motor->GetSpeed());
+     
+  /* Wait for 1 second */  
+  wait_ms(1000);
+  
+//----- Decrease speed while running to one quarter of the previous speed
+  printf("    Motor init step mode: %d\r\n", myStepMode);
+  int currentStepMode = motor->GetStepMode();
+  printf("    Motor current step mode: %d\r\n", currentStepMode);
+  int newSpeed = (motor->GetSpeed()<<(myStepMode-currentStepMode))>>2;
+  printf("    Set motor max speed to: %d step/s.\r\n", newSpeed);
+  if (!motor->SetMaxSpeed(newSpeed))
+  {
+    printf("    Failed: target speed below min speed.\r\n");
+    if (motor->SetMaxSpeed(motor->GetMinSpeed()))
+      printf("    Motor max speed set to min speed: %d step/s.\r\n", motor->GetMinSpeed());
+    else
+      printf("    Failed: check all speed and boost setting.\r\n");
+  }
+  
+  /* Wait until the motor starts decelerating */
+  while (motor->GetStatus()==STEADY);
+  /* Wait and print speed while the motor is not steady running */
+  while (motor->GetStatus()!=STEADY)
+  {
+    /* Print reached speed to the console in step/s or microsteps/s */
+    printf("    Reached Speed: %d step/s.\r\n", motor->GetSpeed());
+    wait_ms(50);    
+  }
+  printf("    Reached Speed: %d step/s.\r\n", motor->GetSpeed());  
+
+  /* Wait for 5 seconds */  
+  wait_ms(5000);
+  
+//----- Soft stop required while running
+  printf("--> Soft stop requested.\r\n");
+  motor->SoftStop();
+  
+  /* Wait for the motor of device ends moving */  
+  motor->WaitWhileActive();
+
+  /* Wait for 2 seconds */
+  wait_ms(2000);
+
+//----- Change step mode to full step mode
+  motor->SetStepMode(StepperMotor::STEP_MODE_FULL);
+  printf("    Motor step mode: %d\r\n", motor->GetStepMode());
+  printf("    0:FS      1:1/2     2:1/4\r\n    3:1/8     4:1/16    5:1/32\r\n");
+  printf("    6:1/64    7:1/128   8:1/256\r\n");
+    
+  /* Get current position of device and print to the console */
+  printf("    Position: %d.\r\n", motor->GetPosition());
+  
+  /* Set speed, acceleration and deceleration to scale with full step mode */
+  motor->SetMinSpeed(initDeviceParameters.minSpeed>>myStepMode);  
+  motor->SetMaxSpeed(initDeviceParameters.maxSpeed>>myStepMode);
+  motor->SetAcceleration(motor->GetAcceleration()>>myStepMode);
+  motor->SetDeceleration(motor->GetDeceleration()>>myStepMode);
+  /* Print parameters to the console */
+  printf("    Motor Min Speed: %d step/s.\r\n", motor->GetMinSpeed());  
+  printf("    Motor Max Speed: %d step/s.\r\n", motor->GetMaxSpeed());
+  printf("    Motor Acceleration: %d step/s.\r\n", motor->GetAcceleration());
+  printf("    Motor Deceleration: %d step/s.\r\n", motor->GetDeceleration());
+  
+//----- Move of 200 steps in the FW direction
+  printf("--> Moving forward 200 steps.\r\n");
+  motor->Move(StepperMotor::FWD, 200);
+  
+  /* Waiting while the motor is active. */
+  motor->WaitWhileActive();
+  
+  /* Get current position of device and print to the console */
+  printf("    Position: %d.\r\n", motor->GetPosition());
+  
+  /* Disable the power bridges */
+  motor->Disable();
+  
+  /* Check that the power bridges are actually disabled */
+  if (motor->CheckStatusHw()!=0) printf("    Motor driver disabled.\r\n");
+  else printf("    Failed to disable the motor driver.\r\n");
+    
+  /* Wait for 2 seconds */  
+  wait_ms(2000);
+  
+//----- Restore step mode to its initialization value
+  motor->SetStepMode((StepperMotor::step_mode_t)initDeviceParameters.stepMode);
+  printf("    Motor step mode: %d\r\n", motor->GetStepMode());
+  printf("    0:FS      1:1/2     2:1/4\r\n    3:1/8     4:1/16    5:1/32\r\n");
+  printf("    6:1/64    7:1/128   8:1/256\r\n");
+
+  /* Set speed, acceleration and deceleration to scale with microstep mode */
+  motor->SetMaxSpeed(motor->GetMaxSpeed()<<myStepMode);
+  motor->SetMinSpeed(motor->GetMinSpeed()<<myStepMode);
+  motor->SetAcceleration(motor->GetAcceleration()<<myStepMode);
+  motor->SetDeceleration(motor->GetDeceleration()<<myStepMode);
+  /* Print parameters to the console */  
+  printf("    Motor Max Speed: %d step/s.\r\n", motor->GetMaxSpeed());
+  printf("    Motor Min Speed: %d step/s.\r\n", motor->GetMinSpeed());
+  printf("    Motor Acceleration: %d step/s.\r\n", motor->GetAcceleration());
+  printf("    Motor Deceleration: %d step/s.\r\n", motor->GetDeceleration());
+
+  /* Get current position of device and print to the console */
+  printf("    Position: %d.\r\n", motor->GetPosition());
+  
+//----- Change torque
+  motor->SetTorque(ACC_TORQUE,30);
+  motor->SetTorque(DEC_TORQUE,20);
+  printf("    Motor acceleration and deceleration torque changed to: %d and %d.\r\n", motor->GetTorque(ACC_TORQUE), motor->GetTorque(DEC_TORQUE));   
+  
+//----- Go to position -6400
+  printf("--> Go to position -6400 steps.\r\n");
+  motor->GoTo(-6400);
+  
+  /* Wait for the motor ends moving */
+  motor->WaitWhileActive();
+
+  /* Get current position of device and print to the console */
+  printf("    Position: %d.\r\n", motor->GetPosition());
+  
+  /* Wait for 2 seconds */  
+  wait_ms(2000);
+  
+//----- Go Home
+  printf("--> Go to home position.\r\n");
+  motor->GoHome();
+ 
+  /* Wait for the motor ends moving */
+  motor->WaitWhileActive();
+  
+  /* Wait for 1 second */
+  wait_ms(1000);
+  
+  /* Infinite Loop. */
+  printf("--> Infinite Loop...\r\n");
+  while (1)
+  {
+    /* Request device to go position -3200 */
+    motor->GoTo(-3200);
+    /* Waiting while the motor is active. */
+    motor->WaitWhileActive();
+    /* Request device to go position 3200 */
+    motor->GoTo(3200);
+    /* Waiting while the motor is active. */
+    motor->WaitWhileActive();
+    wait_ms(500);
+  }
+}
+/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/