Linear driver with st driver
Dependencies: X_NUCLEO_IHM04A1
Dependents: Basic_DC_Control Basic_DC_Control1 DC_Serial
main.cpp
- Committer:
- stebonicelli
- Date:
- 2019-06-19
- Revision:
- 26:c18d6aaa474a
- Parent:
- 25:84d2bd93bf97
- Child:
- 27:bf0109fb61c3
File content as of revision 26:c18d6aaa474a:
#include "mbed.h" #include "L6206.h" #define JOINT_SET_SPEED 20 #define JOINT_ID 1 static volatile uint16_t gLastError; static volatile uint8_t gStep = 0; int current_pose = 0; int speed = 0; L6206_init_t init = { L6206_CONF_PARAM_PARALLE_BRIDGES, {L6206_CONF_PARAM_FREQ_PWM1A, L6206_CONF_PARAM_FREQ_PWM2A, L6206_CONF_PARAM_FREQ_PWM1B, L6206_CONF_PARAM_FREQ_PWM2B}, {100,100,100,100}, {FORWARD,FORWARD,BACKWARD,FORWARD}, {INACTIVE,INACTIVE,INACTIVE,INACTIVE}, {FALSE,FALSE} }; L6206 *motor; Thread canrxa; //Utility InterruptIn button(USER_BUTTON); DigitalOut led(LED1); //Change? void motor_error_handler(uint16_t error) { printf("ERROR: Motor Runtime\n\r"); while(1){}; } void motor_zero() { motor->run(0, BDCMotor::FWD); motor->run(1, BDCMotor::FWD); } void button_int_handler(unsigned int motorId) { printf("MOTOR SPEED: %d\n\r", motor->get_speed(motorId)); motor_zero(); } void my_error_handler(uint16_t error) { /* Backup error number */ gLastError = error; /* Enter your own code here */ } void my_flag_irq_handler(void) { /* Code to be customised */ /************************/ /* Get the state of bridge A */ uint16_t bridgeState = motor->get_bridge_status(0); if (bridgeState == 0) { if ((motor->get_device_state(0) != INACTIVE)|| (motor->get_device_state(1) != INACTIVE)) { /* Bridge A was disabling due to overcurrent or over temperature */ /* When at least on of its motor was running */ my_error_handler(0XBAD0); } } /* Get the state of bridge B */ bridgeState = motor->get_bridge_status(1); if (bridgeState == 0) { if ((motor->get_device_state(2) != INACTIVE)|| (motor->get_device_state(3) != INACTIVE)) { /* Bridge A was disabling due to overcurrent or over temperature */ /* When at least on of its motor was running */ my_error_handler(0XBAD1); } } } // CAN, to revise CAN can1(PB_12, PB_13); // RX, TX CANMessage messageIn; void canrx() { while(1) { if(can1.read(messageIn)) { if(messageIn.id == ((JOINT_SET_SPEED << 8) + JOINT_ID)) { speed = 0; speed = (messageIn.data[0] << 24) | (messageIn.data[1] << 16) | (messageIn.data[2] << 8) | (messageIn.data[3]); motor->set_speed(0, speed); (speed > 0) ? motor->run(0, BDCMotor::BWD) : motor->run(0, BDCMotor::FWD); printf("CAN: mess %d\n\r", speed); } } wait(0.1); } } /* Main ----------------------------------------------------------------------*/ int main() { can1.frequency(500000); messageIn.format=CANExtended; // Motor Initialization #ifdef TARGET_STM32F429 motor = new L6206(D2, A4, PB_4, PC_7, PA_15, PB_3); #else motor = new L6206(D2, A4, D5, D4, A0, A1); #endif if (motor->init(&init) != COMPONENT_OK) { printf("ERROR: vvMotor Init\n\r"); exit(EXIT_FAILURE); } motor->attach_flag_interrupt(my_flag_irq_handler); motor->attach_error_handler(my_error_handler); printf("DONE: Motor Init\n\r"); // CAN Initialization canrxa.start(canrx); printf("DONE: CAN Init\n\r"); printf("Running!\n\r"); while(true) { wait(1); } }