Thomas Byrne / Mbed 2 deprecated HelloWorld_IHM02A1

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed tinyshell X_NUCLEO_IHM02A1

main.cpp@28:19b25daa7777, 2021-04-01 (annotated)

Committer:
tom_astranis
Date:
Thu Apr 01 00:43:00 2021 +0000
Revision:
28:19b25daa7777
Parent:
27:2abcf13d90a3
Child:
29:a510e875936d
Added tinyshell and command to print status register. Not tested.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Davidroid 0:5148e9486cf2 1 /**
Davidroid 0:5148e9486cf2 2 ******************************************************************************
Davidroid 0:5148e9486cf2 3 * @file main.cpp
Davidroid 12:5be6dd48b94a 4 * @author Davide Aliprandi, STMicroelectronics
Davidroid 0:5148e9486cf2 5 * @version V1.0.0
Davidroid 0:5148e9486cf2 6 * @date November 4th, 2015
Davidroid 12:5be6dd48b94a 7 * @brief mbed test application for the STMicroelectronics X-NUCLEO-IHM02A1
Davidroid 1:9f1974b0960d 8 * Motor Control Expansion Board: control of 2 motors.
Davidroid 0:5148e9486cf2 9 ******************************************************************************
Davidroid 0:5148e9486cf2 10 * @attention
Davidroid 0:5148e9486cf2 11 *
Davidroid 0:5148e9486cf2 12 * <h2><center>&copy; COPYRIGHT(c) 2015 STMicroelectronics</center></h2>
Davidroid 0:5148e9486cf2 13 *
Davidroid 0:5148e9486cf2 14 * Redistribution and use in source and binary forms, with or without modification,
Davidroid 0:5148e9486cf2 15 * are permitted provided that the following conditions are met:
Davidroid 0:5148e9486cf2 16 * 1. Redistributions of source code must retain the above copyright notice,
Davidroid 0:5148e9486cf2 17 * this list of conditions and the following disclaimer.
Davidroid 0:5148e9486cf2 18 * 2. Redistributions in binary form must reproduce the above copyright notice,
Davidroid 0:5148e9486cf2 19 * this list of conditions and the following disclaimer in the documentation
Davidroid 0:5148e9486cf2 20 * and/or other materials provided with the distribution.
Davidroid 0:5148e9486cf2 21 * 3. Neither the name of STMicroelectronics nor the names of its contributors
Davidroid 0:5148e9486cf2 22 * may be used to endorse or promote products derived from this software
Davidroid 0:5148e9486cf2 23 * without specific prior written permission.
Davidroid 0:5148e9486cf2 24 *
Davidroid 0:5148e9486cf2 25 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
Davidroid 0:5148e9486cf2 26 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
Davidroid 0:5148e9486cf2 27 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
Davidroid 0:5148e9486cf2 28 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
Davidroid 0:5148e9486cf2 29 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
Davidroid 0:5148e9486cf2 30 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
Davidroid 0:5148e9486cf2 31 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
Davidroid 0:5148e9486cf2 32 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
Davidroid 0:5148e9486cf2 33 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
Davidroid 0:5148e9486cf2 34 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
Davidroid 0:5148e9486cf2 35 *
Davidroid 0:5148e9486cf2 36 ******************************************************************************
Davidroid 0:5148e9486cf2 37 */
Davidroid 0:5148e9486cf2 38
Davidroid 0:5148e9486cf2 39
Davidroid 0:5148e9486cf2 40 /* Includes ------------------------------------------------------------------*/
Davidroid 0:5148e9486cf2 41
Davidroid 0:5148e9486cf2 42 /* mbed specific header files. */
Davidroid 0:5148e9486cf2 43 #include "mbed.h"
Davidroid 0:5148e9486cf2 44
Davidroid 0:5148e9486cf2 45 /* Helper header files. */
Davidroid 0:5148e9486cf2 46 #include "DevSPI.h"
Davidroid 0:5148e9486cf2 47
Davidroid 0:5148e9486cf2 48 /* Expansion Board specific header files. */
Davidroid 26:caec5f51abe8 49 #include "XNucleoIHM02A1.h"
Davidroid 0:5148e9486cf2 50
tom_astranis 28:19b25daa7777 51 /* Tinyshell: https://os.mbed.com/users/murilopontes/code/tinyshell/ */
tom_astranis 28:19b25daa7777 52 #include "tinysh.h"
Davidroid 0:5148e9486cf2 53
Davidroid 0:5148e9486cf2 54 /* Definitions ---------------------------------------------------------------*/
Davidroid 0:5148e9486cf2 55
tom_astranis 27:2abcf13d90a3 56 /* Motor specs */
tom_astranis 27:2abcf13d90a3 57 #define MOTOR_SUPPLY_VOLTAGE_V 12.0
tom_astranis 27:2abcf13d90a3 58 #define STEPS_PER_REV 200.0
tom_astranis 27:2abcf13d90a3 59 #define MAX_PHASE_CURRENT_A 0.67
tom_astranis 27:2abcf13d90a3 60 #define PHASE_RES_OHMS 12.0
tom_astranis 27:2abcf13d90a3 61 #define MOTOR_INITIAL_SPEED_SPS 1000.0
tom_astranis 27:2abcf13d90a3 62 #define MOTOR_ACCEL_SPS2 200.0
tom_astranis 27:2abcf13d90a3 63 #define MOTOR_MAX_SPEED_SPS 2000.0
tom_astranis 27:2abcf13d90a3 64 #define FULL_STEP_TH_SPS 602.7
tom_astranis 27:2abcf13d90a3 65 #define DUMMY_KVAL_V 3.06
tom_astranis 27:2abcf13d90a3 66 #define BEMF_ICPT_SPS 61.52
tom_astranis 27:2abcf13d90a3 67 #define START_SLOPE 392.1569e-6
tom_astranis 27:2abcf13d90a3 68 #define FINAL_SLOPE 643.1372e-6
tom_astranis 27:2abcf13d90a3 69 #define OCD_TH_MA 600.0
tom_astranis 27:2abcf13d90a3 70 #define STALL_TH_MA 1000.0
tom_astranis 27:2abcf13d90a3 71
tom_astranis 28:19b25daa7777 72 /* Behavioral stuff */
tom_astranis 28:19b25daa7777 73 #define STATUS_LOG_RATE_HZ 1
tom_astranis 28:19b25daa7777 74 #define MIN_LOOP_TIME_MS 1
tom_astranis 28:19b25daa7777 75 #define HARD_STOP_WAIT_MS 100
tom_astranis 28:19b25daa7777 76
tom_astranis 28:19b25daa7777 77 #define CMD_BUFFER_SIZE 256
tom_astranis 28:19b25daa7777 78 #define OUTPUT_BUFFER_SIZE 1024
Davidroid 1:9f1974b0960d 79
Davidroid 1:9f1974b0960d 80 /* Number of steps. */
tom_astranis 27:2abcf13d90a3 81 #define STEPS_1 (20000 * 1) /* 1 revolution given a 200 step motor and 100:1 gearbox, with full stepping */
Davidroid 18:591a007effc2 82 #define STEPS_2 (STEPS_1 * 2)
tom_astranis 27:2abcf13d90a3 83 #define SPEED_SPS 10000 // steps per second
Davidroid 0:5148e9486cf2 84
tom_astranis 28:19b25daa7777 85 /* Number of movements per revolution. */
tom_astranis 28:19b25daa7777 86 #define MPR_1 4
tom_astranis 28:19b25daa7777 87
Davidroid 0:5148e9486cf2 88 /* Delay in milliseconds. */
Davidroid 1:9f1974b0960d 89 #define DELAY_1 1000
Davidroid 0:5148e9486cf2 90 #define DELAY_2 2000
Davidroid 0:5148e9486cf2 91 #define DELAY_3 5000
Davidroid 0:5148e9486cf2 92
Davidroid 0:5148e9486cf2 93
Davidroid 0:5148e9486cf2 94 /* Variables -----------------------------------------------------------------*/
Davidroid 0:5148e9486cf2 95
Davidroid 0:5148e9486cf2 96 /* Motor Control Expansion Board. */
Davidroid 26:caec5f51abe8 97 XNucleoIHM02A1 *x_nucleo_ihm02a1;
Davidroid 0:5148e9486cf2 98
Davidroid 9:f35fbeedb8f4 99 /* Initialization parameters of the motors connected to the expansion board. */
davide.aliprandi@st.com 24:d1f487cb02ba 100 L6470_init_t init[L6470DAISYCHAINSIZE] = {
Davidroid 9:f35fbeedb8f4 101 /* First Motor. */
Davidroid 9:f35fbeedb8f4 102 {
tom_astranis 27:2abcf13d90a3 103 MOTOR_SUPPLY_VOLTAGE_V, /* Motor supply voltage in V. */
tom_astranis 27:2abcf13d90a3 104 STEPS_PER_REV, /* Min number of steps per revolution for the motor. */
tom_astranis 27:2abcf13d90a3 105 MAX_PHASE_CURRENT_A, /* Max motor phase voltage in A. */
tom_astranis 27:2abcf13d90a3 106 MAX_PHASE_CURRENT_A * PHASE_RES_OHMS, /* Max motor phase voltage in V. (12 ohms per phase) */
tom_astranis 27:2abcf13d90a3 107 MOTOR_INITIAL_SPEED_SPS, /* Motor initial speed [step/s]. */
tom_astranis 27:2abcf13d90a3 108 MOTOR_ACCEL_SPS2, /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
tom_astranis 27:2abcf13d90a3 109 MOTOR_ACCEL_SPS2, /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */
tom_astranis 27:2abcf13d90a3 110 MOTOR_MAX_SPEED_SPS, /* Motor maximum speed [step/s]. */
tom_astranis 27:2abcf13d90a3 111 0.0, /* Motor minimum speed [step/s]. */
tom_astranis 27:2abcf13d90a3 112 FULL_STEP_TH_SPS, /* Motor full-step speed threshold [step/s]. */
tom_astranis 27:2abcf13d90a3 113 DUMMY_KVAL_V, /* Holding kval [V]. */
tom_astranis 27:2abcf13d90a3 114 DUMMY_KVAL_V, /* Constant speed kval [V]. */
tom_astranis 27:2abcf13d90a3 115 DUMMY_KVAL_V, /* Acceleration starting kval [V]. */
tom_astranis 27:2abcf13d90a3 116 DUMMY_KVAL_V, /* Deceleration starting kval [V]. */
tom_astranis 27:2abcf13d90a3 117 BEMF_ICPT_SPS, /* Intersect speed for bemf compensation curve slope changing [step/s]. */
tom_astranis 27:2abcf13d90a3 118 START_SLOPE, /* Start slope [s/step]. */
tom_astranis 27:2abcf13d90a3 119 FINAL_SLOPE, /* Acceleration final slope [s/step]. */
tom_astranis 27:2abcf13d90a3 120 FINAL_SLOPE, /* Deceleration final slope [s/step]. */
tom_astranis 27:2abcf13d90a3 121 0, /* Thermal compensation factor (range [0, 15]). */
tom_astranis 27:2abcf13d90a3 122 OCD_TH_MA, /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
tom_astranis 27:2abcf13d90a3 123 STALL_TH_MA, /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */
tom_astranis 27:2abcf13d90a3 124 StepperMotor::STEP_MODE_FULL, /* Step mode selection. */
tom_astranis 27:2abcf13d90a3 125 0xFF, /* Alarm conditions enable. */
tom_astranis 27:2abcf13d90a3 126 0x2E88 /* Ic configuration. */
Davidroid 9:f35fbeedb8f4 127 },
Davidroid 9:f35fbeedb8f4 128
Davidroid 9:f35fbeedb8f4 129 /* Second Motor. */
Davidroid 9:f35fbeedb8f4 130 {
tom_astranis 27:2abcf13d90a3 131 MOTOR_SUPPLY_VOLTAGE_V, /* Motor supply voltage in V. */
tom_astranis 27:2abcf13d90a3 132 STEPS_PER_REV, /* Min number of steps per revolution for the motor. */
tom_astranis 27:2abcf13d90a3 133 MAX_PHASE_CURRENT_A, /* Max motor phase voltage in A. */
tom_astranis 27:2abcf13d90a3 134 MAX_PHASE_CURRENT_A * PHASE_RES_OHMS, /* Max motor phase voltage in V. (12 ohms per phase) */
tom_astranis 27:2abcf13d90a3 135 MOTOR_INITIAL_SPEED_SPS, /* Motor initial speed [step/s]. */
tom_astranis 27:2abcf13d90a3 136 MOTOR_ACCEL_SPS2, /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
tom_astranis 27:2abcf13d90a3 137 MOTOR_ACCEL_SPS2, /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */
tom_astranis 27:2abcf13d90a3 138 MOTOR_MAX_SPEED_SPS, /* Motor maximum speed [step/s]. */
tom_astranis 27:2abcf13d90a3 139 0.0, /* Motor minimum speed [step/s]. */
tom_astranis 27:2abcf13d90a3 140 FULL_STEP_TH_SPS, /* Motor full-step speed threshold [step/s]. */
tom_astranis 27:2abcf13d90a3 141 DUMMY_KVAL_V, /* Holding kval [V]. */
tom_astranis 27:2abcf13d90a3 142 DUMMY_KVAL_V, /* Constant speed kval [V]. */
tom_astranis 27:2abcf13d90a3 143 DUMMY_KVAL_V, /* Acceleration starting kval [V]. */
tom_astranis 27:2abcf13d90a3 144 DUMMY_KVAL_V, /* Deceleration starting kval [V]. */
tom_astranis 27:2abcf13d90a3 145 BEMF_ICPT_SPS, /* Intersect speed for bemf compensation curve slope changing [step/s]. */
tom_astranis 27:2abcf13d90a3 146 START_SLOPE, /* Start slope [s/step]. */
tom_astranis 27:2abcf13d90a3 147 FINAL_SLOPE, /* Acceleration final slope [s/step]. */
tom_astranis 27:2abcf13d90a3 148 FINAL_SLOPE, /* Deceleration final slope [s/step]. */
tom_astranis 27:2abcf13d90a3 149 0, /* Thermal compensation factor (range [0, 15]). */
tom_astranis 27:2abcf13d90a3 150 OCD_TH_MA, /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
tom_astranis 27:2abcf13d90a3 151 STALL_TH_MA, /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */
tom_astranis 27:2abcf13d90a3 152 StepperMotor::STEP_MODE_FULL, /* Step mode selection. */
tom_astranis 27:2abcf13d90a3 153 0xFF, /* Alarm conditions enable. */
tom_astranis 27:2abcf13d90a3 154 0x2E88 /* Ic configuration. */
Davidroid 9:f35fbeedb8f4 155 }
Davidroid 9:f35fbeedb8f4 156 };
Davidroid 9:f35fbeedb8f4 157
tom_astranis 28:19b25daa7777 158 /* Serial Port for console */
tom_astranis 28:19b25daa7777 159 Serial pc(USBTX, USBRX);
tom_astranis 28:19b25daa7777 160
tom_astranis 28:19b25daa7777 161 /* Nasty globals haha */
tom_astranis 28:19b25daa7777 162 bool limit_flag = false;
tom_astranis 28:19b25daa7777 163 bool status_flag = false;
tom_astranis 28:19b25daa7777 164 char cmd_buffer[CMD_BUFFER_SIZE];
tom_astranis 28:19b25daa7777 165 char output_buffer[OUTPUT_BUFFER_SIZE];
tom_astranis 28:19b25daa7777 166
tom_astranis 28:19b25daa7777 167 /* Tinyshell command handler functions */
tom_astranis 28:19b25daa7777 168 void print_status(int argc, char **argv)
tom_astranis 28:19b25daa7777 169 {
tom_astranis 28:19b25daa7777 170 status_flag = true;
tom_astranis 28:19b25daa7777 171 }
tom_astranis 28:19b25daa7777 172
tom_astranis 28:19b25daa7777 173 // parent cmd (0 for top) cmd input name, help string, usage string,
tom_astranis 28:19b25daa7777 174 // function to launch, arg when called, next (0 at init), child (0 at init)
tom_astranis 28:19b25daa7777 175 tinysh_cmd_t print_status_cmd = {0, "status", "status command", "[args]", print_status,0,0,0};
tom_astranis 28:19b25daa7777 176
tom_astranis 28:19b25daa7777 177 /* mandatory tiny shell output function */
tom_astranis 28:19b25daa7777 178 void tinysh_char_out(unsigned char c)
tom_astranis 28:19b25daa7777 179 {
tom_astranis 28:19b25daa7777 180 pc.putc(c);
tom_astranis 28:19b25daa7777 181 }
Davidroid 0:5148e9486cf2 182
Davidroid 0:5148e9486cf2 183 /* Main ----------------------------------------------------------------------*/
Davidroid 0:5148e9486cf2 184
Davidroid 0:5148e9486cf2 185 int main()
Davidroid 0:5148e9486cf2 186 {
Davidroid 1:9f1974b0960d 187 /*----- Initialization. -----*/
tom_astranis 28:19b25daa7777 188 unsigned int status_bytes[L6470DAISYCHAINSIZE];
tom_astranis 28:19b25daa7777 189
tom_astranis 28:19b25daa7777 190 /* Set up tinyshell */
tom_astranis 28:19b25daa7777 191 pc.baud(115200);
tom_astranis 28:19b25daa7777 192 pc.printf("Motor controller ready\r\n");
tom_astranis 28:19b25daa7777 193 tinysh_set_prompt("$ ");
tom_astranis 28:19b25daa7777 194
tom_astranis 28:19b25daa7777 195 // Add all tinyshell commands here
tom_astranis 28:19b25daa7777 196 tinysh_add_command(&print_status_cmd);
Davidroid 1:9f1974b0960d 197
Davidroid 2:41eeee48951b 198 /* Initializing SPI bus. */
Davidroid 23:073b26366d03 199 #ifdef TARGET_STM32F429
Davidroid 23:073b26366d03 200 DevSPI dev_spi(D11, D12, D13);
Davidroid 23:073b26366d03 201 #else
Davidroid 3:fd280b953f77 202 DevSPI dev_spi(D11, D12, D3);
Davidroid 23:073b26366d03 203 #endif
Davidroid 0:5148e9486cf2 204
Davidroid 5:3b8e19bbf386 205 /* Initializing Motor Control Expansion Board. */
Davidroid 26:caec5f51abe8 206 x_nucleo_ihm02a1 = new XNucleoIHM02A1(&init[0], &init[1], A4, A5, D4, A2, &dev_spi);
Davidroid 0:5148e9486cf2 207
Davidroid 1:9f1974b0960d 208 /* Building a list of motor control components. */
davide.aliprandi@st.com 24:d1f487cb02ba 209 L6470 **motors = x_nucleo_ihm02a1->get_components();
Davidroid 0:5148e9486cf2 210
tom_astranis 28:19b25daa7777 211 // Hello world: just run the damn motor forever
tom_astranis 28:19b25daa7777 212 printf("Running.\r\n");
Davidroid 22:e81ccf73bc5d 213 for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
tom_astranis 27:2abcf13d90a3 214 motors[m]->prepare_run(StepperMotor::BWD, SPEED_SPS);
Davidroid 22:e81ccf73bc5d 215 }
davide.aliprandi@st.com 24:d1f487cb02ba 216 x_nucleo_ihm02a1->perform_prepared_actions();
tom_astranis 27:2abcf13d90a3 217
tom_astranis 28:19b25daa7777 218
tom_astranis 28:19b25daa7777 219 // Main loop
tom_astranis 28:19b25daa7777 220 while(1) {
tom_astranis 28:19b25daa7777 221 /* 1: Check for hardware flags ----------------------------------------*/
tom_astranis 28:19b25daa7777 222 if (limit_flag) {
tom_astranis 28:19b25daa7777 223 // Hard stop
tom_astranis 28:19b25daa7777 224 for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
tom_astranis 28:19b25daa7777 225 motors[m]->prepare_hard_stop();
tom_astranis 28:19b25daa7777 226 }
tom_astranis 28:19b25daa7777 227 x_nucleo_ihm02a1->perform_prepared_actions();
tom_astranis 28:19b25daa7777 228
tom_astranis 28:19b25daa7777 229 wait_ms(HARD_STOP_WAIT_MS);
tom_astranis 28:19b25daa7777 230
tom_astranis 28:19b25daa7777 231 // High Z
tom_astranis 28:19b25daa7777 232 for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
tom_astranis 28:19b25daa7777 233 motors[m]->prepare_hard_hiz();
tom_astranis 28:19b25daa7777 234 }
tom_astranis 28:19b25daa7777 235 x_nucleo_ihm02a1->perform_prepared_actions();
tom_astranis 28:19b25daa7777 236
tom_astranis 28:19b25daa7777 237 printf("Reached limit.\r\n");
tom_astranis 28:19b25daa7777 238 limit_flag = false;
tom_astranis 28:19b25daa7777 239 }
tom_astranis 28:19b25daa7777 240
tom_astranis 28:19b25daa7777 241 /* 2: Fetch new chars for Tinyshell ----------------------------------*/
tom_astranis 28:19b25daa7777 242 tinysh_char_in(pc.getc());
tom_astranis 28:19b25daa7777 243
tom_astranis 28:19b25daa7777 244 /* 3: Handle Commands ------------------------------------------------*/
tom_astranis 28:19b25daa7777 245 if (status_flag) {
tom_astranis 28:19b25daa7777 246 // Fetch and parse the status register for each motor
tom_astranis 28:19b25daa7777 247 for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
tom_astranis 28:19b25daa7777 248 status_bytes[m] = motors[m]->get_status();
tom_astranis 28:19b25daa7777 249 printf("Status reg %d: 0x%02X\r\n", m, status_bytes[m]);
tom_astranis 28:19b25daa7777 250 printf(" STEP-CLOCK MODE: ");
tom_astranis 28:19b25daa7777 251 printf(status_bytes[m] & 0x8000 ? "SET\r\n" : "NOT SET\r\n");
tom_astranis 28:19b25daa7777 252 printf(" STEP_LOSS_B: ");
tom_astranis 28:19b25daa7777 253 printf(status_bytes[m] & 0x4000 ? "SET\r\n" : "NOT SET\r\n");
tom_astranis 28:19b25daa7777 254 printf(" STEP_LOSS_A: ");
tom_astranis 28:19b25daa7777 255 printf(status_bytes[m] & 0x2000 ? "SET\r\n" : "NOT SET\r\n");
tom_astranis 28:19b25daa7777 256 printf(" OVERCURRENT DETECT: ");
tom_astranis 28:19b25daa7777 257 printf(status_bytes[m] & 0x1000 ? "SET\r\n" : "NOT SET\r\n");
tom_astranis 28:19b25daa7777 258 printf(" THERMAL SHUTDOWN: ");
tom_astranis 28:19b25daa7777 259 printf(status_bytes[m] & 0x0800 ? "SET\r\n" : "NOT SET\r\n");
tom_astranis 28:19b25daa7777 260 printf(" THERMAL WARN: ");
tom_astranis 28:19b25daa7777 261 printf(status_bytes[m] & 0x0400 ? "SET\r\n" : "NOT SET\r\n");
tom_astranis 28:19b25daa7777 262 printf(" UNDERVOLTAGE LOCKOUT: ");
tom_astranis 28:19b25daa7777 263 printf(status_bytes[m] & 0x0200 ? "SET\r\n" : "NOT SET\r\n");
tom_astranis 28:19b25daa7777 264 printf(" WRONG_CMD: ");
tom_astranis 28:19b25daa7777 265 printf(status_bytes[m] & 0x0100 ? "SET\r\n" : "NOT SET\r\n");
tom_astranis 28:19b25daa7777 266 printf(" NOTPERF_CMD: ");
tom_astranis 28:19b25daa7777 267 printf(status_bytes[m] & 0x0080 ? "SET\r\n" : "NOT SET\r\n");
tom_astranis 28:19b25daa7777 268 printf(" MOTOR_STATUS: ");
tom_astranis 28:19b25daa7777 269 if ((status_bytes[m] && 0x0060) >> 5 == 0x00) printf("STOPPED\r\n");
tom_astranis 28:19b25daa7777 270 if ((status_bytes[m] && 0x0060) >> 5 == 0x01) printf("ACCELERATING\r\n");
tom_astranis 28:19b25daa7777 271 if ((status_bytes[m] && 0x0060) >> 5 == 0x10) printf("DECELERATING\r\n");
tom_astranis 28:19b25daa7777 272 if ((status_bytes[m] && 0x0060) >> 5 == 0x11) printf("CONSTANT SPEED\r\n");
tom_astranis 28:19b25daa7777 273 printf(" DIRECTION: ");
tom_astranis 28:19b25daa7777 274 printf(status_bytes[m] & 0x0010 ? "FWD\r\n" : "REV\r\n");
tom_astranis 28:19b25daa7777 275 printf(" SWITCH TURN-ON EVENT: ");
tom_astranis 28:19b25daa7777 276 printf(status_bytes[m] & 0x0008 ? "SET\r\n" : "NOT SET\r\n");
tom_astranis 28:19b25daa7777 277 printf(" SWITCH STATUS: ");
tom_astranis 28:19b25daa7777 278 printf(status_bytes[m] & 0x0004 ? "CLOSED\r\n" : "OPEN\r\n");
tom_astranis 28:19b25daa7777 279 printf(" BUSY: ");
tom_astranis 28:19b25daa7777 280 printf(status_bytes[m] & 0x0002 ? "SET\r\n" : "NOT SET\r\n");
tom_astranis 28:19b25daa7777 281 printf(" HI_Z: ");
tom_astranis 28:19b25daa7777 282 printf(status_bytes[m] & 0x0001 ? "SET\r\n" : "NOT SET\r\n");
tom_astranis 28:19b25daa7777 283 printf("\n\n");
tom_astranis 28:19b25daa7777 284 }
tom_astranis 28:19b25daa7777 285 }
tom_astranis 28:19b25daa7777 286
tom_astranis 28:19b25daa7777 287 } // end main loop
tom_astranis 27:2abcf13d90a3 288 }
Davidroid 0:5148e9486cf2 289
Davidroid 0:5148e9486cf2 290
tom_astranis 27:2abcf13d90a3 291 // /* Printing to the console. */
tom_astranis 27:2abcf13d90a3 292 // printf("--> Setting home position.\r\n");
tom_astranis 27:2abcf13d90a3 293 //
tom_astranis 27:2abcf13d90a3 294 // /* Setting the home position. */
tom_astranis 27:2abcf13d90a3 295 // motors[0]->set_home();
Davidroid 0:5148e9486cf2 296
tom_astranis 27:2abcf13d90a3 297 // /* Waiting. */
tom_astranis 27:2abcf13d90a3 298 // wait_ms(DELAY_1);
tom_astranis 27:2abcf13d90a3 299 //
tom_astranis 27:2abcf13d90a3 300 // /* Getting the current position. */
tom_astranis 27:2abcf13d90a3 301 // int position = motors[0]->get_position();
tom_astranis 27:2abcf13d90a3 302 //
tom_astranis 27:2abcf13d90a3 303 // /* Printing to the console. */
tom_astranis 27:2abcf13d90a3 304 // printf("--> Getting the current position: %d\r\n", position);
Davidroid 1:9f1974b0960d 305
tom_astranis 27:2abcf13d90a3 306 // /* Waiting. */
tom_astranis 27:2abcf13d90a3 307 // wait_ms(DELAY_1);
tom_astranis 27:2abcf13d90a3 308 //
tom_astranis 27:2abcf13d90a3 309 // /* Printing to the console. */
tom_astranis 27:2abcf13d90a3 310 // printf("--> Moving forward %d steps.\r\n", STEPS_1);
tom_astranis 27:2abcf13d90a3 311 //
tom_astranis 27:2abcf13d90a3 312 // /* Moving. */
tom_astranis 27:2abcf13d90a3 313 // motors[0]->move(StepperMotor::FWD, STEPS_1);
tom_astranis 27:2abcf13d90a3 314 //
tom_astranis 27:2abcf13d90a3 315 // /* Waiting while active. */
tom_astranis 27:2abcf13d90a3 316 // motors[0]->wait_while_active();
tom_astranis 27:2abcf13d90a3 317 //
tom_astranis 27:2abcf13d90a3 318 // /* Getting the current position. */
tom_astranis 27:2abcf13d90a3 319 // position = motors[0]->get_position();
tom_astranis 27:2abcf13d90a3 320 //
tom_astranis 27:2abcf13d90a3 321 // /* Printing to the console. */
tom_astranis 27:2abcf13d90a3 322 // printf("--> Getting the current position: %d\r\n", position);
tom_astranis 27:2abcf13d90a3 323 //
tom_astranis 27:2abcf13d90a3 324 // /* Printing to the console. */
tom_astranis 27:2abcf13d90a3 325 // printf("--> Marking the current position.\r\n");
tom_astranis 27:2abcf13d90a3 326 //
tom_astranis 27:2abcf13d90a3 327 // /* Marking the current position. */
tom_astranis 27:2abcf13d90a3 328 // motors[0]->set_mark();
tom_astranis 27:2abcf13d90a3 329 //
tom_astranis 27:2abcf13d90a3 330 // /* Waiting. */
tom_astranis 27:2abcf13d90a3 331 // wait_ms(DELAY_1);
tom_astranis 27:2abcf13d90a3 332 //
tom_astranis 27:2abcf13d90a3 333 // /* Printing to the console. */
tom_astranis 27:2abcf13d90a3 334 // printf("--> Moving backward %d steps.\r\n", STEPS_2);
tom_astranis 27:2abcf13d90a3 335 //
tom_astranis 27:2abcf13d90a3 336 // /* Moving. */
tom_astranis 27:2abcf13d90a3 337 // motors[0]->move(StepperMotor::BWD, STEPS_2);
tom_astranis 27:2abcf13d90a3 338 //
tom_astranis 27:2abcf13d90a3 339 // /* Waiting while active. */
tom_astranis 27:2abcf13d90a3 340 // motors[0]->wait_while_active();
tom_astranis 27:2abcf13d90a3 341 //
tom_astranis 27:2abcf13d90a3 342 // /* Waiting. */
tom_astranis 27:2abcf13d90a3 343 // wait_ms(DELAY_1);
tom_astranis 27:2abcf13d90a3 344 //
tom_astranis 27:2abcf13d90a3 345 // /* Getting the current position. */
tom_astranis 27:2abcf13d90a3 346 // position = motors[0]->get_position();
tom_astranis 27:2abcf13d90a3 347 //
tom_astranis 27:2abcf13d90a3 348 // /* Printing to the console. */
tom_astranis 27:2abcf13d90a3 349 // printf("--> Getting the current position: %d\r\n", position);
tom_astranis 27:2abcf13d90a3 350 //
tom_astranis 27:2abcf13d90a3 351 // /* Waiting. */
tom_astranis 27:2abcf13d90a3 352 // wait_ms(DELAY_1);
tom_astranis 27:2abcf13d90a3 353 //
tom_astranis 27:2abcf13d90a3 354 // /* Printing to the console. */
tom_astranis 27:2abcf13d90a3 355 // printf("--> Going to marked position.\r\n");
tom_astranis 27:2abcf13d90a3 356 //
tom_astranis 27:2abcf13d90a3 357 // /* Going to marked position. */
tom_astranis 27:2abcf13d90a3 358 // motors[0]->go_mark();
tom_astranis 27:2abcf13d90a3 359 //
tom_astranis 27:2abcf13d90a3 360 // /* Waiting while active. */
tom_astranis 27:2abcf13d90a3 361 // motors[0]->wait_while_active();
tom_astranis 27:2abcf13d90a3 362 //
tom_astranis 27:2abcf13d90a3 363 // /* Waiting. */
tom_astranis 27:2abcf13d90a3 364 // wait_ms(DELAY_1);
tom_astranis 27:2abcf13d90a3 365 //
tom_astranis 27:2abcf13d90a3 366 // /* Getting the current position. */
tom_astranis 27:2abcf13d90a3 367 // position = motors[0]->get_position();
tom_astranis 27:2abcf13d90a3 368 //
tom_astranis 27:2abcf13d90a3 369 // /* Printing to the console. */
tom_astranis 27:2abcf13d90a3 370 // printf("--> Getting the current position: %d\r\n", position);
tom_astranis 27:2abcf13d90a3 371 //
tom_astranis 27:2abcf13d90a3 372 // /* Waiting. */
tom_astranis 27:2abcf13d90a3 373 // wait_ms(DELAY_1);
tom_astranis 27:2abcf13d90a3 374 //
tom_astranis 27:2abcf13d90a3 375 // /* Printing to the console. */
tom_astranis 27:2abcf13d90a3 376 // printf("--> Going to home position.\r\n");
tom_astranis 27:2abcf13d90a3 377 //
tom_astranis 27:2abcf13d90a3 378 // /* Going to home position. */
tom_astranis 27:2abcf13d90a3 379 // motors[0]->go_home();
tom_astranis 27:2abcf13d90a3 380 //
tom_astranis 27:2abcf13d90a3 381 // /* Waiting while active. */
tom_astranis 27:2abcf13d90a3 382 // motors[0]->wait_while_active();
tom_astranis 27:2abcf13d90a3 383 //
tom_astranis 27:2abcf13d90a3 384 // /* Waiting. */
tom_astranis 27:2abcf13d90a3 385 // wait_ms(DELAY_1);
tom_astranis 27:2abcf13d90a3 386 //
tom_astranis 27:2abcf13d90a3 387 // /* Getting the current position. */
tom_astranis 27:2abcf13d90a3 388 // position = motors[0]->get_position();
tom_astranis 27:2abcf13d90a3 389 //
tom_astranis 27:2abcf13d90a3 390 // /* Printing to the console. */
tom_astranis 27:2abcf13d90a3 391 // printf("--> Getting the current position: %d\r\n", position);
tom_astranis 27:2abcf13d90a3 392 //
tom_astranis 27:2abcf13d90a3 393 // /* Waiting. */
tom_astranis 27:2abcf13d90a3 394 // wait_ms(DELAY_1);
tom_astranis 27:2abcf13d90a3 395 //
tom_astranis 27:2abcf13d90a3 396 // /* Printing to the console. */
tom_astranis 27:2abcf13d90a3 397 // printf("--> Halving the microsteps.\r\n");
tom_astranis 27:2abcf13d90a3 398 //
tom_astranis 27:2abcf13d90a3 399 // /* Halving the microsteps. */
tom_astranis 27:2abcf13d90a3 400 // init[0].step_sel = (init[0].step_sel > 0 ? init[0].step_sel - 1 : init[0].step_sel);
tom_astranis 27:2abcf13d90a3 401 // if (!motors[0]->set_step_mode((StepperMotor::step_mode_t) init[0].step_sel)) {
tom_astranis 27:2abcf13d90a3 402 // printf(" Step Mode not allowed.\r\n");
tom_astranis 27:2abcf13d90a3 403 // }
tom_astranis 27:2abcf13d90a3 404 //
tom_astranis 27:2abcf13d90a3 405 // /* Waiting. */
tom_astranis 27:2abcf13d90a3 406 // wait_ms(DELAY_1);
tom_astranis 27:2abcf13d90a3 407 //
tom_astranis 27:2abcf13d90a3 408 // /* Printing to the console. */
tom_astranis 27:2abcf13d90a3 409 // printf("--> Setting home position.\r\n");
tom_astranis 27:2abcf13d90a3 410 //
tom_astranis 27:2abcf13d90a3 411 // /* Setting the home position. */
tom_astranis 27:2abcf13d90a3 412 // motors[0]->set_home();
tom_astranis 27:2abcf13d90a3 413 //
tom_astranis 27:2abcf13d90a3 414 // /* Waiting. */
tom_astranis 27:2abcf13d90a3 415 // wait_ms(DELAY_1);
tom_astranis 27:2abcf13d90a3 416 //
tom_astranis 27:2abcf13d90a3 417 // /* Getting the current position. */
tom_astranis 27:2abcf13d90a3 418 // position = motors[0]->get_position();
tom_astranis 27:2abcf13d90a3 419 //
tom_astranis 27:2abcf13d90a3 420 // /* Printing to the console. */
tom_astranis 27:2abcf13d90a3 421 // printf("--> Getting the current position: %d\r\n", position);
tom_astranis 27:2abcf13d90a3 422 //
tom_astranis 27:2abcf13d90a3 423 // /* Waiting. */
tom_astranis 27:2abcf13d90a3 424 // wait_ms(DELAY_1);
tom_astranis 27:2abcf13d90a3 425 //
tom_astranis 27:2abcf13d90a3 426 // /* Printing to the console. */
tom_astranis 27:2abcf13d90a3 427 // printf("--> Moving forward %d steps.\r\n", STEPS_1);
tom_astranis 27:2abcf13d90a3 428 //
tom_astranis 27:2abcf13d90a3 429 // /* Moving. */
tom_astranis 27:2abcf13d90a3 430 // motors[0]->move(StepperMotor::FWD, STEPS_1);
tom_astranis 27:2abcf13d90a3 431 //
tom_astranis 27:2abcf13d90a3 432 // /* Waiting while active. */
tom_astranis 27:2abcf13d90a3 433 // motors[0]->wait_while_active();
tom_astranis 27:2abcf13d90a3 434 //
tom_astranis 27:2abcf13d90a3 435 // /* Getting the current position. */
tom_astranis 27:2abcf13d90a3 436 // position = motors[0]->get_position();
tom_astranis 27:2abcf13d90a3 437 //
tom_astranis 27:2abcf13d90a3 438 // /* Printing to the console. */
tom_astranis 27:2abcf13d90a3 439 // printf("--> Getting the current position: %d\r\n", position);
tom_astranis 27:2abcf13d90a3 440 //
tom_astranis 27:2abcf13d90a3 441 // /* Printing to the console. */
tom_astranis 27:2abcf13d90a3 442 // printf("--> Marking the current position.\r\n");
tom_astranis 27:2abcf13d90a3 443 //
tom_astranis 27:2abcf13d90a3 444 // /* Marking the current position. */
tom_astranis 27:2abcf13d90a3 445 // motors[0]->set_mark();
tom_astranis 27:2abcf13d90a3 446 //
tom_astranis 27:2abcf13d90a3 447 // /* Waiting. */
tom_astranis 27:2abcf13d90a3 448 // wait_ms(DELAY_2);
tom_astranis 27:2abcf13d90a3 449 //
tom_astranis 27:2abcf13d90a3 450 //
tom_astranis 27:2abcf13d90a3 451 // /*----- Running together for a certain amount of time. -----*/
tom_astranis 27:2abcf13d90a3 452 //
tom_astranis 27:2abcf13d90a3 453 // /* Printing to the console. */
tom_astranis 27:2abcf13d90a3 454 // printf("--> Running together for %d seconds.\r\n", DELAY_3 / 1000);
tom_astranis 27:2abcf13d90a3 455 //
tom_astranis 27:2abcf13d90a3 456 // /* Preparing each motor to perform a run at a specified speed. */
tom_astranis 27:2abcf13d90a3 457 // for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
tom_astranis 27:2abcf13d90a3 458 // motors[m]->prepare_run(StepperMotor::BWD, 400);
tom_astranis 27:2abcf13d90a3 459 // }
tom_astranis 27:2abcf13d90a3 460 //
tom_astranis 27:2abcf13d90a3 461 // /* Performing the action on each motor at the same time. */
tom_astranis 27:2abcf13d90a3 462 // x_nucleo_ihm02a1->perform_prepared_actions();
tom_astranis 27:2abcf13d90a3 463 //
tom_astranis 27:2abcf13d90a3 464 // /* Waiting. */
tom_astranis 27:2abcf13d90a3 465 // wait_ms(DELAY_3);
tom_astranis 27:2abcf13d90a3 466 //
tom_astranis 27:2abcf13d90a3 467 //
tom_astranis 27:2abcf13d90a3 468 // /*----- Increasing the speed while running. -----*/
tom_astranis 27:2abcf13d90a3 469 //
tom_astranis 27:2abcf13d90a3 470 // /* Preparing each motor to perform a run at a specified speed. */
tom_astranis 27:2abcf13d90a3 471 // for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
tom_astranis 27:2abcf13d90a3 472 // motors[m]->prepare_get_speed();
tom_astranis 27:2abcf13d90a3 473 // }
tom_astranis 27:2abcf13d90a3 474 //
tom_astranis 27:2abcf13d90a3 475 // /* Performing the action on each motor at the same time. */
tom_astranis 27:2abcf13d90a3 476 // uint32_t* results = x_nucleo_ihm02a1->perform_prepared_actions();
tom_astranis 27:2abcf13d90a3 477 //
tom_astranis 27:2abcf13d90a3 478 // /* Printing to the console. */
tom_astranis 27:2abcf13d90a3 479 // printf(" Speed: M1 %d, M2 %d.\r\n", results[0], results[1]);
tom_astranis 27:2abcf13d90a3 480 //
tom_astranis 27:2abcf13d90a3 481 // /* Printing to the console. */
tom_astranis 27:2abcf13d90a3 482 // printf("--> Doublig the speed while running again for %d seconds.\r\n", DELAY_3 / 1000);
tom_astranis 27:2abcf13d90a3 483 //
tom_astranis 27:2abcf13d90a3 484 // /* Preparing each motor to perform a run at a specified speed. */
tom_astranis 27:2abcf13d90a3 485 // for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
tom_astranis 27:2abcf13d90a3 486 // motors[m]->prepare_run(StepperMotor::BWD, results[m] << 1);
tom_astranis 27:2abcf13d90a3 487 // }
tom_astranis 27:2abcf13d90a3 488 //
tom_astranis 27:2abcf13d90a3 489 // /* Performing the action on each motor at the same time. */
tom_astranis 27:2abcf13d90a3 490 // results = x_nucleo_ihm02a1->perform_prepared_actions();
tom_astranis 27:2abcf13d90a3 491 //
tom_astranis 27:2abcf13d90a3 492 // /* Waiting. */
tom_astranis 27:2abcf13d90a3 493 // wait_ms(DELAY_3);
tom_astranis 27:2abcf13d90a3 494 //
tom_astranis 27:2abcf13d90a3 495 // /* Preparing each motor to perform a run at a specified speed. */
tom_astranis 27:2abcf13d90a3 496 // for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
tom_astranis 27:2abcf13d90a3 497 // motors[m]->prepare_get_speed();
tom_astranis 27:2abcf13d90a3 498 // }
tom_astranis 27:2abcf13d90a3 499 //
tom_astranis 27:2abcf13d90a3 500 // /* Performing the action on each motor at the same time. */
tom_astranis 27:2abcf13d90a3 501 // results = x_nucleo_ihm02a1->perform_prepared_actions();
tom_astranis 27:2abcf13d90a3 502 //
tom_astranis 27:2abcf13d90a3 503 // /* Printing to the console. */
tom_astranis 27:2abcf13d90a3 504 // printf(" Speed: M1 %d, M2 %d.\r\n", results[0], results[1]);
tom_astranis 27:2abcf13d90a3 505 //
tom_astranis 27:2abcf13d90a3 506 // /* Waiting. */
tom_astranis 27:2abcf13d90a3 507 // wait_ms(DELAY_1);
tom_astranis 27:2abcf13d90a3 508 //
tom_astranis 27:2abcf13d90a3 509 //
tom_astranis 27:2abcf13d90a3 510 // /*----- Doing a full revolution on each motor, one after the other. -----*/
tom_astranis 27:2abcf13d90a3 511 //
tom_astranis 27:2abcf13d90a3 512 // /* Printing to the console. */
tom_astranis 27:2abcf13d90a3 513 // printf("--> Doing a full revolution on each motor, one after the other.\r\n");
tom_astranis 27:2abcf13d90a3 514 //
tom_astranis 27:2abcf13d90a3 515 // /* Doing a full revolution on each motor, one after the other. */
tom_astranis 27:2abcf13d90a3 516 // for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
tom_astranis 27:2abcf13d90a3 517 // for (int i = 0; i < MPR_1; i++) {
tom_astranis 27:2abcf13d90a3 518 // /* Computing the number of steps. */
tom_astranis 27:2abcf13d90a3 519 // int steps = (int) (((int) init[m].fullstepsperrevolution * pow(2.0f, init[m].step_sel)) / MPR_1);
tom_astranis 27:2abcf13d90a3 520 //
tom_astranis 27:2abcf13d90a3 521 // /* Moving. */
tom_astranis 27:2abcf13d90a3 522 // motors[m]->move(StepperMotor::FWD, steps);
tom_astranis 27:2abcf13d90a3 523 //
tom_astranis 27:2abcf13d90a3 524 // /* Waiting while active. */
tom_astranis 27:2abcf13d90a3 525 // motors[m]->wait_while_active();
tom_astranis 27:2abcf13d90a3 526 //
tom_astranis 27:2abcf13d90a3 527 // /* Waiting. */
tom_astranis 27:2abcf13d90a3 528 // wait_ms(DELAY_1);
tom_astranis 27:2abcf13d90a3 529 // }
tom_astranis 27:2abcf13d90a3 530 // }
tom_astranis 27:2abcf13d90a3 531 //
tom_astranis 27:2abcf13d90a3 532 // /* Waiting. */
tom_astranis 27:2abcf13d90a3 533 // wait_ms(DELAY_2);
tom_astranis 27:2abcf13d90a3 534 //
tom_astranis 27:2abcf13d90a3 535 //}