Team DIANA / Mbed OS arm_dcmotor_can

__

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?

UserRevisionLine numberNew 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 }