CARLO TARALLO / Mbed 2 deprecated RoboCane_Motore

Robo l minuscolo

Dependencies:   X_NUCLEO_IHM04A1 mbed

Fork of HelloWorld_IHM04A1 by ST

main.cpp@6:d5d776281222, 2018-02-10 (annotated)

Committer:
carlotara
Date:
Sat Feb 10 18:25:08 2018 +0000
Revision:
6:d5d776281222
Parent:
5:4c1e581bbb8b 
RoboCaMot

Who changed what in which revision?

UserRevisionLine numberNew contents of line
brdirais 0:dcc35ef6effc 1 /**
brdirais 0:dcc35ef6effc 2 ******************************************************************************
brdirais 0:dcc35ef6effc 3 * @file Multi/Examples/MotionControl/IHM04A1_ExampleFor4UniDirMotors/Src/main.c
brdirais 0:dcc35ef6effc 4 * @author IPC Rennes
brdirais 0:dcc35ef6effc 5 * @version V1.0.0
brdirais 1:4d9b9123d9e1 6 * @date May 16, 2016
brdirais 0:dcc35ef6effc 7 * @brief This example shows how to use 1 IHM04A1 expansion board with
brdirais 0:dcc35ef6effc 8 * 4 unidirectionnal Brush DC motors.
brdirais 0:dcc35ef6effc 9 * Each motor has one lead connected to one of the bridge output,
brdirais 0:dcc35ef6effc 10 * the other lead to the ground. The input bridges are not parallelised.
brdirais 0:dcc35ef6effc 11 * The demo sequence starts when the user button is pressed.
brdirais 1:4d9b9123d9e1 12 * Each time, the user button is pressed, one new motor is activated
brdirais 0:dcc35ef6effc 13 ******************************************************************************
brdirais 0:dcc35ef6effc 14 * @attention
brdirais 0:dcc35ef6effc 15 *
brdirais 0:dcc35ef6effc 16 * <h2><center>&copy; COPYRIGHT(c) 2015 STMicroelectronics</center></h2>
brdirais 0:dcc35ef6effc 17 *
brdirais 0:dcc35ef6effc 18 * Redistribution and use in source and binary forms, with or without modification,
brdirais 0:dcc35ef6effc 19 * are permitted provided that the following conditions are met:
brdirais 0:dcc35ef6effc 20 * 1. Redistributions of source code must retain the above copyright notice,
brdirais 0:dcc35ef6effc 21 * this list of conditions and the following disclaimer.
brdirais 0:dcc35ef6effc 22 * 2. Redistributions in binary form must reproduce the above copyright notice,
brdirais 0:dcc35ef6effc 23 * this list of conditions and the following disclaimer in the documentation
brdirais 0:dcc35ef6effc 24 * and/or other materials provided with the distribution.
brdirais 0:dcc35ef6effc 25 * 3. Neither the name of STMicroelectronics nor the names of its contributors
brdirais 0:dcc35ef6effc 26 * may be used to endorse or promote products derived from this software
brdirais 0:dcc35ef6effc 27 * without specific prior written permission.
brdirais 0:dcc35ef6effc 28 *
brdirais 0:dcc35ef6effc 29 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
brdirais 0:dcc35ef6effc 30 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
brdirais 0:dcc35ef6effc 31 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
brdirais 0:dcc35ef6effc 32 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
brdirais 0:dcc35ef6effc 33 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
brdirais 0:dcc35ef6effc 34 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
brdirais 0:dcc35ef6effc 35 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
brdirais 0:dcc35ef6effc 36 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
brdirais 0:dcc35ef6effc 37 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
brdirais 0:dcc35ef6effc 38 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
brdirais 0:dcc35ef6effc 39 *
brdirais 0:dcc35ef6effc 40 ******************************************************************************
brdirais 0:dcc35ef6effc 41 */
brdirais 0:dcc35ef6effc 42
brdirais 0:dcc35ef6effc 43 /* Includes ------------------------------------------------------------------*/
brdirais 0:dcc35ef6effc 44
brdirais 0:dcc35ef6effc 45 /* mbed specific header files. */
brdirais 0:dcc35ef6effc 46 #include "mbed.h"
brdirais 0:dcc35ef6effc 47
brdirais 0:dcc35ef6effc 48 /* Component specific header files. */
brdirais 0:dcc35ef6effc 49 #include "l6206_class.h"
brdirais 0:dcc35ef6effc 50
brdirais 0:dcc35ef6effc 51
brdirais 0:dcc35ef6effc 52
brdirais 0:dcc35ef6effc 53 /* Definitions ---------------------------------------------------------------*/
brdirais 0:dcc35ef6effc 54
brdirais 0:dcc35ef6effc 55
brdirais 0:dcc35ef6effc 56 #define MAX_MOTOR (4)
brdirais 0:dcc35ef6effc 57
brdirais 0:dcc35ef6effc 58
brdirais 0:dcc35ef6effc 59
brdirais 0:dcc35ef6effc 60 /* Variables -----------------------------------------------------------------*/
brdirais 0:dcc35ef6effc 61
brdirais 0:dcc35ef6effc 62 static volatile uint16_t gLastError;
brdirais 0:dcc35ef6effc 63 static volatile uint8_t gStep = 0;
brdirais 0:dcc35ef6effc 64
brdirais 0:dcc35ef6effc 65
brdirais 0:dcc35ef6effc 66
brdirais 0:dcc35ef6effc 67 /* Variables -----------------------------------------------------------------*/
brdirais 0:dcc35ef6effc 68
brdirais 0:dcc35ef6effc 69 /* Initialization parameters. */
brdirais 0:dcc35ef6effc 70 L6206_Init_t init =
brdirais 0:dcc35ef6effc 71 {
brdirais 0:dcc35ef6effc 72 L6206_CONF_PARAM_PARALLE_BRIDGES,
brdirais 0:dcc35ef6effc 73 {L6206_CONF_PARAM_FREQ_PWM1A, L6206_CONF_PARAM_FREQ_PWM2A, L6206_CONF_PARAM_FREQ_PWM1B, L6206_CONF_PARAM_FREQ_PWM2B},
brdirais 0:dcc35ef6effc 74 {100,100,100,100},
brdirais 0:dcc35ef6effc 75 {FORWARD,FORWARD,BACKWARD,FORWARD},
brdirais 0:dcc35ef6effc 76 {INACTIVE,INACTIVE,INACTIVE,INACTIVE},
brdirais 0:dcc35ef6effc 77 {FALSE,FALSE}
brdirais 0:dcc35ef6effc 78 };
brdirais 0:dcc35ef6effc 79
brdirais 0:dcc35ef6effc 80 /* Motor Control Component. */
brdirais 0:dcc35ef6effc 81 L6206 *motor;
brdirais 0:dcc35ef6effc 82
brdirais 0:dcc35ef6effc 83 /* User button on Nucleo board */
carlotara 6:d5d776281222 84 //InterruptIn mybutton_irq(USER_BUTTON);
brdirais 0:dcc35ef6effc 85
brdirais 0:dcc35ef6effc 86
brdirais 0:dcc35ef6effc 87
brdirais 0:dcc35ef6effc 88 /* Functions -----------------------------------------------------------------*/
brdirais 0:dcc35ef6effc 89
brdirais 0:dcc35ef6effc 90 /**
brdirais 0:dcc35ef6effc 91 * @brief This function is executed in case of error occurrence.
brdirais 0:dcc35ef6effc 92 * @param error number of the error
brdirais 0:dcc35ef6effc 93 * @retval None
brdirais 0:dcc35ef6effc 94 */
brdirais 0:dcc35ef6effc 95 void Error_Handler(uint16_t error)
brdirais 0:dcc35ef6effc 96 {
brdirais 0:dcc35ef6effc 97 /* Backup error number */
brdirais 0:dcc35ef6effc 98 gLastError = error;
brdirais 0:dcc35ef6effc 99
brdirais 0:dcc35ef6effc 100 /* Enter your own code here */
brdirais 0:dcc35ef6effc 101 }
brdirais 0:dcc35ef6effc 102
brdirais 0:dcc35ef6effc 103 /**
brdirais 0:dcc35ef6effc 104 * @brief This function is the User handler for the flag interrupt
brdirais 0:dcc35ef6effc 105 * @param None
brdirais 0:dcc35ef6effc 106 * @retval None
brdirais 0:dcc35ef6effc 107 * @note If needed, implement it, and then attach and enable it:
brdirais 0:dcc35ef6effc 108 * + motor->AttachFlagInterrupt(MyFlagInterruptHandler);
brdirais 0:dcc35ef6effc 109 */
brdirais 0:dcc35ef6effc 110 void MyFlagInterruptHandler(void)
brdirais 0:dcc35ef6effc 111 {
brdirais 0:dcc35ef6effc 112 /* Code to be customised */
brdirais 0:dcc35ef6effc 113 /************************/
brdirais 0:dcc35ef6effc 114 /* Get the state of bridge A */
brdirais 5:4c1e581bbb8b 115 uint16_t bridgeState = motor->GetBridgeStatus(0);
brdirais 0:dcc35ef6effc 116
brdirais 0:dcc35ef6effc 117 if (bridgeState == 0)
brdirais 0:dcc35ef6effc 118 {
brdirais 0:dcc35ef6effc 119 if ((motor->GetDeviceState(0) != INACTIVE)||
brdirais 0:dcc35ef6effc 120 (motor->GetDeviceState(1) != INACTIVE))
brdirais 0:dcc35ef6effc 121 {
brdirais 0:dcc35ef6effc 122 /* Bridge A was disabling due to overcurrent or over temperature */
brdirais 0:dcc35ef6effc 123 /* When at least on of its motor was running */
brdirais 0:dcc35ef6effc 124 Error_Handler(0XBAD0);
brdirais 0:dcc35ef6effc 125 }
brdirais 0:dcc35ef6effc 126 }
brdirais 0:dcc35ef6effc 127
brdirais 0:dcc35ef6effc 128 /* Get the state of bridge B */
brdirais 5:4c1e581bbb8b 129 bridgeState = motor->GetBridgeStatus(1);
brdirais 0:dcc35ef6effc 130
brdirais 0:dcc35ef6effc 131 if (bridgeState == 0)
brdirais 0:dcc35ef6effc 132 {
brdirais 0:dcc35ef6effc 133 if ((motor->GetDeviceState(2) != INACTIVE)||
brdirais 0:dcc35ef6effc 134 (motor->GetDeviceState(3) != INACTIVE))
brdirais 0:dcc35ef6effc 135 {
brdirais 0:dcc35ef6effc 136 /* Bridge A was disabling due to overcurrent or over temperature */
brdirais 0:dcc35ef6effc 137 /* When at least on of its motor was running */
brdirais 0:dcc35ef6effc 138 Error_Handler(0XBAD1);
brdirais 0:dcc35ef6effc 139 }
brdirais 0:dcc35ef6effc 140 }
brdirais 0:dcc35ef6effc 141 }
brdirais 0:dcc35ef6effc 142
brdirais 0:dcc35ef6effc 143
brdirais 0:dcc35ef6effc 144 /* Private functions ---------------------------------------------------------*/
brdirais 0:dcc35ef6effc 145
brdirais 0:dcc35ef6effc 146 /**
brdirais 0:dcc35ef6effc 147 * @brief Button Irq
brdirais 0:dcc35ef6effc 148 * @param None
brdirais 0:dcc35ef6effc 149 * @retval None
brdirais 0:dcc35ef6effc 150 */
brdirais 0:dcc35ef6effc 151
carlotara 6:d5d776281222 152 /*void button_pressed(void)
brdirais 0:dcc35ef6effc 153 {
brdirais 0:dcc35ef6effc 154 mybutton_irq.disable_irq();
brdirais 0:dcc35ef6effc 155 gStep++;
brdirais 0:dcc35ef6effc 156 if (gStep > MAX_MOTOR)
brdirais 0:dcc35ef6effc 157 {
brdirais 0:dcc35ef6effc 158 gStep = 0;
brdirais 0:dcc35ef6effc 159 }
brdirais 0:dcc35ef6effc 160 wait_ms(200);
brdirais 0:dcc35ef6effc 161 mybutton_irq.enable_irq();
brdirais 0:dcc35ef6effc 162
brdirais 0:dcc35ef6effc 163 }
carlotara 6:d5d776281222 164 */
brdirais 0:dcc35ef6effc 165
brdirais 0:dcc35ef6effc 166 /**
brdirais 0:dcc35ef6effc 167 * @brief Main program
brdirais 0:dcc35ef6effc 168 * @param None
brdirais 0:dcc35ef6effc 169 * @retval None
brdirais 0:dcc35ef6effc 170 */
brdirais 0:dcc35ef6effc 171 int main(void)
brdirais 0:dcc35ef6effc 172 {
carlotara 6:d5d776281222 173
carlotara 6:d5d776281222 174 printf("ESTRONCIO\n");
carlotara 6:d5d776281222 175 /*Con il codice
carlotara 6:d5d776281222 176
carlotara 6:d5d776281222 177 motor->run(0, BDCMotor::FWD); // Forward
carlotara 6:d5d776281222 178
carlotara 6:d5d776281222 179 fai andare avanti.
carlotara 6:d5d776281222 180
carlotara 6:d5d776281222 181
carlotara 6:d5d776281222 182
carlotara 6:d5d776281222 183 Se vuoi cambiare il verso devi usare questo codice
carlotara 6:d5d776281222 184
carlotara 6:d5d776281222 185 motor->run(0, BDCMotor::BWD); // Backword
carlotara 6:d5d776281222 186
carlotara 6:d5d776281222 187
carlotara 6:d5d776281222 188
carlotara 6:d5d776281222 189 Lo 0 indica il motore 0, per il secondo motore puoi usare 1 collegandolo opportunamente.
carlotara 6:d5d776281222 190
carlotara 6:d5d776281222 191
carlotara 6:d5d776281222 192
carlotara 6:d5d776281222 193 Quindi ad esempio se devi fare andare tutti e due avanti, usi questo codice
carlotara 6:d5d776281222 194
carlotara 6:d5d776281222 195 motor->run(0, BDCMotor::FWD); // Forward
carlotara 6:d5d776281222 196
carlotara 6:d5d776281222 197 motor->run(1, BDCMotor::FWD); // Forward
carlotara 6:d5d776281222 198
carlotara 6:d5d776281222 199
carlotara 6:d5d776281222 200
carlotara 6:d5d776281222 201 Per farli andare indietro
carlotara 6:d5d776281222 202
carlotara 6:d5d776281222 203 motor->run(0, BDCMotor::BWD); // Forward
carlotara 6:d5d776281222 204
carlotara 6:d5d776281222 205 motor->run(1, BDCMotor::BWD); // Forward
carlotara 6:d5d776281222 206 */
brdirais 0:dcc35ef6effc 207 /*----- Initialization. -----*/
brdirais 0:dcc35ef6effc 208
brdirais 0:dcc35ef6effc 209 /* Initializing Motor Control Component. */
brdirais 0:dcc35ef6effc 210 motor = new L6206( D2, A4, D5, D4, A0, A1);
brdirais 0:dcc35ef6effc 211
brdirais 0:dcc35ef6effc 212 /* When Init method is called with NULL pointer, the L6206 parameters are set */
brdirais 0:dcc35ef6effc 213 /* with the predefined values from file l6206_target_config.h, otherwise the */
brdirais 0:dcc35ef6effc 214 /* parameters are set using the initDeviceParameters structure values. */
carlotara 6:d5d776281222 215 // if (motor->Init(&init) != COMPONENT_OK)
carlotara 6:d5d776281222 216 // exit(EXIT_FAILURE);
brdirais 0:dcc35ef6effc 217
brdirais 0:dcc35ef6effc 218 /* Attach the function MyFlagInterruptHandler (defined below) to the flag interrupt */
carlotara 6:d5d776281222 219 // motor->AttachFlagInterrupt(MyFlagInterruptHandler);
brdirais 0:dcc35ef6effc 220
brdirais 0:dcc35ef6effc 221 /* Attach the function Error_Handler (defined below) to the error Handler*/
carlotara 6:d5d776281222 222 // motor->AttachErrorHandler(Error_Handler);
brdirais 0:dcc35ef6effc 223
brdirais 0:dcc35ef6effc 224 /* Printing to the console. */
brdirais 1:4d9b9123d9e1 225 printf("Motor Control Application Example for 4 Motor\r\n\n");
brdirais 0:dcc35ef6effc 226
brdirais 0:dcc35ef6effc 227
brdirais 0:dcc35ef6effc 228 /* Select the configuration with no bridge paralleling, two unidirectionnal motors on bridge A
brdirais 0:dcc35ef6effc 229 and two unidirectionnal motors on bridge B */
carlotara 6:d5d776281222 230
carlotara 6:d5d776281222 231 // motor->SetDualFullBridgeConfig(PARALLELING_NONE___2_UNDIR_MOTOR_BRIDGE_A__2_UNDIR_MOTOR_BRIDGE_B);
carlotara 6:d5d776281222 232 motor->SetDualFullBridgeConfig(PARALLELING_NONE___1_BIDIR_MOTOR_BRIDGE_A__1_BIDIR_MOTOR_BRIDGE_B);
brdirais 0:dcc35ef6effc 233
brdirais 1:4d9b9123d9e1 234 /* Set PWM Frequency of bridge A inputs to 1000 Hz */
carlotara 6:d5d776281222 235 motor->SetBridgeInputPwmFreq(0,2000);
brdirais 0:dcc35ef6effc 236
brdirais 1:4d9b9123d9e1 237 /* Set PWM Frequency of bridge B inputs to 2000 Hz */
brdirais 0:dcc35ef6effc 238 motor->SetBridgeInputPwmFreq(1,2000);
brdirais 0:dcc35ef6effc 239
carlotara 6:d5d776281222 240 // // Attach button_pressed function to Irq
carlotara 6:d5d776281222 241 // mybutton_irq.fall(&button_pressed);
brdirais 0:dcc35ef6effc 242
brdirais 0:dcc35ef6effc 243 /* Infinite loop */
brdirais 0:dcc35ef6effc 244 while(1)
brdirais 0:dcc35ef6effc 245 {
brdirais 0:dcc35ef6effc 246
carlotara 6:d5d776281222 247 // if (gStep > 0)
carlotara 6:d5d776281222 248 // {
carlotara 6:d5d776281222 249 printf("ESTRONCIOLETTO\n");
carlotara 6:d5d776281222 250 // printf("Run motor 0 at 20%% of the maximum speed\n");
carlotara 6:d5d776281222 251 // /* Set speed of motor 0 to 20% */
carlotara 6:d5d776281222 252 // motor->SetSpeed(0,40);
brdirais 0:dcc35ef6effc 253 /* start motor 0 */
carlotara 6:d5d776281222 254 // motor->Run(0, BDCMotor::FWD);
carlotara 6:d5d776281222 255 // }
brdirais 0:dcc35ef6effc 256
carlotara 6:d5d776281222 257 // if (gStep > 1)
carlotara 6:d5d776281222 258 // {
carlotara 6:d5d776281222 259 printf("Run motor 1 e 2 at 50% avanti\n");
brdirais 0:dcc35ef6effc 260 /* Set speed of motor 1 to 30 % */
carlotara 6:d5d776281222 261 motor->SetSpeed(0,100);
carlotara 6:d5d776281222 262 motor->SetSpeed(1,100);
brdirais 0:dcc35ef6effc 263 /* start motor 1 */
brdirais 5:4c1e581bbb8b 264 motor->Run(1, BDCMotor::FWD);
carlotara 6:d5d776281222 265 motor->Run(0, BDCMotor::FWD); // Avanti
carlotara 6:d5d776281222 266 printf("Motori 1 e 2 FERMI\n");
carlotara 6:d5d776281222 267 wait_ms(5000);
carlotara 6:d5d776281222 268 motor->HardHiZ(0);
carlotara 6:d5d776281222 269 motor->HardHiZ(1);
carlotara 6:d5d776281222 270 // printf("Motori 1 e 2 FERMI\n");
carlotara 6:d5d776281222 271 wait_ms(1000);
carlotara 6:d5d776281222 272 printf("Run motor 1 e 2 at 50% indietro\n");
carlotara 6:d5d776281222 273 motor->Run(1, BDCMotor::BWD);
carlotara 6:d5d776281222 274 motor->Run(0, BDCMotor::BWD); // Indietro
carlotara 6:d5d776281222 275 wait_ms(5000);
carlotara 6:d5d776281222 276 motor->HardHiZ(0);
carlotara 6:d5d776281222 277 motor->HardHiZ(1);
carlotara 6:d5d776281222 278 printf("Motori 1 e 2 FERMI\n");
carlotara 6:d5d776281222 279 wait_ms(1000);
carlotara 6:d5d776281222 280 // }
brdirais 0:dcc35ef6effc 281
carlotara 6:d5d776281222 282 // if (gStep > 2)
carlotara 6:d5d776281222 283 // {
carlotara 6:d5d776281222 284 // printf("Run motor 2 at 40%% of the maximum speed\n");
carlotara 6:d5d776281222 285 // /* Set speed of motor 2 to 40 % */
carlotara 6:d5d776281222 286 // motor->SetSpeed(2,100);
carlotara 6:d5d776281222 287 // /* start motor 2 */
carlotara 6:d5d776281222 288 // motor->Run(2, BDCMotor::FWD);
carlotara 6:d5d776281222 289 // }
brdirais 0:dcc35ef6effc 290
carlotara 6:d5d776281222 291 // if (gStep > 3)
carlotara 6:d5d776281222 292 // {
carlotara 6:d5d776281222 293 // printf("Run motor 3 at 50%% of the maximum speed\n");
carlotara 6:d5d776281222 294 // /* Set speed of motor 3 to 50 % */
carlotara 6:d5d776281222 295 // motor->SetSpeed(3,100);
carlotara 6:d5d776281222 296 // /* start motor 3 */
carlotara 6:d5d776281222 297 // motor->Run(3, BDCMotor::FWD);
carlotara 6:d5d776281222 298 // }
carlotara 6:d5d776281222 299
carlotara 6:d5d776281222 300 // if (gStep > 0)
carlotara 6:d5d776281222 301 // {
carlotara 6:d5d776281222 302 // wait_ms(1000);
brdirais 0:dcc35ef6effc 303
carlotara 6:d5d776281222 304 // motor->HardHiZ(0);
carlotara 6:d5d776281222 305 // motor->HardHiZ(1);
carlotara 6:d5d776281222 306 // motor->HardHiZ(2);
carlotara 6:d5d776281222 307 // motor->HardHiZ(3);
brdirais 0:dcc35ef6effc 308
carlotara 6:d5d776281222 309 // wait_ms(1000);
carlotara 6:d5d776281222 310 // }
carlotara 6:d5d776281222 311
brdirais 0:dcc35ef6effc 312 }
brdirais 0:dcc35ef6effc 313 }
brdirais 0:dcc35ef6effc 314
brdirais 0:dcc35ef6effc 315
brdirais 0:dcc35ef6effc 316
brdirais 0:dcc35ef6effc 317 /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
brdirais 0:dcc35ef6effc 318