Prova con controller motore
Dependencies: X-NUCLEO-IHM05A1 mbed
Fork of HelloWorld_IHM05A1 by
Diff: main.cpp
- Revision:
- 0:36aa6787d4f9
- Child:
- 2:c5f455885df7
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Wed Apr 27 16:32:15 2016 +0000 @@ -0,0 +1,307 @@ +/** + ****************************************************************************** + * @file main.cpp + * @author IPC Rennes + * @version V1.0.0 + * @date April 13th, 2016 + * @brief mbed simple application for the STMicroelectronics X-NUCLEO-IHM03A1 + * Motor Control Expansion Board: control of 1 motor. + ****************************************************************************** + * @attention + * + * <h2><center>© 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" + +/* Helper header files. */ +#include "DevSPI.h" + +/* Component specific header files. */ +#include "l6208_class.h" + + +/* Definitions ---------------------------------------------------------------*/ +#ifdef TARGET_NUCLEO_F334R8 +#define VREFA_PWM_PIN D11 +#else +#define VREFA_PWM_PIN D3 +#endif + +/* Variables -----------------------------------------------------------------*/ + +/* Initialization parameters of the motor connected to the expansion board. */ +l6208_Init_t initDeviceParameters = +{ + 1500, //Acceleration rate in step/s^2 or (1/16)th step/s^2 for microstep modes + 40, //Acceleration current torque in % (from 0 to 100) + 1500, //Deceleration rate in step/s^2 or (1/16)th step/s^2 for microstep modes + 30, //Deceleration current torque in % (from 0 to 100) + 1500, //Running speed in step/s or (1/16)th step/s for microstep modes + 50, //Running current torque in % (from 0 to 100) + 20, //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) +}; + +/* Motor Control Component. */ +L6208 *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 unit is in agreement to the step mode.\r\n"); + printf(" The speed, acceleration or deceleration unit depend on the step mode:\r\n"); + printf(" - For normal mode and half step mode, the unit is steps/s or /s^2.\r\n"); + printf(" - For microstep modes, the unit is (1/16)steps/s or /s^2.\r\n"); + +//----- Initialization + /* Initializing Motor Control Component. */ + motor = new L6208(D2, D8, D7, D4, D5, D6, VREFA_PWM_PIN, D9); + 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"); + +//----- 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 microstep/s.\r\n", motor->GetSpeed()); + //wait_ms(50); + } + //printf(" Reached Speed: %d microstep/s.\r\n", motor->GetSpeed()); + + /* Wait for 1 second */ + wait_ms(1000); + +//----- Decrease speed while running to one quarter of the previous speed + motor->SetMaxSpeed(motor->GetSpeed()>>2); + + /* 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 microstep/s.\r\n", motor->GetSpeed()); + //wait_ms(50); + } + //printf(" Reached Speed: %d microstep/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 (0:FS, 1:1/2, 2:1/4, 3:1/8, 4:1/16).\r\n", motor->GetStepMode()); + + /* 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 normal mode */ + motor->SetMaxSpeed(initDeviceParameters.maxSpeedSps>>4); + motor->SetAcceleration(motor->GetAcceleration()>>4); + motor->SetDeceleration(motor->GetDeceleration()>>4); + /* 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()); + +//----- 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); + + //----- Change step mode to 1/4 microstepping mode + motor->SetStepMode(StepperMotor::STEP_MODE_1_4); + printf(" Motor step mode: %d (0:FS, 1:1/2, 2:1/4, 3:1/8, 4:1/16).\r\n", motor->GetStepMode()); + + /* 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 microstep mode */ + motor->SetMaxSpeed(motor->GetMaxSpeed()<<4); + motor->SetAcceleration(motor->GetAcceleration()<<4); + motor->SetDeceleration(motor->GetDeceleration()<<4); + /* 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()); + + /* Request to go position 800 (quarter steps) */ + motor->GoTo(800); + + /* 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); + +//----- Restore step mode to its initialization value + motor->SetStepMode((StepperMotor::step_mode_t)initDeviceParameters.stepMode); + printf(" Motor step mode: %d (0:FS, 1:1/2, 2:1/4, 3:1/8, 4:1/16).\r\n", motor->GetStepMode()); + + /* Get current position of device and print to the console */ + printf(" Position: %d.\r\n", motor->GetPosition()); + +//----- Change decay mode + motor->SetDecayMode(SLOW_DECAY); + printf(" Motor decay mode: %d (0:slow decay, 1:fast decay).\r\n", motor->GetDecayMode()); + +//----- 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); + +//----- Restore decay mode to its initialization value + motor->SetDecayMode(initDeviceParameters.decayMode); + printf(" Motor decay mode: %d (0:slow decay, 1:fast decay).\r\n", motor->GetDecayMode()); + +//----- 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(); + } +} +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/