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@27:bf0109fb61c3, 2019-06-21 (annotated)

Committer:
gidiana
Date:
Fri Jun 21 07:43:15 2019 +0000
Revision:
27:bf0109fb61c3
Parent:
26:c18d6aaa474a
Child:
28:637b6f726971
Paolone

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
gidiana 27:bf0109fb61c3 6 #define JOINT_ID 3
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 {
gidiana 27:bf0109fb61c3 17 PARALLELING_NONE___1_BIDIR_MOTOR_BRIDGE_A__1_BIDIR_MOTOR_BRIDGE_B,
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},
gidiana 27:bf0109fb61c3 20 {FORWARD,BACKWARD,FORWARD,BACKWARD},
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 27:bf0109fb61c3 90 CAN can1(PB_8, PB_9); // RX, TX
stebonicelli 17:dc1b04f0b55d 91
stebonicelli 17:dc1b04f0b55d 92 CANMessage messageIn;
gidiana 27:bf0109fb61c3 93 CANMessage messageOut;
stebonicelli 17:dc1b04f0b55d 94
gidiana 18:65707db67191 95 void canrx()
stebonicelli 17:dc1b04f0b55d 96 {
stebonicelli 19:9680ebe86f4a 97 while(1)
stebonicelli 21:533d014f09e0 98 {
stebonicelli 26:c18d6aaa474a 99 if(can1.read(messageIn))
danielfpq 24:37f139e067b2 100 {
gidiana 27:bf0109fb61c3 101 printf("READ!\n\r");
gidiana 27:bf0109fb61c3 102
stebonicelli 26:c18d6aaa474a 103 if(messageIn.id == ((JOINT_SET_SPEED << 8) + JOINT_ID))
danielfpq 24:37f139e067b2 104 {
stebonicelli 26:c18d6aaa474a 105 speed = 0;
stebonicelli 26:c18d6aaa474a 106 speed = (messageIn.data[0] << 24) | (messageIn.data[1] << 16) | (messageIn.data[2] << 8) | (messageIn.data[3]);
stebonicelli 26:c18d6aaa474a 107
stebonicelli 26:c18d6aaa474a 108 motor->set_speed(0, speed);
stebonicelli 26:c18d6aaa474a 109 (speed > 0) ? motor->run(0, BDCMotor::BWD) : motor->run(0, BDCMotor::FWD);
stebonicelli 26:c18d6aaa474a 110
stebonicelli 26:c18d6aaa474a 111 printf("CAN: mess %d\n\r", speed);
danielfpq 24:37f139e067b2 112 }
stebonicelli 26:c18d6aaa474a 113 }
stebonicelli 26:c18d6aaa474a 114
gidiana 27:bf0109fb61c3 115 int speed = 5000;
gidiana 27:bf0109fb61c3 116
gidiana 27:bf0109fb61c3 117 messageOut.data[0] = speed >> 24;
gidiana 27:bf0109fb61c3 118 messageOut.data[1] = speed >> 16;
gidiana 27:bf0109fb61c3 119 messageOut.data[2] = speed >> 8;
gidiana 27:bf0109fb61c3 120 messageOut.data[3] = speed;
gidiana 27:bf0109fb61c3 121
gidiana 27:bf0109fb61c3 122 messageOut.id = (3 << 8) + 2;
gidiana 27:bf0109fb61c3 123 int status = can1.write(messageOut);
gidiana 27:bf0109fb61c3 124
gidiana 27:bf0109fb61c3 125 printf("STATUS: %d\n\r", status);
gidiana 27:bf0109fb61c3 126
gidiana 27:bf0109fb61c3 127 wait(0.01);
stebonicelli 26:c18d6aaa474a 128 }
nucleosam 0:36aa6787d4f9 129 }
stebonicelli 26:c18d6aaa474a 130
stebonicelli 17:dc1b04f0b55d 131
nucleosam 0:36aa6787d4f9 132 /* Main ----------------------------------------------------------------------*/
nucleosam 0:36aa6787d4f9 133 int main()
nucleosam 0:36aa6787d4f9 134 {
gidiana 27:bf0109fb61c3 135 can1.frequency(125000);
danielfpq 24:37f139e067b2 136 messageIn.format=CANExtended;
gidiana 27:bf0109fb61c3 137 messageOut.format=CANExtended;
gidiana 27:bf0109fb61c3 138
stebonicelli 26:c18d6aaa474a 139
stebonicelli 17:dc1b04f0b55d 140 // Motor Initialization
stebonicelli 17:dc1b04f0b55d 141
danielfpq 24:37f139e067b2 142 #ifdef TARGET_STM32F429
danielfpq 24:37f139e067b2 143 motor = new L6206(D2, A4, PB_4, PC_7, PA_15, PB_3);
danielfpq 24:37f139e067b2 144 #else
danielfpq 24:37f139e067b2 145 motor = new L6206(D2, A4, D5, D4, A0, A1);
danielfpq 24:37f139e067b2 146 #endif
danielfpq 24:37f139e067b2 147
danielfpq 24:37f139e067b2 148 if (motor->init(&init) != COMPONENT_OK)
stebonicelli 17:dc1b04f0b55d 149 {
gidiana 18:65707db67191 150 printf("ERROR: vvMotor Init\n\r");
davide.aliprandi@st.com 5:bc710d77d801 151 exit(EXIT_FAILURE);
davide.aliprandi@st.com 5:bc710d77d801 152 }
nucleosam 0:36aa6787d4f9 153
danielfpq 24:37f139e067b2 154 motor->attach_flag_interrupt(my_flag_irq_handler);
danielfpq 24:37f139e067b2 155 motor->attach_error_handler(my_error_handler);
danielfpq 24:37f139e067b2 156
stebonicelli 21:533d014f09e0 157
stebonicelli 19:9680ebe86f4a 158 printf("DONE: Motor Init\n\r");
nucleosam 0:36aa6787d4f9 159
stebonicelli 17:dc1b04f0b55d 160 // CAN Initialization
stebonicelli 21:533d014f09e0 161
stebonicelli 19:9680ebe86f4a 162 canrxa.start(canrx);
stebonicelli 17:dc1b04f0b55d 163
stebonicelli 17:dc1b04f0b55d 164 printf("DONE: CAN Init\n\r");
stebonicelli 17:dc1b04f0b55d 165
gidiana 27:bf0109fb61c3 166 //motor->set_speed(1, 50);
gidiana 27:bf0109fb61c3 167 //motor->run(1, BDCMotor::FWD);
stebonicelli 17:dc1b04f0b55d 168
stebonicelli 17:dc1b04f0b55d 169 printf("Running!\n\r");
stebonicelli 17:dc1b04f0b55d 170
stebonicelli 17:dc1b04f0b55d 171 while(true)
gidiana 13:08617f604d55 172 {
stebonicelli 26:c18d6aaa474a 173 wait(1);
stebonicelli 17:dc1b04f0b55d 174 }
nucleosam 0:36aa6787d4f9 175 }