BASE
Dependencies: X-NUCLEO-IHM05A1
Diff: main.cpp
- Revision:
- 25:281c8e913db4
- Parent:
- 24:37f139e067b2
- Child:
- 26:44175c51a820
diff -r 37f139e067b2 -r 281c8e913db4 main.cpp --- a/main.cpp Wed Jun 12 15:53:00 2019 +0000 +++ b/main.cpp Wed Jun 12 16:10:21 2019 +0000 @@ -1,209 +1,152 @@ #include "mbed.h" -#include "L6206.h" +#include "L6208.h" + +#define VREFA_PWM_PIN D3 +#define VREFB_PWM_PIN D9 -#define MAX_MOTOR (2) +l6208_init_t init = +{ + 8000, //Acceleration rate in step/s^2 or (1/16)th step/s^2 for microstep modes + 80, //Acceleration current torque in % (from 0 to 100) + 8000, //Deceleration rate in step/s^2 or (1/16)th step/s^2 for microstep modes + 80, //Deceleration current torque in % (from 0 to 100) + 8000, //Running speed in step/s or (1/16)th step/s for microstep modes + 80, //Running current torque in % (from 0 to 100) + 40, //Holding current torque in % (from 0 to 100) + STEP_MODE_1_16, //Step mode via enum motorStepMode_t + FAST_DECAY, //Decay mode via enum motorDecayMode_t + 0, //Dwelling time in ms + FALSE, //Automatic HIZ STOP + 100000 //VREFA and VREFB PWM frequency (Hz) +}; -static volatile uint16_t gLastError; -static volatile uint8_t gStep = 0; +Thread canrxa; + +// Utility +InterruptIn button(USER_BUTTON); +DigitalOut led(LED1); + +// Motor Control +L6208 *motor; + +InterruptIn end0(PC_10, PullUp); +InterruptIn end1(PC_11, PullUp); +InterruptIn enc(PC_12, PullUp); 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? - +int pose = 0; void motor_error_handler(uint16_t error) { printf("ERROR: Motor Runtime\n\r"); - while(1){}; + while(1){} } void motor_zero() { - motor->run(0, BDCMotor::FWD); - motor->run(1, BDCMotor::FWD); + motor->run(StepperMotor::FWD); } -void button_int_handler(unsigned int motorId) +void button_int_handler() { - printf("MOTOR SPEED: %d\n\r", motor->get_speed(motorId)); + printf("POSITION: %d\n\r", motor->get_position()); 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) +void end0_int_handler() { - /* 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)); + printf("END0: Pressed\n\rPOSITION: %d\n\r", motor->get_position()); } void end1_int_handler() { - motor->hard_stop(0); - motor->hard_stop(1); //or hard_hiz(); for disabling the bridge? + motor->hard_stop(); - motor->run(0, BDCMotor::BWD); - motor->run(1, BDCMotor::BWD); + motor->run(StepperMotor::BWD); printf("END1: Pressed\n\r"); } - +void motor_set_home() +{ + motor->hard_stop(); + motor->set_home(); + motor->go_to(0); + + current_pose = 0; + pose = 0; +} -// CAN, to revise +// CAN 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 + if(can1.read(messageIn,filter)&&messageIn.id==0x010) { - 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, "\% "); + pose=messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24); + printf("CAN: mess %d\n\r", pose); - //CAN MESSAGE WITH SPEED TO REVISE - //Ci sarebbe anche il set speed. - if (speedMap == 0) + + if (pose == 1) { - 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); + motor->run(StepperMotor::FWD); } - else if (speedMap>0) - { - motor->run(0,BDCMotor::FWD); - motor->set_speed(0,(unsigned int) speedMap); - } - else if (speedMap<0) + else if (pose == 0) { - motor->run(0,BDCMotor::BWD); - motor->set_speed(0,(unsigned int) -speedMap); + motor->soft_stop(); + current_pose= motor->get_position(); + motor->go_to(current_pose); + } + else if (pose == 2) + { + motor->run(StepperMotor::BWD); } 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); + motor->soft_stop(); + current_pose= motor->get_position(); + 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 - } - } + + + // motor->go_to(pose); + } + } } -//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) + // Motor Initialization + motor = new L6208(D2, D8, D7, D4, D5, D6, VREFA_PWM_PIN, VREFB_PWM_PIN); + + 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); - - + motor->attach_error_handler(&motor_error_handler); + + end0.rise(&end0_int_handler); + end1.rise(&end1_int_handler); + end1.fall(&motor_set_home); button.rise(&button_int_handler); + motor->set_step_mode(StepperMotor::STEP_MODE_1_16); printf("DONE: Motor Init\n\r"); // CAN Initialization