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@41:7f91e949ca88, 2018-05-03 (annotated)

Committer:
leogrotti
Date:
Thu May 03 18:23:42 2018 +0000
Revision:
41:7f91e949ca88
Parent:
40:0b517b49f70d
Child:
42:a04bff02f231
3 motores + joystick+2botoes;

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). */
leogrotti 34:098efd69c86d 65 #define SPEED_1 1300
leogrotti 35:a05bef8dd995 66 #define SPEED_2 1300
Davidroid 24:8cb3c4ad055f 67
Davidroid 0:e6a49a092e2a 68
Brunoporto2702 32:481674c0f90d 69
Davidroid 0:e6a49a092e2a 70 /* Variables -----------------------------------------------------------------*/
leogrotti 41:7f91e949ca88 71 InterruptIn button1(PC_1);
leogrotti 41:7f91e949ca88 72 InterruptIn button2(PC_0);
leogrotti 40:0b517b49f70d 73
Brunoporto2702 32:481674c0f90d 74 /* Initialization parameters. */
leogrotti 39:7e30bcc989d3 75 //L6474_init_t init = {
leogrotti 39:7e30bcc989d3 76 // 160, /* Acceleration rate in pps^2. Range: (0..+inf). */
leogrotti 39:7e30bcc989d3 77 // 160, /* Deceleration rate in pps^2. Range: (0..+inf). */
leogrotti 39:7e30bcc989d3 78 // 4000, /* Maximum speed in pps. Range: (30..10000]. */
leogrotti 39:7e30bcc989d3 79 // 1300, /* Minimum speed in pps. Range: [30..10000). */
leogrotti 39:7e30bcc989d3 80 // 1500, /* Torque regulation current in mA. Range: 31.25mA to 4000mA. */
leogrotti 39:7e30bcc989d3 81 // L6474_OCD_TH_1875mA, /* Overcurrent threshold (OCD_TH register). */
leogrotti 39:7e30bcc989d3 82 // L6474_CONFIG_OC_SD_ENABLE, /* Overcurrent shutwdown (OC_SD field of CONFIG register). */
leogrotti 39:7e30bcc989d3 83 // L6474_CONFIG_EN_TQREG_TVAL_USED, /* Torque regulation method (EN_TQREG field of CONFIG register). */
leogrotti 39:7e30bcc989d3 84 // L6474_STEP_SEL_1_2, /* Step selection (STEP_SEL field of STEP_MODE register). */
leogrotti 39:7e30bcc989d3 85 // L6474_SYNC_SEL_1_2, /* Sync selection (SYNC_SEL field of STEP_MODE register). */
leogrotti 39:7e30bcc989d3 86 // L6474_FAST_STEP_12us, /* Fall time value (T_FAST field of T_FAST register). Range: 2us to 32us. */
leogrotti 39:7e30bcc989d3 87 // L6474_TOFF_FAST_8us, /* Maximum fast decay time (T_OFF field of T_FAST register). Range: 2us to 32us. */
leogrotti 39:7e30bcc989d3 88 // 3, /* Minimum ON time in us (TON_MIN register). Range: 0.5us to 64us. */
leogrotti 39:7e30bcc989d3 89 // 21, /* Minimum OFF time in us (TOFF_MIN register). Range: 0.5us to 64us. */
leogrotti 39:7e30bcc989d3 90 // L6474_CONFIG_TOFF_044us, /* Target Swicthing Period (field TOFF of CONFIG register). */
leogrotti 39:7e30bcc989d3 91 // L6474_CONFIG_SR_320V_us, /* Slew rate (POW_SR field of CONFIG register). */
leogrotti 39:7e30bcc989d3 92 // L6474_CONFIG_INT_16MHZ, /* Clock setting (OSC_CLK_SEL field of CONFIG register). */
leogrotti 39:7e30bcc989d3 93 // L6474_ALARM_EN_OVERCURRENT |
leogrotti 39:7e30bcc989d3 94 // L6474_ALARM_EN_THERMAL_SHUTDOWN |
leogrotti 39:7e30bcc989d3 95 // L6474_ALARM_EN_THERMAL_WARNING |
leogrotti 39:7e30bcc989d3 96 // L6474_ALARM_EN_UNDERVOLTAGE |
leogrotti 39:7e30bcc989d3 97 // L6474_ALARM_EN_SW_TURN_ON |
leogrotti 39:7e30bcc989d3 98 // L6474_ALARM_EN_WRONG_NPERF_CMD /* Alarm (ALARM_EN register). */
leogrotti 39:7e30bcc989d3 99 //};
Brunoporto2702 32:481674c0f90d 100
Brunoporto2702 32:481674c0f90d 101 Serial pc(USBTX, USBRX);
Brunoporto2702 32:481674c0f90d 102
Brunoporto2702 32:481674c0f90d 103 AnalogIn eixo_X(A0);
Brunoporto2702 32:481674c0f90d 104 AnalogIn eixo_Y(A1);
Brunoporto2702 32:481674c0f90d 105
Brunoporto2702 32:481674c0f90d 106
Brunoporto2702 32:481674c0f90d 107
Brunoporto2702 32:481674c0f90d 108 float x;
Brunoporto2702 32:481674c0f90d 109 float y;
Brunoporto2702 32:481674c0f90d 110 float w;
Brunoporto2702 32:481674c0f90d 111 float u;
Brunoporto2702 32:481674c0f90d 112
leogrotti 38:c3d77ff8168a 113 //float a;
leogrotti 36:0cd4fdbb40af 114
leogrotti 35:a05bef8dd995 115
leogrotti 35:a05bef8dd995 116
leogrotti 34:098efd69c86d 117 unsigned int minspeed = 1300;
Brunoporto2702 32:481674c0f90d 118
leogrotti 34:098efd69c86d 119 //int step = 0x08;
Brunoporto2702 32:481674c0f90d 120
leogrotti 35:a05bef8dd995 121 unsigned int speed1;
leogrotti 35:a05bef8dd995 122 unsigned int speed2;
leogrotti 39:7e30bcc989d3 123 unsigned int speed3;
leogrotti 36:0cd4fdbb40af 124
leogrotti 38:c3d77ff8168a 125 //unsigned int pos3;
Brunoporto2702 32:481674c0f90d 126
Davidroid 0:e6a49a092e2a 127
Davidroid 0:e6a49a092e2a 128 /* Motor Control Component. */
Davidroid 3:02d9ec4f88b2 129 L6474 *motor1;
Davidroid 3:02d9ec4f88b2 130 L6474 *motor2;
leogrotti 39:7e30bcc989d3 131 L6474 *motor3;
Davidroid 0:e6a49a092e2a 132
leogrotti 40:0b517b49f70d 133 void flag_irq_handler(void)
leogrotti 40:0b517b49f70d 134 {
leogrotti 40:0b517b49f70d 135 /* Set ISR flag. */
leogrotti 40:0b517b49f70d 136 motor3->isr_flag = TRUE;
leogrotti 40:0b517b49f70d 137
leogrotti 40:0b517b49f70d 138 /* Get the value of the status register. */
leogrotti 40:0b517b49f70d 139 unsigned int status = motor3->get_status();
leogrotti 40:0b517b49f70d 140
leogrotti 40:0b517b49f70d 141 /* Check NOTPERF_CMD flag: if set, the command received by SPI can't be performed. */
leogrotti 40:0b517b49f70d 142 /* This often occures when a command is sent to the L6474 while it is not in HiZ state. */
leogrotti 40:0b517b49f70d 143 if ((status & L6474_STATUS_NOTPERF_CMD) == L6474_STATUS_NOTPERF_CMD) {
leogrotti 40:0b517b49f70d 144 printf(" WARNING: \"FLAG\" interrupt triggered. Non-performable command detected when updating L6474's registers while not in HiZ state.\r\n");
leogrotti 40:0b517b49f70d 145 }
leogrotti 40:0b517b49f70d 146
leogrotti 40:0b517b49f70d 147 /* Reset ISR flag. */
leogrotti 40:0b517b49f70d 148 motor3->isr_flag = FALSE;
leogrotti 40:0b517b49f70d 149 }
leogrotti 40:0b517b49f70d 150
Davidroid 0:e6a49a092e2a 151
Davidroid 0:e6a49a092e2a 152 /* Main ----------------------------------------------------------------------*/
Davidroid 0:e6a49a092e2a 153
leogrotti 41:7f91e949ca88 154 void pressed_cima() {
leogrotti 41:7f91e949ca88 155
leogrotti 41:7f91e949ca88 156 motor3->run(StepperMotor::FWD);
leogrotti 41:7f91e949ca88 157 printf ("Ariba! \r\n");
leogrotti 41:7f91e949ca88 158
leogrotti 41:7f91e949ca88 159 }
leogrotti 41:7f91e949ca88 160 void pressed_baixo() {
leogrotti 41:7f91e949ca88 161
leogrotti 41:7f91e949ca88 162 motor3->run(StepperMotor::BWD);
leogrotti 41:7f91e949ca88 163 printf ("Abajo! \r\n");
leogrotti 41:7f91e949ca88 164
leogrotti 41:7f91e949ca88 165 }
leogrotti 41:7f91e949ca88 166 void released_1()
leogrotti 41:7f91e949ca88 167 {
leogrotti 41:7f91e949ca88 168 motor3->hard_stop();
leogrotti 41:7f91e949ca88 169 printf ("parou \r\n");
leogrotti 41:7f91e949ca88 170
leogrotti 41:7f91e949ca88 171 }
leogrotti 41:7f91e949ca88 172
leogrotti 41:7f91e949ca88 173 void released_2()
leogrotti 41:7f91e949ca88 174 {
leogrotti 41:7f91e949ca88 175 motor3->hard_stop();
leogrotti 41:7f91e949ca88 176 printf ("parou \r\n");
leogrotti 41:7f91e949ca88 177
leogrotti 41:7f91e949ca88 178 }
leogrotti 41:7f91e949ca88 179
Davidroid 0:e6a49a092e2a 180 int main()
Davidroid 0:e6a49a092e2a 181 {
Davidroid 8:cec4c2c03a27 182 /*----- Initialization. -----*/
Davidroid 8:cec4c2c03a27 183
Davidroid 0:e6a49a092e2a 184 /* Initializing SPI bus. */
Davidroid 0:e6a49a092e2a 185 DevSPI dev_spi(D11, D12, D13);
Davidroid 0:e6a49a092e2a 186
Davidroid 9:a9e51320aee4 187 /* Initializing Motor Control Components. */
Davidroid 5:a0268a435bb1 188 motor1 = new L6474(D2, D8, D7, D9, D10, dev_spi);
Davidroid 16:810667a9f31f 189 motor2 = new L6474(D2, D8, D4, D3, D10, dev_spi);
leogrotti 39:7e30bcc989d3 190 motor3 = new L6474(D2, D8, D5, D6, D10, dev_spi);
Davidroid 29:526970c1d998 191 if (motor1->init() != COMPONENT_OK) {
Davidroid 14:fcd452b03d1a 192 exit(EXIT_FAILURE);
Davidroid 28:3f17a4152bcf 193 }
Davidroid 29:526970c1d998 194 if (motor2->init() != COMPONENT_OK) {
Davidroid 14:fcd452b03d1a 195 exit(EXIT_FAILURE);
Davidroid 28:3f17a4152bcf 196 }
leogrotti 39:7e30bcc989d3 197 if (motor3->init() != COMPONENT_OK) {
leogrotti 39:7e30bcc989d3 198 exit(EXIT_FAILURE);
leogrotti 39:7e30bcc989d3 199 }
Davidroid 0:e6a49a092e2a 200
leogrotti 35:a05bef8dd995 201
Davidroid 8:cec4c2c03a27 202
Davidroid 8:cec4c2c03a27 203
leogrotti 39:7e30bcc989d3 204
leogrotti 40:0b517b49f70d 205 /* Attaching and enabling interrupt handlers. */
leogrotti 40:0b517b49f70d 206 motor3->attach_flag_irq(&flag_irq_handler);
leogrotti 40:0b517b49f70d 207 motor3->enable_flag_irq();
leogrotti 41:7f91e949ca88 208
leogrotti 39:7e30bcc989d3 209
leogrotti 41:7f91e949ca88 210
leogrotti 41:7f91e949ca88 211 button1.rise(&pressed_cima);
leogrotti 41:7f91e949ca88 212 button2.rise(&pressed_baixo);
leogrotti 41:7f91e949ca88 213 button1.fall(&released_1);
leogrotti 41:7f91e949ca88 214 button2.fall(&released_2);
leogrotti 39:7e30bcc989d3 215
Davidroid 28:3f17a4152bcf 216 while(true) {
Brunoporto2702 32:481674c0f90d 217 u = eixo_X.read();
Brunoporto2702 32:481674c0f90d 218 w = eixo_Y.read();
Brunoporto2702 32:481674c0f90d 219
leogrotti 41:7f91e949ca88 220 /* if (button1.read()==0()){
leogrotti 40:0b517b49f70d 221
leogrotti 41:7f91e949ca88 222 button.fall()
leogrotti 40:0b517b49f70d 223 printf("button1 fwd \r\n\n");
leogrotti 39:7e30bcc989d3 224
leogrotti 40:0b517b49f70d 225 }
leogrotti 40:0b517b49f70d 226
leogrotti 41:7f91e949ca88 227 // else {
leogrotti 41:7f91e949ca88 228 // motor3->run(StepperMotor::BWD);
leogrotti 41:7f91e949ca88 229 // printf("button1 bwd \r\n\n");
leogrotti 40:0b517b49f70d 230
leogrotti 41:7f91e949ca88 231 // }
leogrotti 41:7f91e949ca88 232
leogrotti 41:7f91e949ca88 233 if (button1.read()==0){
leogrotti 41:7f91e949ca88 234
leogrotti 41:7f91e949ca88 235 button1.rise()
leogrotti 41:7f91e949ca88 236 printf("button1 bwd \r\n\n");
leogrotti 41:7f91e949ca88 237
leogrotti 41:7f91e949ca88 238 }*/
leogrotti 35:a05bef8dd995 239
leogrotti 35:a05bef8dd995 240
leogrotti 34:098efd69c86d 241 if (u>0.740) {
leogrotti 34:098efd69c86d 242
Brunoporto2702 32:481674c0f90d 243 motor1->run(StepperMotor::FWD);
leogrotti 39:7e30bcc989d3 244 //wait_ms(DELAY_1);
leogrotti 39:7e30bcc989d3 245
Brunoporto2702 32:481674c0f90d 246 speed1 = motor1->get_speed();
leogrotti 34:098efd69c86d 247
leogrotti 38:c3d77ff8168a 248 printf("speed1 =%d \r\n\n", speed1);
leogrotti 38:c3d77ff8168a 249 printf("U =%f \r\n\n", u);
leogrotti 34:098efd69c86d 250
Brunoporto2702 32:481674c0f90d 251 }
leogrotti 34:098efd69c86d 252 else{ if(u<0.65) {
leogrotti 34:098efd69c86d 253
Brunoporto2702 32:481674c0f90d 254 motor1->run(StepperMotor::BWD);
leogrotti 39:7e30bcc989d3 255 // wait_ms(DELAY_1);
leogrotti 39:7e30bcc989d3 256
Brunoporto2702 32:481674c0f90d 257 speed1 = motor1->get_speed();
leogrotti 34:098efd69c86d 258
leogrotti 38:c3d77ff8168a 259 printf("speed1 =%d \r\n\n", speed1);
leogrotti 35:a05bef8dd995 260
leogrotti 38:c3d77ff8168a 261 printf("U =%f \r\n\n", u);
leogrotti 34:098efd69c86d 262
Brunoporto2702 32:481674c0f90d 263 }
Brunoporto2702 32:481674c0f90d 264 else
Brunoporto2702 32:481674c0f90d 265 motor1->hard_stop();
leogrotti 39:7e30bcc989d3 266 // wait_ms(DELAY_1);
leogrotti 39:7e30bcc989d3 267
leogrotti 35:a05bef8dd995 268 speed1 = 0;
leogrotti 38:c3d77ff8168a 269 printf("speed1 =%d \r\n\n", speed1);
leogrotti 34:098efd69c86d 270
leogrotti 38:c3d77ff8168a 271 printf("U =%f \r\n\n", u);
leogrotti 34:098efd69c86d 272
Brunoporto2702 32:481674c0f90d 273 }
Brunoporto2702 32:481674c0f90d 274
leogrotti 34:098efd69c86d 275 if (w>0.76) {
leogrotti 34:098efd69c86d 276
leogrotti 34:098efd69c86d 277 motor2->run(StepperMotor::BWD);
Brunoporto2702 32:481674c0f90d 278 speed2 = motor2->get_speed();
leogrotti 35:a05bef8dd995 279
leogrotti 38:c3d77ff8168a 280 printf("speed2 =%d \r\n\n", speed2);
leogrotti 38:c3d77ff8168a 281 printf("W =%f \r\n\n", w);
leogrotti 34:098efd69c86d 282
Brunoporto2702 32:481674c0f90d 283 }
leogrotti 34:098efd69c86d 284 else{ if(w<0.67) {
leogrotti 34:098efd69c86d 285
leogrotti 34:098efd69c86d 286
leogrotti 34:098efd69c86d 287 motor2->run(StepperMotor::FWD);
leogrotti 35:a05bef8dd995 288 speed2 = motor2->get_speed();
leogrotti 35:a05bef8dd995 289
leogrotti 38:c3d77ff8168a 290 printf("speed2 =%d \r\n\n", speed2);
leogrotti 38:c3d77ff8168a 291 printf("W =%f \r\n\n", w);
leogrotti 34:098efd69c86d 292
Brunoporto2702 32:481674c0f90d 293 }
Brunoporto2702 32:481674c0f90d 294 else
Brunoporto2702 32:481674c0f90d 295 motor2->hard_stop();
leogrotti 35:a05bef8dd995 296 speed2 = 0;
leogrotti 38:c3d77ff8168a 297 printf("speed2 =%d \r\n\n", speed2);
leogrotti 38:c3d77ff8168a 298 printf("W =%f \r\n\n", w);
leogrotti 34:098efd69c86d 299
Brunoporto2702 32:481674c0f90d 300 }
leogrotti 36:0cd4fdbb40af 301
leogrotti 36:0cd4fdbb40af 302
Davidroid 0:e6a49a092e2a 303 }
Brunoporto2702 32:481674c0f90d 304 }