Team DIANA / Mbed OS arm_linear_can_2

Linear driver with st driver

Dependencies:   X_NUCLEO_IHM04A1

Dependents:   Basic_DC_Control Basic_DC_Control1 DC_Serial

main.cpp@26:c18d6aaa474a, 2019-06-19 (annotated)

Committer:
stebonicelli
Date:
Wed Jun 19 16:29:11 2019 +0000
Revision:
26:c18d6aaa474a
Parent:
25:84d2bd93bf97
Child:
27:bf0109fb61c3
Linear Driver

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
stebonicelli 26:c18d6aaa474a 4 #define JOINT_SET_SPEED 20
stebonicelli 26:c18d6aaa474a 5
stebonicelli 26:c18d6aaa474a 6 #define JOINT_ID 1
stebonicelli 26:c18d6aaa474a 7
nucleosam 0:36aa6787d4f9 8
danielfpq 24:37f139e067b2 9 static volatile uint16_t gLastError;
danielfpq 24:37f139e067b2 10 static volatile uint8_t gStep = 0;
stebonicelli 17:dc1b04f0b55d 11
danielfpq 24:37f139e067b2 12 int current_pose = 0;
danielfpq 24:37f139e067b2 13 int speed = 0;
danielfpq 24:37f139e067b2 14
danielfpq 24:37f139e067b2 15 L6206_init_t init =
nucleosam 0:36aa6787d4f9 16 {
danielfpq 24:37f139e067b2 17 L6206_CONF_PARAM_PARALLE_BRIDGES,
danielfpq 24:37f139e067b2 18 {L6206_CONF_PARAM_FREQ_PWM1A, L6206_CONF_PARAM_FREQ_PWM2A, L6206_CONF_PARAM_FREQ_PWM1B, L6206_CONF_PARAM_FREQ_PWM2B},
danielfpq 24:37f139e067b2 19 {100,100,100,100},
danielfpq 24:37f139e067b2 20 {FORWARD,FORWARD,BACKWARD,FORWARD},
danielfpq 24:37f139e067b2 21 {INACTIVE,INACTIVE,INACTIVE,INACTIVE},
danielfpq 24:37f139e067b2 22 {FALSE,FALSE}
nucleosam 0:36aa6787d4f9 23 };
stebonicelli 21:533d014f09e0 24
danielfpq 24:37f139e067b2 25 L6206 *motor;
stebonicelli 26:c18d6aaa474a 26
gidiana 18:65707db67191 27 Thread canrxa;
stebonicelli 17:dc1b04f0b55d 28
danielfpq 24:37f139e067b2 29 //Utility
stebonicelli 17:dc1b04f0b55d 30 InterruptIn button(USER_BUTTON);
danielfpq 24:37f139e067b2 31 DigitalOut led(LED1); //Change?
stebonicelli 17:dc1b04f0b55d 32
nucleosam 0:36aa6787d4f9 33
stebonicelli 17:dc1b04f0b55d 34 void motor_error_handler(uint16_t error)
stebonicelli 17:dc1b04f0b55d 35 {
stebonicelli 17:dc1b04f0b55d 36 printf("ERROR: Motor Runtime\n\r");
danielfpq 24:37f139e067b2 37 while(1){};
stebonicelli 17:dc1b04f0b55d 38 }
stebonicelli 17:dc1b04f0b55d 39
stebonicelli 21:533d014f09e0 40 void motor_zero()
stebonicelli 21:533d014f09e0 41 {
danielfpq 24:37f139e067b2 42 motor->run(0, BDCMotor::FWD);
danielfpq 24:37f139e067b2 43 motor->run(1, BDCMotor::FWD);
stebonicelli 21:533d014f09e0 44 }
stebonicelli 21:533d014f09e0 45
danielfpq 24:37f139e067b2 46 void button_int_handler(unsigned int motorId)
nucleosam 0:36aa6787d4f9 47 {
danielfpq 24:37f139e067b2 48 printf("MOTOR SPEED: %d\n\r", motor->get_speed(motorId));
stebonicelli 21:533d014f09e0 49 motor_zero();
stebonicelli 17:dc1b04f0b55d 50 }
stebonicelli 17:dc1b04f0b55d 51
danielfpq 24:37f139e067b2 52 void my_error_handler(uint16_t error)
danielfpq 24:37f139e067b2 53 {
danielfpq 24:37f139e067b2 54 /* Backup error number */
danielfpq 24:37f139e067b2 55 gLastError = error;
danielfpq 24:37f139e067b2 56
danielfpq 24:37f139e067b2 57 /* Enter your own code here */
danielfpq 24:37f139e067b2 58 }
danielfpq 24:37f139e067b2 59
danielfpq 24:37f139e067b2 60 void my_flag_irq_handler(void)
stebonicelli 17:dc1b04f0b55d 61 {
danielfpq 24:37f139e067b2 62 /* Code to be customised */
danielfpq 24:37f139e067b2 63 /************************/
danielfpq 24:37f139e067b2 64 /* Get the state of bridge A */
danielfpq 24:37f139e067b2 65 uint16_t bridgeState = motor->get_bridge_status(0);
danielfpq 24:37f139e067b2 66
danielfpq 24:37f139e067b2 67 if (bridgeState == 0) {
danielfpq 24:37f139e067b2 68 if ((motor->get_device_state(0) != INACTIVE)||
danielfpq 24:37f139e067b2 69 (motor->get_device_state(1) != INACTIVE)) {
danielfpq 24:37f139e067b2 70 /* Bridge A was disabling due to overcurrent or over temperature */
danielfpq 24:37f139e067b2 71 /* When at least on of its motor was running */
danielfpq 24:37f139e067b2 72 my_error_handler(0XBAD0);
danielfpq 24:37f139e067b2 73 }
danielfpq 24:37f139e067b2 74 }
danielfpq 24:37f139e067b2 75
danielfpq 24:37f139e067b2 76 /* Get the state of bridge B */
danielfpq 24:37f139e067b2 77 bridgeState = motor->get_bridge_status(1);
danielfpq 24:37f139e067b2 78
danielfpq 24:37f139e067b2 79 if (bridgeState == 0) {
danielfpq 24:37f139e067b2 80 if ((motor->get_device_state(2) != INACTIVE)||
danielfpq 24:37f139e067b2 81 (motor->get_device_state(3) != INACTIVE)) {
danielfpq 24:37f139e067b2 82 /* Bridge A was disabling due to overcurrent or over temperature */
danielfpq 24:37f139e067b2 83 /* When at least on of its motor was running */
danielfpq 24:37f139e067b2 84 my_error_handler(0XBAD1);
danielfpq 24:37f139e067b2 85 }
danielfpq 24:37f139e067b2 86 }
danielfpq 24:37f139e067b2 87 }
stebonicelli 17:dc1b04f0b55d 88
danielfpq 24:37f139e067b2 89 // CAN, to revise
gidiana 23:3110010d98a0 90 CAN can1(PB_12, PB_13); // RX, TX
stebonicelli 17:dc1b04f0b55d 91
stebonicelli 17:dc1b04f0b55d 92 CANMessage messageIn;
stebonicelli 17:dc1b04f0b55d 93
gidiana 18:65707db67191 94 void canrx()
stebonicelli 17:dc1b04f0b55d 95 {
stebonicelli 19:9680ebe86f4a 96 while(1)
stebonicelli 21:533d014f09e0 97 {
stebonicelli 26:c18d6aaa474a 98 if(can1.read(messageIn))
danielfpq 24:37f139e067b2 99 {
stebonicelli 26:c18d6aaa474a 100 if(messageIn.id == ((JOINT_SET_SPEED << 8) + JOINT_ID))
danielfpq 24:37f139e067b2 101 {
stebonicelli 26:c18d6aaa474a 102 speed = 0;
stebonicelli 26:c18d6aaa474a 103 speed = (messageIn.data[0] << 24) | (messageIn.data[1] << 16) | (messageIn.data[2] << 8) | (messageIn.data[3]);
stebonicelli 26:c18d6aaa474a 104
stebonicelli 26:c18d6aaa474a 105 motor->set_speed(0, speed);
stebonicelli 26:c18d6aaa474a 106 (speed > 0) ? motor->run(0, BDCMotor::BWD) : motor->run(0, BDCMotor::FWD);
stebonicelli 26:c18d6aaa474a 107
stebonicelli 26:c18d6aaa474a 108 printf("CAN: mess %d\n\r", speed);
danielfpq 24:37f139e067b2 109 }
stebonicelli 26:c18d6aaa474a 110 }
stebonicelli 26:c18d6aaa474a 111
stebonicelli 26:c18d6aaa474a 112 wait(0.1);
stebonicelli 26:c18d6aaa474a 113 }
nucleosam 0:36aa6787d4f9 114 }
stebonicelli 26:c18d6aaa474a 115
stebonicelli 17:dc1b04f0b55d 116
nucleosam 0:36aa6787d4f9 117 /* Main ----------------------------------------------------------------------*/
nucleosam 0:36aa6787d4f9 118 int main()
nucleosam 0:36aa6787d4f9 119 {
stebonicelli 26:c18d6aaa474a 120 can1.frequency(500000);
danielfpq 24:37f139e067b2 121 messageIn.format=CANExtended;
stebonicelli 26:c18d6aaa474a 122
stebonicelli 17:dc1b04f0b55d 123 // Motor Initialization
stebonicelli 17:dc1b04f0b55d 124
danielfpq 24:37f139e067b2 125 #ifdef TARGET_STM32F429
danielfpq 24:37f139e067b2 126 motor = new L6206(D2, A4, PB_4, PC_7, PA_15, PB_3);
danielfpq 24:37f139e067b2 127 #else
danielfpq 24:37f139e067b2 128 motor = new L6206(D2, A4, D5, D4, A0, A1);
danielfpq 24:37f139e067b2 129 #endif
danielfpq 24:37f139e067b2 130
danielfpq 24:37f139e067b2 131 if (motor->init(&init) != COMPONENT_OK)
stebonicelli 17:dc1b04f0b55d 132 {
gidiana 18:65707db67191 133 printf("ERROR: vvMotor Init\n\r");
davide.aliprandi@st.com 5:bc710d77d801 134 exit(EXIT_FAILURE);
davide.aliprandi@st.com 5:bc710d77d801 135 }
nucleosam 0:36aa6787d4f9 136
danielfpq 24:37f139e067b2 137 motor->attach_flag_interrupt(my_flag_irq_handler);
danielfpq 24:37f139e067b2 138 motor->attach_error_handler(my_error_handler);
danielfpq 24:37f139e067b2 139
stebonicelli 21:533d014f09e0 140
stebonicelli 19:9680ebe86f4a 141 printf("DONE: Motor Init\n\r");
nucleosam 0:36aa6787d4f9 142
stebonicelli 17:dc1b04f0b55d 143 // CAN Initialization
stebonicelli 21:533d014f09e0 144
stebonicelli 19:9680ebe86f4a 145 canrxa.start(canrx);
stebonicelli 17:dc1b04f0b55d 146
stebonicelli 17:dc1b04f0b55d 147 printf("DONE: CAN Init\n\r");
stebonicelli 17:dc1b04f0b55d 148
gidiana 20:8e5dd30b1b59 149
stebonicelli 17:dc1b04f0b55d 150
stebonicelli 17:dc1b04f0b55d 151 printf("Running!\n\r");
stebonicelli 17:dc1b04f0b55d 152
stebonicelli 17:dc1b04f0b55d 153 while(true)
gidiana 13:08617f604d55 154 {
stebonicelli 26:c18d6aaa474a 155 wait(1);
stebonicelli 17:dc1b04f0b55d 156 }
nucleosam 0:36aa6787d4f9 157 }