The Technology Partnership / Mbed 2 deprecated KYPHOS_Stepper_Motor_Control

Stepper motor control for KYPHOS rig

Dependencies:   mbed X_NUCLEO_IHM03A1 HX711

main.cpp@2:ad3a480d3376, 2020-03-16 (annotated)

Committer:
paullj
Date:
Mon Mar 16 15:27:39 2020 +0000
Revision:
2:ad3a480d3376
Parent:
1:5822a95491d3 
Added basic motor control;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
paullj 0:1cb05bc2807c 1 #include "mbed.h"
paullj 1:5822a95491d3 2 #include "DevSPI.h"
paullj 1:5822a95491d3 3
paullj 0:1cb05bc2807c 4 #include "PowerStep01.h"
paullj 0:1cb05bc2807c 5
paullj 0:1cb05bc2807c 6 powerstep01_init_u_t init = {
paullj 0:1cb05bc2807c 7 /* common parameters */
paullj 0:1cb05bc2807c 8 .cm.cp.cmVmSelection = POWERSTEP01_CM_VM_CURRENT, // enum powerstep01_CmVm_t
paullj 0:1cb05bc2807c 9 582, // Acceleration rate in step/s2, range 14.55 to 59590 steps/s^2
paullj 0:1cb05bc2807c 10 582, // Deceleration rate in step/s2, range 14.55 to 59590 steps/s^2
paullj 2:ad3a480d3376 11 48, // Maximum speed in step/s, range 15.25 to 15610 steps/s
paullj 0:1cb05bc2807c 12 0, // Minimum speed in step/s, range 0 to 976.3 steps/s
paullj 0:1cb05bc2807c 13 POWERSTEP01_LSPD_OPT_OFF, // Low speed optimization bit, enum powerstep01_LspdOpt_t
paullj 0:1cb05bc2807c 14 244.16, // Full step speed in step/s, range 7.63 to 15625 steps/s
paullj 0:1cb05bc2807c 15 POWERSTEP01_BOOST_MODE_OFF, // Boost of the amplitude square wave, enum powerstep01_BoostMode_t
paullj 0:1cb05bc2807c 16 281.25, // Overcurrent threshold settings via enum powerstep01_OcdTh_t
paullj 0:1cb05bc2807c 17 STEP_MODE_1_16, // Step mode settings via enum motorStepMode_t
paullj 0:1cb05bc2807c 18 POWERSTEP01_SYNC_SEL_DISABLED, // Synch. Mode settings via enum powerstep01_SyncSel_t
paullj 0:1cb05bc2807c 19 (POWERSTEP01_ALARM_EN_OVERCURRENT|
paullj 0:1cb05bc2807c 20 POWERSTEP01_ALARM_EN_THERMAL_SHUTDOWN|
paullj 0:1cb05bc2807c 21 POWERSTEP01_ALARM_EN_THERMAL_WARNING|
paullj 0:1cb05bc2807c 22 POWERSTEP01_ALARM_EN_UVLO|
paullj 0:1cb05bc2807c 23 POWERSTEP01_ALARM_EN_STALL_DETECTION|
paullj 0:1cb05bc2807c 24 POWERSTEP01_ALARM_EN_SW_TURN_ON|
paullj 0:1cb05bc2807c 25 POWERSTEP01_ALARM_EN_WRONG_NPERF_CMD), // Alarm settings via bitmap enum powerstep01_AlarmEn_t
paullj 0:1cb05bc2807c 26 POWERSTEP01_IGATE_64mA, // Gate sink/source current via enum powerstep01_Igate_t
paullj 0:1cb05bc2807c 27 POWERSTEP01_TBOOST_0ns, // Duration of the overboost phase during gate turn-off via enum powerstep01_Tboost_t
paullj 0:1cb05bc2807c 28 POWERSTEP01_TCC_500ns, // Controlled current time via enum powerstep01_Tcc_t
paullj 0:1cb05bc2807c 29 POWERSTEP01_WD_EN_DISABLE, // External clock watchdog, enum powerstep01_WdEn_t
paullj 0:1cb05bc2807c 30 POWERSTEP01_TBLANK_375ns, // Duration of the blanking time via enum powerstep01_TBlank_t
paullj 0:1cb05bc2807c 31 POWERSTEP01_TDT_125ns, // Duration of the dead time via enum powerstep01_Tdt_t
paullj 0:1cb05bc2807c 32 /* current mode parameters */
paullj 0:1cb05bc2807c 33 328.12, // Hold torque in mV, range from 7.8mV to 1000 mV
paullj 0:1cb05bc2807c 34 328.12, // Running torque in mV, range from 7.8mV to 1000 mV
paullj 0:1cb05bc2807c 35 328.12, // Acceleration torque in mV, range from 7.8mV to 1000 mV
paullj 0:1cb05bc2807c 36 328.12, // Deceleration torque in mV, range from 7.8mV to 1000 mV
paullj 0:1cb05bc2807c 37 POWERSTEP01_TOFF_FAST_8us, //Maximum fast decay time , enum powerstep01_ToffFast_t
paullj 0:1cb05bc2807c 38 POWERSTEP01_FAST_STEP_12us, //Maximum fall step time , enum powerstep01_FastStep_t
paullj 0:1cb05bc2807c 39 3.0, // Minimum on-time in us, range 0.5us to 64us
paullj 0:1cb05bc2807c 40 21.0, // Minimum off-time in us, range 0.5us to 64us
paullj 0:1cb05bc2807c 41 POWERSTEP01_CONFIG_INT_16MHZ_OSCOUT_2MHZ, // Clock setting , enum powerstep01_ConfigOscMgmt_t
paullj 0:1cb05bc2807c 42 POWERSTEP01_CONFIG_SW_HARD_STOP, // External switch hard stop interrupt mode, enum powerstep01_ConfigSwMode_t
paullj 0:1cb05bc2807c 43 POWERSTEP01_CONFIG_TQ_REG_TVAL_USED, // External torque regulation enabling , enum powerstep01_ConfigEnTqReg_t
paullj 0:1cb05bc2807c 44 POWERSTEP01_CONFIG_VS_COMP_DISABLE, // Motor Supply Voltage Compensation enabling , enum powerstep01_ConfigEnVscomp_t
paullj 0:1cb05bc2807c 45 POWERSTEP01_CONFIG_OC_SD_DISABLE, // Over current shutwdown enabling, enum powerstep01_ConfigOcSd_t
paullj 0:1cb05bc2807c 46 POWERSTEP01_CONFIG_UVLOVAL_LOW, // UVLO Threshold via powerstep01_ConfigUvLoVal_t
paullj 0:1cb05bc2807c 47 POWERSTEP01_CONFIG_VCCVAL_15V, // VCC Val, enum powerstep01_ConfigVccVal_t
paullj 0:1cb05bc2807c 48 POWERSTEP01_CONFIG_TSW_048us, // Switching period, enum powerstep01_ConfigTsw_t
paullj 0:1cb05bc2807c 49 POWERSTEP01_CONFIG_PRED_DISABLE // Predictive current enabling , enum powerstep01_ConfigPredEn_t
paullj 0:1cb05bc2807c 50 };
paullj 0:1cb05bc2807c 51
paullj 1:5822a95491d3 52 DigitalIn button(USER_BUTTON);
paullj 0:1cb05bc2807c 53 PowerStep01 *motor;
paullj 0:1cb05bc2807c 54
paullj 0:1cb05bc2807c 55 void flag_irq_handler(void)
paullj 0:1cb05bc2807c 56 {
paullj 0:1cb05bc2807c 57 /* Set ISR flag. */
paullj 0:1cb05bc2807c 58 motor->isrFlag = TRUE;
paullj 0:1cb05bc2807c 59 /* Get the value of the status register. */
paullj 0:1cb05bc2807c 60 unsigned int statusRegister = motor->get_status();
paullj 0:1cb05bc2807c 61 printf(" WARNING: \"FLAG\" interrupt triggered.\r\n");
paullj 0:1cb05bc2807c 62 /* Check SW_F flag: if not set, the SW input is opened */
paullj 0:1cb05bc2807c 63 if ((statusRegister & POWERSTEP01_STATUS_SW_F ) != 0) {
paullj 0:1cb05bc2807c 64 printf(" SW closed (connected to ground).\r\n");
paullj 0:1cb05bc2807c 65 }
paullj 0:1cb05bc2807c 66 /* Check SW_EN bit */
paullj 0:1cb05bc2807c 67 if ((statusRegister & POWERSTEP01_STATUS_SW_EVN) == POWERSTEP01_STATUS_SW_EVN) {
paullj 0:1cb05bc2807c 68 printf(" SW turn_on event.\r\n");
paullj 0:1cb05bc2807c 69 }
paullj 0:1cb05bc2807c 70 /* Check Command Error flag: if set, the command received by SPI can't be */
paullj 0:1cb05bc2807c 71 /* performed. This occurs for instance when a move command is sent to the */
paullj 0:1cb05bc2807c 72 /* Powerstep01 while it is already running */
paullj 0:1cb05bc2807c 73 if ((statusRegister & POWERSTEP01_STATUS_CMD_ERROR) == POWERSTEP01_STATUS_CMD_ERROR) {
paullj 0:1cb05bc2807c 74 printf(" Non-performable command detected.\r\n");
paullj 0:1cb05bc2807c 75 }
paullj 0:1cb05bc2807c 76 /* Check UVLO flag: if not set, there is an undervoltage lock-out */
paullj 0:1cb05bc2807c 77 if ((statusRegister & POWERSTEP01_STATUS_UVLO)==0) {
paullj 0:1cb05bc2807c 78 printf(" undervoltage lock-out.\r\n");
paullj 0:1cb05bc2807c 79 }
paullj 0:1cb05bc2807c 80 /* Check thermal STATUS flags: if set, the thermal status is not normal */
paullj 0:1cb05bc2807c 81 if ((statusRegister & POWERSTEP01_STATUS_TH_STATUS)!=0) {
paullj 0:1cb05bc2807c 82 //thermal status: 1: Warning, 2: Bridge shutdown, 3: Device shutdown
paullj 0:1cb05bc2807c 83 printf(" Thermal status: %d.\r\n", (statusRegister & POWERSTEP01_STATUS_TH_STATUS)>>11);
paullj 0:1cb05bc2807c 84 }
paullj 0:1cb05bc2807c 85 /* Check OCD flag: if not set, there is an overcurrent detection */
paullj 0:1cb05bc2807c 86 if ((statusRegister & POWERSTEP01_STATUS_OCD)==0) {
paullj 0:1cb05bc2807c 87 printf(" Overcurrent detection.\r\n");
paullj 0:1cb05bc2807c 88 }
paullj 0:1cb05bc2807c 89 /* Reset ISR flag. */
paullj 0:1cb05bc2807c 90 motor->isrFlag = FALSE;
paullj 0:1cb05bc2807c 91 }
paullj 0:1cb05bc2807c 92
paullj 0:1cb05bc2807c 93 /**
paullj 0:1cb05bc2807c 94 * @brief This is an example of error handler.
paullj 0:1cb05bc2807c 95 * @param[in] error Number of the error
paullj 0:1cb05bc2807c 96 * @retval None
paullj 0:1cb05bc2807c 97 * @note If needed, implement it, and then attach it:
paullj 0:1cb05bc2807c 98 * + motor->attach_error_handler(&my_error_handler);
paullj 0:1cb05bc2807c 99 */
paullj 0:1cb05bc2807c 100 void error_handler(uint16_t error)
paullj 0:1cb05bc2807c 101 {
paullj 0:1cb05bc2807c 102 printf("Error %d detected\r\n\n", error);
paullj 1:5822a95491d3 103 while (true);
paullj 0:1cb05bc2807c 104 }
paullj 0:1cb05bc2807c 105
paullj 0:1cb05bc2807c 106 /* Main ----------------------------------------------------------------------*/
paullj 0:1cb05bc2807c 107
paullj 0:1cb05bc2807c 108 int main()
paullj 0:1cb05bc2807c 109 {
paullj 0:1cb05bc2807c 110 printf("STARTING MAIN PROGRAM\r\n");
paullj 0:1cb05bc2807c 111
paullj 0:1cb05bc2807c 112 /* Initializing SPI bus. */
paullj 0:1cb05bc2807c 113 DevSPI dev_spi(D11, D12, D13);
paullj 0:1cb05bc2807c 114
paullj 0:1cb05bc2807c 115 /* Initializing Motor Control Component. */
paullj 0:1cb05bc2807c 116 motor = new PowerStep01(D2, D4, D8, D9, D10, dev_spi);
paullj 0:1cb05bc2807c 117 if (motor->init(&init) != COMPONENT_OK) {
paullj 0:1cb05bc2807c 118 exit(EXIT_FAILURE);
paullj 0:1cb05bc2807c 119 }
paullj 0:1cb05bc2807c 120
paullj 0:1cb05bc2807c 121 /* Attaching and enabling an interrupt handler. */
paullj 0:1cb05bc2807c 122 motor->attach_flag_irq(&flag_irq_handler);
paullj 0:1cb05bc2807c 123 motor->enable_flag_irq();
paullj 0:1cb05bc2807c 124
paullj 0:1cb05bc2807c 125 /* Attaching an error handler */
paullj 0:1cb05bc2807c 126 motor->attach_error_handler(&error_handler);
paullj 0:1cb05bc2807c 127
paullj 1:5822a95491d3 128 printf("Press button to start motor control.\r\n");
paullj 1:5822a95491d3 129 while(button);
paullj 2:ad3a480d3376 130
paullj 2:ad3a480d3376 131 printf("--> Going to start position.\r\n");
paullj 2:ad3a480d3376 132 motor->move(StepperMotor::FWD, 2000);
paullj 0:1cb05bc2807c 133 motor->wait_while_active();
paullj 0:1cb05bc2807c 134
paullj 2:ad3a480d3376 135 while(button) {
paullj 2:ad3a480d3376 136 //----- Go to position 7680
paullj 2:ad3a480d3376 137 printf("--> Go to position -2000 steps.\r\n");
paullj 2:ad3a480d3376 138 /* Request device to go to position 7680 */
paullj 2:ad3a480d3376 139 motor->go_to(-2000);
paullj 2:ad3a480d3376 140 /* Wait for the motor ends moving */
paullj 2:ad3a480d3376 141 motor->wait_while_active();
paullj 2:ad3a480d3376 142 printf("--> Go to position 2000 steps.\r\n");
paullj 2:ad3a480d3376 143 motor->go_to(2000);
paullj 2:ad3a480d3376 144 /* Wait for the motor ends moving */
paullj 2:ad3a480d3376 145 motor->wait_while_active();
paullj 2:ad3a480d3376 146 }
paullj 2:ad3a480d3376 147
paullj 1:5822a95491d3 148 //----- Go Home
paullj 0:1cb05bc2807c 149 /* Printing to the console. */
paullj 0:1cb05bc2807c 150 printf("--> Go to home position.\r\n");
paullj 0:1cb05bc2807c 151
paullj 0:1cb05bc2807c 152 /* Request device to go to Home */
paullj 0:1cb05bc2807c 153 motor->go_home();
paullj 0:1cb05bc2807c 154 motor->wait_while_active();
paullj 0:1cb05bc2807c 155 /* Request a soft stop of device and keep the power bridges enabled */
paullj 0:1cb05bc2807c 156 motor->soft_hiz();
paullj 0:1cb05bc2807c 157 /* Wait for the motor of device ends moving */
paullj 0:1cb05bc2807c 158 motor->wait_while_active();
paullj 0:1cb05bc2807c 159 }