![](/media/cache/group/Diana_su_nero.jpg.50x50_q85.jpg)
__
Dependencies: X_NUCLEO_IHM04A1
main.cpp@25:84d2bd93bf97, 2019-06-12 (annotated)
- Committer:
- stebonicelli
- Date:
- Wed Jun 12 16:03:30 2019 +0000
- Revision:
- 25:84d2bd93bf97
- Parent:
- 24:37f139e067b2
fixed!!
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
stebonicelli | 17:dc1b04f0b55d | 1 | #include "mbed.h" |
danielfpq | 24:37f139e067b2 | 2 | #include "L6206.h" |
danielfpq | 24:37f139e067b2 | 3 | |
danielfpq | 24:37f139e067b2 | 4 | #define MAX_MOTOR (2) |
nucleosam | 0:36aa6787d4f9 | 5 | |
danielfpq | 24:37f139e067b2 | 6 | static volatile uint16_t gLastError; |
danielfpq | 24:37f139e067b2 | 7 | static volatile uint8_t gStep = 0; |
stebonicelli | 17:dc1b04f0b55d | 8 | |
danielfpq | 24:37f139e067b2 | 9 | int current_pose = 0; |
danielfpq | 24:37f139e067b2 | 10 | int speed = 0; |
danielfpq | 24:37f139e067b2 | 11 | |
danielfpq | 24:37f139e067b2 | 12 | L6206_init_t init = |
nucleosam | 0:36aa6787d4f9 | 13 | { |
danielfpq | 24:37f139e067b2 | 14 | L6206_CONF_PARAM_PARALLE_BRIDGES, |
danielfpq | 24:37f139e067b2 | 15 | {L6206_CONF_PARAM_FREQ_PWM1A, L6206_CONF_PARAM_FREQ_PWM2A, L6206_CONF_PARAM_FREQ_PWM1B, L6206_CONF_PARAM_FREQ_PWM2B}, |
danielfpq | 24:37f139e067b2 | 16 | {100,100,100,100}, |
danielfpq | 24:37f139e067b2 | 17 | {FORWARD,FORWARD,BACKWARD,FORWARD}, |
danielfpq | 24:37f139e067b2 | 18 | {INACTIVE,INACTIVE,INACTIVE,INACTIVE}, |
danielfpq | 24:37f139e067b2 | 19 | {FALSE,FALSE} |
nucleosam | 0:36aa6787d4f9 | 20 | }; |
stebonicelli | 21:533d014f09e0 | 21 | |
danielfpq | 24:37f139e067b2 | 22 | L6206 *motor; |
danielfpq | 24:37f139e067b2 | 23 | InterruptIn my_button_irq(USER_BUTTON); /* User button on Nucleo board */ |
gidiana | 18:65707db67191 | 24 | Thread canrxa; |
stebonicelli | 17:dc1b04f0b55d | 25 | |
danielfpq | 24:37f139e067b2 | 26 | //Utility |
stebonicelli | 17:dc1b04f0b55d | 27 | InterruptIn button(USER_BUTTON); |
danielfpq | 24:37f139e067b2 | 28 | DigitalOut led(LED1); //Change? |
stebonicelli | 17:dc1b04f0b55d | 29 | |
nucleosam | 0:36aa6787d4f9 | 30 | |
stebonicelli | 17:dc1b04f0b55d | 31 | void motor_error_handler(uint16_t error) |
stebonicelli | 17:dc1b04f0b55d | 32 | { |
stebonicelli | 17:dc1b04f0b55d | 33 | printf("ERROR: Motor Runtime\n\r"); |
danielfpq | 24:37f139e067b2 | 34 | while(1){}; |
stebonicelli | 17:dc1b04f0b55d | 35 | } |
stebonicelli | 17:dc1b04f0b55d | 36 | |
stebonicelli | 21:533d014f09e0 | 37 | void motor_zero() |
stebonicelli | 21:533d014f09e0 | 38 | { |
danielfpq | 24:37f139e067b2 | 39 | motor->run(0, BDCMotor::FWD); |
danielfpq | 24:37f139e067b2 | 40 | motor->run(1, BDCMotor::FWD); |
stebonicelli | 21:533d014f09e0 | 41 | } |
stebonicelli | 21:533d014f09e0 | 42 | |
danielfpq | 24:37f139e067b2 | 43 | void button_int_handler(unsigned int motorId) |
nucleosam | 0:36aa6787d4f9 | 44 | { |
danielfpq | 24:37f139e067b2 | 45 | printf("MOTOR SPEED: %d\n\r", motor->get_speed(motorId)); |
stebonicelli | 21:533d014f09e0 | 46 | motor_zero(); |
stebonicelli | 17:dc1b04f0b55d | 47 | } |
stebonicelli | 17:dc1b04f0b55d | 48 | |
danielfpq | 24:37f139e067b2 | 49 | void my_error_handler(uint16_t error) |
danielfpq | 24:37f139e067b2 | 50 | { |
danielfpq | 24:37f139e067b2 | 51 | /* Backup error number */ |
danielfpq | 24:37f139e067b2 | 52 | gLastError = error; |
danielfpq | 24:37f139e067b2 | 53 | |
danielfpq | 24:37f139e067b2 | 54 | /* Enter your own code here */ |
danielfpq | 24:37f139e067b2 | 55 | } |
danielfpq | 24:37f139e067b2 | 56 | |
danielfpq | 24:37f139e067b2 | 57 | void my_flag_irq_handler(void) |
stebonicelli | 17:dc1b04f0b55d | 58 | { |
danielfpq | 24:37f139e067b2 | 59 | /* Code to be customised */ |
danielfpq | 24:37f139e067b2 | 60 | /************************/ |
danielfpq | 24:37f139e067b2 | 61 | /* Get the state of bridge A */ |
danielfpq | 24:37f139e067b2 | 62 | uint16_t bridgeState = motor->get_bridge_status(0); |
danielfpq | 24:37f139e067b2 | 63 | |
danielfpq | 24:37f139e067b2 | 64 | if (bridgeState == 0) { |
danielfpq | 24:37f139e067b2 | 65 | if ((motor->get_device_state(0) != INACTIVE)|| |
danielfpq | 24:37f139e067b2 | 66 | (motor->get_device_state(1) != INACTIVE)) { |
danielfpq | 24:37f139e067b2 | 67 | /* Bridge A was disabling due to overcurrent or over temperature */ |
danielfpq | 24:37f139e067b2 | 68 | /* When at least on of its motor was running */ |
danielfpq | 24:37f139e067b2 | 69 | my_error_handler(0XBAD0); |
danielfpq | 24:37f139e067b2 | 70 | } |
danielfpq | 24:37f139e067b2 | 71 | } |
danielfpq | 24:37f139e067b2 | 72 | |
danielfpq | 24:37f139e067b2 | 73 | /* Get the state of bridge B */ |
danielfpq | 24:37f139e067b2 | 74 | bridgeState = motor->get_bridge_status(1); |
danielfpq | 24:37f139e067b2 | 75 | |
danielfpq | 24:37f139e067b2 | 76 | if (bridgeState == 0) { |
danielfpq | 24:37f139e067b2 | 77 | if ((motor->get_device_state(2) != INACTIVE)|| |
danielfpq | 24:37f139e067b2 | 78 | (motor->get_device_state(3) != INACTIVE)) { |
danielfpq | 24:37f139e067b2 | 79 | /* Bridge A was disabling due to overcurrent or over temperature */ |
danielfpq | 24:37f139e067b2 | 80 | /* When at least on of its motor was running */ |
danielfpq | 24:37f139e067b2 | 81 | my_error_handler(0XBAD1); |
danielfpq | 24:37f139e067b2 | 82 | } |
danielfpq | 24:37f139e067b2 | 83 | } |
danielfpq | 24:37f139e067b2 | 84 | } |
danielfpq | 24:37f139e067b2 | 85 | void end0_int_handler(unsigned int motorId) |
danielfpq | 24:37f139e067b2 | 86 | { |
danielfpq | 24:37f139e067b2 | 87 | printf("END0: Pressed\n\rSPEED: %d\n\r", motor->get_speed(motorId)); |
nucleosam | 0:36aa6787d4f9 | 88 | } |
nucleosam | 0:36aa6787d4f9 | 89 | |
stebonicelli | 17:dc1b04f0b55d | 90 | void end1_int_handler() |
stebonicelli | 17:dc1b04f0b55d | 91 | { |
danielfpq | 24:37f139e067b2 | 92 | motor->hard_stop(0); |
danielfpq | 24:37f139e067b2 | 93 | motor->hard_stop(1); //or hard_hiz(); for disabling the bridge? |
stebonicelli | 17:dc1b04f0b55d | 94 | |
danielfpq | 24:37f139e067b2 | 95 | motor->run(0, BDCMotor::BWD); |
danielfpq | 24:37f139e067b2 | 96 | motor->run(1, BDCMotor::BWD); |
stebonicelli | 17:dc1b04f0b55d | 97 | |
stebonicelli | 17:dc1b04f0b55d | 98 | printf("END1: Pressed\n\r"); |
stebonicelli | 17:dc1b04f0b55d | 99 | } |
stebonicelli | 17:dc1b04f0b55d | 100 | |
danielfpq | 24:37f139e067b2 | 101 | |
stebonicelli | 17:dc1b04f0b55d | 102 | |
danielfpq | 24:37f139e067b2 | 103 | // CAN, to revise |
gidiana | 23:3110010d98a0 | 104 | CAN can1(PB_12, PB_13); // RX, TX |
stebonicelli | 17:dc1b04f0b55d | 105 | |
stebonicelli | 17:dc1b04f0b55d | 106 | CANMessage messageIn; |
stebonicelli | 17:dc1b04f0b55d | 107 | CANMessage messageOut; |
stebonicelli | 17:dc1b04f0b55d | 108 | |
danielfpq | 24:37f139e067b2 | 109 | |
gidiana | 23:3110010d98a0 | 110 | int filter = can1.filter(0x010, 0x4FF, CANStandard); |
stebonicelli | 17:dc1b04f0b55d | 111 | |
gidiana | 18:65707db67191 | 112 | void canrx() |
stebonicelli | 17:dc1b04f0b55d | 113 | { |
stebonicelli | 19:9680ebe86f4a | 114 | while(1) |
stebonicelli | 21:533d014f09e0 | 115 | { |
danielfpq | 24:37f139e067b2 | 116 | if(can1.read(messageIn,filter)&& ((messageIn.id>>8 == 20) && (messageIn.id & 0x00FF==6))) //Primo motore |
stebonicelli | 19:9680ebe86f4a | 117 | { |
danielfpq | 24:37f139e067b2 | 118 | speed=messageIn.data[0]; //Messaggio da 0 a 255, devo sottrrarre 127 e imporre velocità con segno |
danielfpq | 24:37f139e067b2 | 119 | float speedMap=(speed-127)/127*100; |
stebonicelli | 25:84d2bd93bf97 | 120 | printf("CAN: mess %f\n\r", speedMap); |
stebonicelli | 21:533d014f09e0 | 121 | |
danielfpq | 24:37f139e067b2 | 122 | //CAN MESSAGE WITH SPEED TO REVISE |
danielfpq | 24:37f139e067b2 | 123 | //Ci sarebbe anche il set speed. |
danielfpq | 24:37f139e067b2 | 124 | if (speedMap == 0) |
stebonicelli | 21:533d014f09e0 | 125 | { |
danielfpq | 24:37f139e067b2 | 126 | motor->set_speed(0,0); //There's no soft stop. could it work like this? |
danielfpq | 24:37f139e067b2 | 127 | //current_speed= motor->get_speed(0); We could do lie this? |
danielfpq | 24:37f139e067b2 | 128 | //motor->go_to(current_pose); |
stebonicelli | 21:533d014f09e0 | 129 | } |
danielfpq | 24:37f139e067b2 | 130 | else if (speedMap>0) |
danielfpq | 24:37f139e067b2 | 131 | { |
danielfpq | 24:37f139e067b2 | 132 | motor->run(0,BDCMotor::FWD); |
danielfpq | 24:37f139e067b2 | 133 | motor->set_speed(0,(unsigned int) speedMap); |
danielfpq | 24:37f139e067b2 | 134 | } |
danielfpq | 24:37f139e067b2 | 135 | else if (speedMap<0) |
stebonicelli | 21:533d014f09e0 | 136 | { |
danielfpq | 24:37f139e067b2 | 137 | motor->run(0,BDCMotor::BWD); |
danielfpq | 24:37f139e067b2 | 138 | motor->set_speed(0,(unsigned int) -speedMap); |
stebonicelli | 21:533d014f09e0 | 139 | } |
stebonicelli | 21:533d014f09e0 | 140 | else |
stebonicelli | 21:533d014f09e0 | 141 | { |
danielfpq | 24:37f139e067b2 | 142 | motor->set_speed(0,0); //Riportare errore? NO |
stebonicelli | 21:533d014f09e0 | 143 | } |
danielfpq | 24:37f139e067b2 | 144 | } |
danielfpq | 24:37f139e067b2 | 145 | else if(can1.read(messageIn,filter)&& ((messageIn.id>>8 == 20) && (messageIn.id & 0x00FF==7))) //Secondo motore |
danielfpq | 24:37f139e067b2 | 146 | { |
danielfpq | 24:37f139e067b2 | 147 | speed=messageIn.data[0]; //Messaggio da 0 a 255, devo sottrrarre 127 e imporre velocità con segno |
danielfpq | 24:37f139e067b2 | 148 | float speedMap=(speed-127)/127*100; |
stebonicelli | 25:84d2bd93bf97 | 149 | printf("CAN: mess %f\n\r", speedMap); |
stebonicelli | 21:533d014f09e0 | 150 | |
danielfpq | 24:37f139e067b2 | 151 | //CAN MESSAGE WITH SPEED TO REVISE |
danielfpq | 24:37f139e067b2 | 152 | //Ci sarebbe anche il set speed. |
danielfpq | 24:37f139e067b2 | 153 | if (speedMap == 0) |
danielfpq | 24:37f139e067b2 | 154 | { |
danielfpq | 24:37f139e067b2 | 155 | motor->set_speed(1,0); //There's no soft stop. could it work like this? |
danielfpq | 24:37f139e067b2 | 156 | //current_speed= motor->get_speed(0); We could do lie this? |
danielfpq | 24:37f139e067b2 | 157 | //motor->go_to(current_pose); |
danielfpq | 24:37f139e067b2 | 158 | } |
danielfpq | 24:37f139e067b2 | 159 | else if (speedMap>0) |
danielfpq | 24:37f139e067b2 | 160 | { |
danielfpq | 24:37f139e067b2 | 161 | motor->run(1,BDCMotor::FWD); |
danielfpq | 24:37f139e067b2 | 162 | motor->set_speed(1,(unsigned int) speedMap); |
danielfpq | 24:37f139e067b2 | 163 | } |
danielfpq | 24:37f139e067b2 | 164 | else if (speedMap<0) |
danielfpq | 24:37f139e067b2 | 165 | { |
danielfpq | 24:37f139e067b2 | 166 | motor->run(1,BDCMotor::BWD); |
danielfpq | 24:37f139e067b2 | 167 | motor->set_speed(1,(unsigned int) -speedMap); |
danielfpq | 24:37f139e067b2 | 168 | } |
danielfpq | 24:37f139e067b2 | 169 | else |
danielfpq | 24:37f139e067b2 | 170 | { |
danielfpq | 24:37f139e067b2 | 171 | motor->set_speed(0,0); //Riportare errore? NO |
danielfpq | 24:37f139e067b2 | 172 | } |
danielfpq | 24:37f139e067b2 | 173 | } |
stebonicelli | 25:84d2bd93bf97 | 174 | } |
nucleosam | 0:36aa6787d4f9 | 175 | } |
danielfpq | 24:37f139e067b2 | 176 | //Aggiungere un get speed,FATTO ci sarebbe anche il set_speed |
danielfpq | 24:37f139e067b2 | 177 | //Dentro la libreria controllare se c'è controllo di Duty cycle FATTO |
danielfpq | 24:37f139e067b2 | 178 | //motor->go_to(current_pose); |
stebonicelli | 17:dc1b04f0b55d | 179 | |
nucleosam | 0:36aa6787d4f9 | 180 | /* Main ----------------------------------------------------------------------*/ |
nucleosam | 0:36aa6787d4f9 | 181 | |
nucleosam | 0:36aa6787d4f9 | 182 | int main() |
nucleosam | 0:36aa6787d4f9 | 183 | { |
stebonicelli | 21:533d014f09e0 | 184 | can1.frequency(125000); |
danielfpq | 24:37f139e067b2 | 185 | messageIn.format=CANExtended; |
danielfpq | 24:37f139e067b2 | 186 | messageOut.format=CANExtended; |
stebonicelli | 17:dc1b04f0b55d | 187 | // Motor Initialization |
stebonicelli | 17:dc1b04f0b55d | 188 | |
danielfpq | 24:37f139e067b2 | 189 | #ifdef TARGET_STM32F429 |
danielfpq | 24:37f139e067b2 | 190 | motor = new L6206(D2, A4, PB_4, PC_7, PA_15, PB_3); |
danielfpq | 24:37f139e067b2 | 191 | #else |
danielfpq | 24:37f139e067b2 | 192 | motor = new L6206(D2, A4, D5, D4, A0, A1); |
danielfpq | 24:37f139e067b2 | 193 | #endif |
danielfpq | 24:37f139e067b2 | 194 | |
danielfpq | 24:37f139e067b2 | 195 | if (motor->init(&init) != COMPONENT_OK) |
stebonicelli | 17:dc1b04f0b55d | 196 | { |
gidiana | 18:65707db67191 | 197 | printf("ERROR: vvMotor Init\n\r"); |
davide.aliprandi@st.com | 5:bc710d77d801 | 198 | exit(EXIT_FAILURE); |
davide.aliprandi@st.com | 5:bc710d77d801 | 199 | } |
nucleosam | 0:36aa6787d4f9 | 200 | |
danielfpq | 24:37f139e067b2 | 201 | motor->attach_flag_interrupt(my_flag_irq_handler); |
danielfpq | 24:37f139e067b2 | 202 | motor->attach_error_handler(my_error_handler); |
danielfpq | 24:37f139e067b2 | 203 | |
stebonicelli | 21:533d014f09e0 | 204 | |
stebonicelli | 19:9680ebe86f4a | 205 | printf("DONE: Motor Init\n\r"); |
nucleosam | 0:36aa6787d4f9 | 206 | |
stebonicelli | 17:dc1b04f0b55d | 207 | // CAN Initialization |
stebonicelli | 21:533d014f09e0 | 208 | |
stebonicelli | 19:9680ebe86f4a | 209 | canrxa.start(canrx); |
stebonicelli | 17:dc1b04f0b55d | 210 | |
stebonicelli | 17:dc1b04f0b55d | 211 | printf("DONE: CAN Init\n\r"); |
stebonicelli | 17:dc1b04f0b55d | 212 | |
gidiana | 20:8e5dd30b1b59 | 213 | |
stebonicelli | 17:dc1b04f0b55d | 214 | |
stebonicelli | 17:dc1b04f0b55d | 215 | printf("Running!\n\r"); |
stebonicelli | 17:dc1b04f0b55d | 216 | |
stebonicelli | 17:dc1b04f0b55d | 217 | while(true) |
gidiana | 13:08617f604d55 | 218 | { |
stebonicelli | 17:dc1b04f0b55d | 219 | wait(1000); |
stebonicelli | 17:dc1b04f0b55d | 220 | } |
nucleosam | 0:36aa6787d4f9 | 221 | } |