50_135_507_38_Insper / Mbed OS Teste_2Motores_Joystick_copy

2 Motores + Joystick

Dependencies:   X_NUCLEO_IHM01A1 TextLCD

Fork of HelloWorld_IHM01A1_2Motors_mbedOS by ST

main.cpp@32:481674c0f90d, 2018-04-23 (annotated)

Committer:
Brunoporto2702
Date:
Mon Apr 23 10:55:24 2018 +0000
Revision:
32:481674c0f90d
Parent:
29:526970c1d998
Child:
33:b276c9c00088
2 Motores + joystick

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Davidroid 0:e6a49a092e2a 1 /**
Davidroid 0:e6a49a092e2a 2 ******************************************************************************
Davidroid 0:e6a49a092e2a 3 * @file main.cpp
Davidroid 17:aae1446c67f4 4 * @author Davide Aliprandi, STMicroelectronics
Davidroid 0:e6a49a092e2a 5 * @version V1.0.0
Davidroid 0:e6a49a092e2a 6 * @date October 14th, 2015
Davidroid 17:aae1446c67f4 7 * @brief mbed test application for the STMicroelectronics X-NUCLEO-IHM01A1
Davidroid 3:02d9ec4f88b2 8 * Motor Control Expansion Board: control of 2 motors.
Davidroid 0:e6a49a092e2a 9 ******************************************************************************
Davidroid 0:e6a49a092e2a 10 * @attention
Davidroid 0:e6a49a092e2a 11 *
Davidroid 0:e6a49a092e2a 12 * <h2><center>&copy; COPYRIGHT(c) 2015 STMicroelectronics</center></h2>
Davidroid 0:e6a49a092e2a 13 *
Davidroid 0:e6a49a092e2a 14 * Redistribution and use in source and binary forms, with or without modification,
Davidroid 0:e6a49a092e2a 15 * are permitted provided that the following conditions are met:
Davidroid 0:e6a49a092e2a 16 * 1. Redistributions of source code must retain the above copyright notice,
Davidroid 0:e6a49a092e2a 17 * this list of conditions and the following disclaimer.
Davidroid 0:e6a49a092e2a 18 * 2. Redistributions in binary form must reproduce the above copyright notice,
Davidroid 0:e6a49a092e2a 19 * this list of conditions and the following disclaimer in the documentation
Davidroid 0:e6a49a092e2a 20 * and/or other materials provided with the distribution.
Davidroid 0:e6a49a092e2a 21 * 3. Neither the name of STMicroelectronics nor the names of its contributors
Davidroid 0:e6a49a092e2a 22 * may be used to endorse or promote products derived from this software
Davidroid 0:e6a49a092e2a 23 * without specific prior written permission.
Davidroid 0:e6a49a092e2a 24 *
Davidroid 0:e6a49a092e2a 25 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
Davidroid 0:e6a49a092e2a 26 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
Davidroid 0:e6a49a092e2a 27 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
Davidroid 0:e6a49a092e2a 28 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
Davidroid 0:e6a49a092e2a 29 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
Davidroid 0:e6a49a092e2a 30 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
Davidroid 0:e6a49a092e2a 31 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
Davidroid 0:e6a49a092e2a 32 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
Davidroid 0:e6a49a092e2a 33 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
Davidroid 0:e6a49a092e2a 34 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
Davidroid 0:e6a49a092e2a 35 *
Davidroid 0:e6a49a092e2a 36 ******************************************************************************
Davidroid 0:e6a49a092e2a 37 */
Davidroid 0:e6a49a092e2a 38
Davidroid 0:e6a49a092e2a 39
Davidroid 0:e6a49a092e2a 40 /* Includes ------------------------------------------------------------------*/
Davidroid 0:e6a49a092e2a 41
Davidroid 0:e6a49a092e2a 42 /* mbed specific header files. */
Davidroid 0:e6a49a092e2a 43 #include "mbed.h"
Davidroid 0:e6a49a092e2a 44
Davidroid 0:e6a49a092e2a 45 /* Helper header files. */
Davidroid 0:e6a49a092e2a 46 #include "DevSPI.h"
Davidroid 0:e6a49a092e2a 47
Davidroid 0:e6a49a092e2a 48 /* Component specific header files. */
Davidroid 29:526970c1d998 49 #include "L6474.h"
Davidroid 0:e6a49a092e2a 50
Davidroid 0:e6a49a092e2a 51
Davidroid 0:e6a49a092e2a 52 /* Definitions ---------------------------------------------------------------*/
Davidroid 0:e6a49a092e2a 53
Davidroid 24:8cb3c4ad055f 54 /* Number of steps. */
Davidroid 9:a9e51320aee4 55 #define STEPS 3200
Davidroid 0:e6a49a092e2a 56
Davidroid 24:8cb3c4ad055f 57 /* Delay in milliseconds. */
Davidroid 24:8cb3c4ad055f 58 #define DELAY_1 2000
Davidroid 24:8cb3c4ad055f 59 #define DELAY_2 6000
Davidroid 24:8cb3c4ad055f 60 #define DELAY_3 8000
Davidroid 24:8cb3c4ad055f 61
Davidroid 24:8cb3c4ad055f 62 /* Speed in pps (Pulses Per Second).
Davidroid 24:8cb3c4ad055f 63 In Full Step mode: 1 pps = 1 step/s).
Davidroid 24:8cb3c4ad055f 64 In 1/N Step Mode: N pps = 1 step/s). */
Davidroid 24:8cb3c4ad055f 65 #define SPEED_1 2400
Davidroid 24:8cb3c4ad055f 66 #define SPEED_2 1200
Davidroid 24:8cb3c4ad055f 67
Davidroid 0:e6a49a092e2a 68
Brunoporto2702 32:481674c0f90d 69
Davidroid 0:e6a49a092e2a 70 /* Variables -----------------------------------------------------------------*/
Brunoporto2702 32:481674c0f90d 71 /* Initialization parameters. */
Brunoporto2702 32:481674c0f90d 72 L6474_init_t init = {
Brunoporto2702 32:481674c0f90d 73 160, /* Acceleration rate in pps^2. Range: (0..+inf). */
Brunoporto2702 32:481674c0f90d 74 160, /* Deceleration rate in pps^2. Range: (0..+inf). */
Brunoporto2702 32:481674c0f90d 75 2400, /* Maximum speed in pps. Range: (30..10000]. */
Brunoporto2702 32:481674c0f90d 76 30, /* Minimum speed in pps. Range: [30..10000). */
Brunoporto2702 32:481674c0f90d 77 250, /* Torque regulation current in mA. Range: 31.25mA to 4000mA. */
Brunoporto2702 32:481674c0f90d 78 L6474_OCD_TH_750mA, /* Overcurrent threshold (OCD_TH register). */
Brunoporto2702 32:481674c0f90d 79 L6474_CONFIG_OC_SD_ENABLE, /* Overcurrent shutwdown (OC_SD field of CONFIG register). */
Brunoporto2702 32:481674c0f90d 80 L6474_CONFIG_EN_TQREG_TVAL_USED, /* Torque regulation method (EN_TQREG field of CONFIG register). */
Brunoporto2702 32:481674c0f90d 81 L6474_STEP_SEL_1, /* Step selection (STEP_SEL field of STEP_MODE register). */
Brunoporto2702 32:481674c0f90d 82 L6474_SYNC_SEL_1, /* Sync selection (SYNC_SEL field of STEP_MODE register). */
Brunoporto2702 32:481674c0f90d 83 L6474_FAST_STEP_12us, /* Fall time value (T_FAST field of T_FAST register). Range: 2us to 32us. */
Brunoporto2702 32:481674c0f90d 84 L6474_TOFF_FAST_8us, /* Maximum fast decay time (T_OFF field of T_FAST register). Range: 2us to 32us. */
Brunoporto2702 32:481674c0f90d 85 3, /* Minimum ON time in us (TON_MIN register). Range: 0.5us to 64us. */
Brunoporto2702 32:481674c0f90d 86 21, /* Minimum OFF time in us (TOFF_MIN register). Range: 0.5us to 64us. */
Brunoporto2702 32:481674c0f90d 87 L6474_CONFIG_TOFF_044us, /* Target Swicthing Period (field TOFF of CONFIG register). */
Brunoporto2702 32:481674c0f90d 88 L6474_CONFIG_SR_320V_us, /* Slew rate (POW_SR field of CONFIG register). */
Brunoporto2702 32:481674c0f90d 89 L6474_CONFIG_INT_16MHZ, /* Clock setting (OSC_CLK_SEL field of CONFIG register). */
Brunoporto2702 32:481674c0f90d 90 L6474_ALARM_EN_OVERCURRENT |
Brunoporto2702 32:481674c0f90d 91 L6474_ALARM_EN_THERMAL_SHUTDOWN |
Brunoporto2702 32:481674c0f90d 92 L6474_ALARM_EN_THERMAL_WARNING |
Brunoporto2702 32:481674c0f90d 93 L6474_ALARM_EN_UNDERVOLTAGE |
Brunoporto2702 32:481674c0f90d 94 L6474_ALARM_EN_SW_TURN_ON |
Brunoporto2702 32:481674c0f90d 95 L6474_ALARM_EN_WRONG_NPERF_CMD /* Alarm (ALARM_EN register). */
Brunoporto2702 32:481674c0f90d 96 };
Brunoporto2702 32:481674c0f90d 97
Brunoporto2702 32:481674c0f90d 98 Serial pc(USBTX, USBRX);
Brunoporto2702 32:481674c0f90d 99
Brunoporto2702 32:481674c0f90d 100 AnalogIn eixo_X(A0);
Brunoporto2702 32:481674c0f90d 101 AnalogIn eixo_Y(A1);
Brunoporto2702 32:481674c0f90d 102
Brunoporto2702 32:481674c0f90d 103
Brunoporto2702 32:481674c0f90d 104
Brunoporto2702 32:481674c0f90d 105 float x;
Brunoporto2702 32:481674c0f90d 106 float y;
Brunoporto2702 32:481674c0f90d 107 float w;
Brunoporto2702 32:481674c0f90d 108 float u;
Brunoporto2702 32:481674c0f90d 109
Brunoporto2702 32:481674c0f90d 110 unsigned int minspeed = 1600;
Brunoporto2702 32:481674c0f90d 111
Brunoporto2702 32:481674c0f90d 112 int step = 0x08;
Brunoporto2702 32:481674c0f90d 113
Brunoporto2702 32:481674c0f90d 114 int speed1;
Brunoporto2702 32:481674c0f90d 115 int speed2;
Brunoporto2702 32:481674c0f90d 116
Davidroid 0:e6a49a092e2a 117
Davidroid 0:e6a49a092e2a 118 /* Motor Control Component. */
Davidroid 3:02d9ec4f88b2 119 L6474 *motor1;
Davidroid 3:02d9ec4f88b2 120 L6474 *motor2;
Davidroid 0:e6a49a092e2a 121
Davidroid 0:e6a49a092e2a 122
Davidroid 0:e6a49a092e2a 123 /* Main ----------------------------------------------------------------------*/
Davidroid 0:e6a49a092e2a 124
Davidroid 0:e6a49a092e2a 125 int main()
Davidroid 0:e6a49a092e2a 126 {
Davidroid 8:cec4c2c03a27 127 /*----- Initialization. -----*/
Davidroid 8:cec4c2c03a27 128
Davidroid 0:e6a49a092e2a 129 /* Initializing SPI bus. */
Davidroid 0:e6a49a092e2a 130 DevSPI dev_spi(D11, D12, D13);
Davidroid 0:e6a49a092e2a 131
Davidroid 9:a9e51320aee4 132 /* Initializing Motor Control Components. */
Davidroid 5:a0268a435bb1 133 motor1 = new L6474(D2, D8, D7, D9, D10, dev_spi);
Davidroid 16:810667a9f31f 134 motor2 = new L6474(D2, D8, D4, D3, D10, dev_spi);
Davidroid 29:526970c1d998 135 if (motor1->init() != COMPONENT_OK) {
Davidroid 14:fcd452b03d1a 136 exit(EXIT_FAILURE);
Davidroid 28:3f17a4152bcf 137 }
Davidroid 29:526970c1d998 138 if (motor2->init() != COMPONENT_OK) {
Davidroid 14:fcd452b03d1a 139 exit(EXIT_FAILURE);
Davidroid 28:3f17a4152bcf 140 }
Davidroid 0:e6a49a092e2a 141
Davidroid 0:e6a49a092e2a 142 /* Printing to the console. */
Davidroid 3:02d9ec4f88b2 143 printf("Motor Control Application Example for 2 Motors\r\n\n");
Davidroid 0:e6a49a092e2a 144
Davidroid 8:cec4c2c03a27 145
Davidroid 8:cec4c2c03a27 146 /*----- Moving. -----*/
Davidroid 8:cec4c2c03a27 147
Davidroid 8:cec4c2c03a27 148 /* Printing to the console. */
Davidroid 9:a9e51320aee4 149 printf("--> Moving forward: M1 %d steps, M2 %d steps.\r\n", STEPS >> 1, STEPS);
Davidroid 8:cec4c2c03a27 150
Davidroid 8:cec4c2c03a27 151 /* Moving N steps in the forward direction. */
Davidroid 29:526970c1d998 152 motor1->move(StepperMotor::FWD, STEPS >> 1);
Davidroid 29:526970c1d998 153 motor2->move(StepperMotor::FWD, STEPS);
Davidroid 8:cec4c2c03a27 154
Davidroid 8:cec4c2c03a27 155 /* Waiting while the motor is active. */
Davidroid 29:526970c1d998 156 motor1->wait_while_active();
Davidroid 29:526970c1d998 157 motor2->wait_while_active();
Davidroid 8:cec4c2c03a27 158
Davidroid 8:cec4c2c03a27 159 /* Getting current position. */
Davidroid 29:526970c1d998 160 int position1 = motor1->get_position();
Davidroid 29:526970c1d998 161 int position2 = motor2->get_position();
Davidroid 8:cec4c2c03a27 162
Davidroid 8:cec4c2c03a27 163 /* Printing to the console. */
Davidroid 8:cec4c2c03a27 164 printf(" Position: M1 %d, M2 %d.\r\n", position1, position2);
Davidroid 8:cec4c2c03a27 165
Davidroid 8:cec4c2c03a27 166 /* Waiting 2 seconds. */
Davidroid 24:8cb3c4ad055f 167 wait_ms(DELAY_1);
Davidroid 8:cec4c2c03a27 168
Davidroid 8:cec4c2c03a27 169
Davidroid 8:cec4c2c03a27 170 /*----- Moving. -----*/
Davidroid 8:cec4c2c03a27 171
Davidroid 8:cec4c2c03a27 172
Davidroid 28:3f17a4152bcf 173 while(true) {
Brunoporto2702 32:481674c0f90d 174 u = eixo_X.read();
Brunoporto2702 32:481674c0f90d 175 w = eixo_Y.read();
Brunoporto2702 32:481674c0f90d 176
Brunoporto2702 32:481674c0f90d 177 // wait (1.5);
Brunoporto2702 32:481674c0f90d 178 // pc.printf ("X: %f \r\n",u);
Brunoporto2702 32:481674c0f90d 179 // pc.printf ("Y: %f \r\n",w);
Brunoporto2702 32:481674c0f90d 180
Brunoporto2702 32:481674c0f90d 181 if (u>0.520) {
Brunoporto2702 32:481674c0f90d 182 motor1->set_min_speed(minspeed);
Brunoporto2702 32:481674c0f90d 183 motor1->set_step_mode(step_mode_t_1);
Brunoporto2702 32:481674c0f90d 184 motor1->run(StepperMotor::FWD);
Brunoporto2702 32:481674c0f90d 185 speed1 = motor1->get_speed();
Brunoporto2702 32:481674c0f90d 186 printf("velocidade: motor 1: %d \r\n",speed1);
Brunoporto2702 32:481674c0f90d 187 printf("movendo motor 1 pra frente\r\n");
Brunoporto2702 32:481674c0f90d 188 }
Brunoporto2702 32:481674c0f90d 189 else{ if(u<0.455) {
Brunoporto2702 32:481674c0f90d 190 motor1->set_min_speed(minspeed);
Brunoporto2702 32:481674c0f90d 191 motor1->run(StepperMotor::BWD);
Brunoporto2702 32:481674c0f90d 192 speed1 = motor1->get_speed();
Brunoporto2702 32:481674c0f90d 193 printf("velocidade: motor 1: %d \r\n",speed1);
Brunoporto2702 32:481674c0f90d 194 printf("movendo motor 1 pra tras\r\n");
Brunoporto2702 32:481674c0f90d 195 }
Brunoporto2702 32:481674c0f90d 196 else
Brunoporto2702 32:481674c0f90d 197 motor1->hard_stop();
Brunoporto2702 32:481674c0f90d 198 printf("parado1\r\n");
Brunoporto2702 32:481674c0f90d 199 printf("velocidade: motor 1: %d \r\n",speed1);
Brunoporto2702 32:481674c0f90d 200 }
Brunoporto2702 32:481674c0f90d 201
Brunoporto2702 32:481674c0f90d 202 if (w>0.535) {
Brunoporto2702 32:481674c0f90d 203 motor2->set_min_speed(minspeed);
Brunoporto2702 32:481674c0f90d 204 motor2->run(StepperMotor::FWD);
Brunoporto2702 32:481674c0f90d 205 speed2 = motor2->get_speed();
Brunoporto2702 32:481674c0f90d 206 printf("velocidade: motor 1: %d \r\n",speed2);
Brunoporto2702 32:481674c0f90d 207 printf("movendo motor 2 pra frente\r\n");
Brunoporto2702 32:481674c0f90d 208 }
Brunoporto2702 32:481674c0f90d 209 else{ if(w<0.491) {
Brunoporto2702 32:481674c0f90d 210 motor1->set_min_speed(minspeed);
Brunoporto2702 32:481674c0f90d 211 motor2->run(StepperMotor::BWD);
Brunoporto2702 32:481674c0f90d 212 speed2 = motor1->get_speed();
Brunoporto2702 32:481674c0f90d 213 printf("velocidade: motor 1: %d \r\n",speed2);
Brunoporto2702 32:481674c0f90d 214 printf("movendo motor 2 pra tras\r\n");
Brunoporto2702 32:481674c0f90d 215 }
Brunoporto2702 32:481674c0f90d 216 else
Brunoporto2702 32:481674c0f90d 217 motor2->hard_stop();
Brunoporto2702 32:481674c0f90d 218 printf("parado2\r\n");
Brunoporto2702 32:481674c0f90d 219 printf("velocidade: motor 2: %d \r\n",speed2);
Brunoporto2702 32:481674c0f90d 220 }
Davidroid 0:e6a49a092e2a 221 }
Brunoporto2702 32:481674c0f90d 222 }