BASE
Dependencies: X-NUCLEO-IHM05A1
main.cpp
- Committer:
- danielfpq
- Date:
- 2019-06-12
- Revision:
- 24:37f139e067b2
- Parent:
- 23:3110010d98a0
- Child:
- 25:281c8e913db4
File content as of revision 24:37f139e067b2:
#include "mbed.h" #include "L6206.h" #define MAX_MOTOR (2) 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; InterruptIn my_button_irq(USER_BUTTON); /* User button on Nucleo board */ 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); } } } void end0_int_handler(unsigned int motorId) { printf("END0: Pressed\n\rSPEED: %d\n\r", motor->get_speed(motorId)); } void end1_int_handler() { motor->hard_stop(0); motor->hard_stop(1); //or hard_hiz(); for disabling the bridge? motor->run(0, BDCMotor::BWD); motor->run(1, BDCMotor::BWD); printf("END1: Pressed\n\r"); } // CAN, to revise CAN can1(PB_12, PB_13); // RX, TX CANMessage messageIn; CANMessage messageOut; int filter = can1.filter(0x010, 0x4FF, CANStandard); void canrx() { while(1) { if(can1.read(messageIn,filter)&& ((messageIn.id>>8 == 20) && (messageIn.id & 0x00FF==6))) //Primo motore { speed=messageIn.data[0]; //Messaggio da 0 a 255, devo sottrrarre 127 e imporre velocità con segno float speedMap=(speed-127)/127*100; printf("CAN: mess %d\n\r", speedMap, "\% "); //CAN MESSAGE WITH SPEED TO REVISE //Ci sarebbe anche il set speed. if (speedMap == 0) { motor->set_speed(0,0); //There's no soft stop. could it work like this? //current_speed= motor->get_speed(0); We could do lie this? //motor->go_to(current_pose); } else if (speedMap>0) { motor->run(0,BDCMotor::FWD); motor->set_speed(0,(unsigned int) speedMap); } else if (speedMap<0) { motor->run(0,BDCMotor::BWD); motor->set_speed(0,(unsigned int) -speedMap); } else { motor->set_speed(0,0); //Riportare errore? NO } } else if(can1.read(messageIn,filter)&& ((messageIn.id>>8 == 20) && (messageIn.id & 0x00FF==7))) //Secondo motore { speed=messageIn.data[0]; //Messaggio da 0 a 255, devo sottrrarre 127 e imporre velocità con segno float speedMap=(speed-127)/127*100; printf("CAN: mess %d\n\r", speedMap, "\% "); //CAN MESSAGE WITH SPEED TO REVISE //Ci sarebbe anche il set speed. if (speedMap == 0) { motor->set_speed(1,0); //There's no soft stop. could it work like this? //current_speed= motor->get_speed(0); We could do lie this? //motor->go_to(current_pose); } else if (speedMap>0) { motor->run(1,BDCMotor::FWD); motor->set_speed(1,(unsigned int) speedMap); } else if (speedMap<0) { motor->run(1,BDCMotor::BWD); motor->set_speed(1,(unsigned int) -speedMap); } else { motor->set_speed(0,0); //Riportare errore? NO } } } //Aggiungere un get speed,FATTO ci sarebbe anche il set_speed //Dentro la libreria controllare se c'è controllo di Duty cycle FATTO //motor->go_to(current_pose); /* Main ----------------------------------------------------------------------*/ int main() { can1.frequency(125000); messageIn.format=CANExtended; messageOut.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); button.rise(&button_int_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(1000); } }