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@28:637b6f726971, 2019-06-21 (annotated)

Committer:
stebonicelli
Date:
Fri Jun 21 07:56:33 2019 +0000
Revision:
28:637b6f726971
Parent:
27:bf0109fb61c3 
Revision and CleanUp

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