BASE
Dependencies: X-NUCLEO-IHM05A1
Diff: main.cpp
- Revision:
- 17:dc1b04f0b55d
- Parent:
- 16:1dc772fba09f
- Child:
- 18:65707db67191
diff -r 1dc772fba09f -r dc1b04f0b55d main.cpp --- a/main.cpp Fri Dec 07 17:40:15 2018 +0000 +++ b/main.cpp Thu Feb 07 09:13:56 2019 +0000 @@ -1,21 +1,16 @@ +#include "mbed.h" +#include "L6208.h" -/* Includes ------------------------------------------------------------------*/ -/* mbed specific header files. */ -#include "mbed.h" -/* Component specific header files. */ -#include "L6208.h" -/* Definitions ---------------------------------------------------------------*/ #define VREFA_PWM_PIN D3 #define VREFB_PWM_PIN D9 -/* Variables -----------------------------------------------------------------*/ -/* Initialization parameters of the motor connected to the expansion board. */ + l6208_init_t init = { - 1500, //Acceleration rate in step/s^2 or (1/16)th step/s^2 for microstep modes + 2000, //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) - 1500, //Deceleration rate in step/s^2 or (1/16)th step/s^2 for microstep modes + 2000, //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) - 1000, //Running speed in step/s or (1/16)th step/s for microstep modes + 6000, //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 @@ -25,121 +20,121 @@ 100000 //VREFA and VREFB PWM frequency (Hz) }; -/* Motor Control Component. */ + +// Utility +InterruptIn button(USER_BUTTON); +DigitalOut led(LED1); + +// Motor Control L6208 *motor; -DigitalIn button (USER_BUTTON); -DigitalIn end(PC_10); -DigitalOut led(LED1); -/* Functions -----------------------------------------------------------------*/ + +InterruptIn end0(PC_10, PullUp); +InterruptIn end1(PC_11, PullUp); +InterruptIn enc(PC_12, PullUp); + +int current_pose = 0; +int pose = 0; -/** - * @brief This is an example of user handler for the flag interrupt. - * @param None - * @retval None - * @note If needed, implement it, and then attach and enable it: - * + motor->attach_flag_irq(&my_flag_irq_handler); - * + motor->enable_flag_irq(); - * To disable it: - * + motor->DisbleFlagIRQ(); - */ -void my_flag_irq_handler(void) +void motor_error_handler(uint16_t error) +{ + printf("ERROR: Motor Runtime\n\r"); + while(1){} +} + +void button_int_handler() { - printf(" WARNING: \"FLAG\" interrupt triggered:\r\n"); - motor->disable(); - printf(" Motor disabled.\r\n\n"); + printf("POSITION: %d\n\r", motor->get_position()); + + motor->go_to(-160000); +} + +void end0_int_handler() +{ + printf("END0: Pressed\n\rPOSITION: %d\n\r", motor->get_position()); } -/** - * @brief This is an example of error handler. - * @param[in] error Number of the error - * @retval None - * @note If needed, implement it, and then attach it: - * + motor->attach_error_handler(&my_error_handler); - */ -void my_error_handler(uint16_t error) +void end1_int_handler() +{ + motor->hard_stop(); + + 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; +} + +void motor_zero() { - /* Printing to the console. */ - printf("Error %d detected\r\n\n", error); + motor->run(StepperMotor::BWD); +} + +// CAN +CAN can1(PB_8, PB_9); // RX, TX + +CANMessage messageIn; +CANMessage messageOut; + +int filter = can1.filter(0x020, 0x4FF, CANStandard); + +void can_rx_handler() +{ + printf("CAN: Rec\n\r"); - /* Infinite loop */ - while (true) { - } + if(can1.read(messageIn, filter)) + { + printf("CAN: Rec %d\n\r", (messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24))); + } } + + /* Main ----------------------------------------------------------------------*/ int main() { - -//----- Initialization - /* Initializing Motor Control Component. */ + // 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 init\n\r"); + + if (motor->init(&init) != COMPONENT_OK) + { + printf("ERROR: Motor Init\n\r"); exit(EXIT_FAILURE); } - /* Attaching and enabling an interrupt handler. */ - motor->attach_flag_irq(&my_flag_irq_handler); - motor->enable_flag_irq(); - - /* Attaching an error handler */ - motor->attach_error_handler(&my_error_handler); - - motor->set_step_mode(StepperMotor::STEP_MODE_1_8); - - /* Set speed, acceleration and deceleration to scale with microstep mode */ - //motor->set_max_speed(2000); - //motor->set_acceleration(2400); - //motor->set_deceleration(2400); - motor->set_max_speed(motor->get_max_speed()<<2); - motor->set_acceleration(motor->get_acceleration()<<2); - motor->set_deceleration(motor->get_deceleration()<<2); - printf(" init\n\r"); - end.mode(PullUp); - /* Request to go position 9000 (quarter steps) */ - motor->run(StepperMotor::BWD); - while(1) - { - // motor->wait_while_active(); - printf("while"); - if(end) break; - } - motor->hard_stop(); - led=1; + 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); + + printf("DONE: Motor Init\n\r"); - int pos=motor->get_position(); - printf("pos:%d\n\r", pos); - motor->go_to(pos+200); - motor->wait_while_active(); - led=0; - wait(0.5); - - motor->set_home(); - wait(1); - - motor->go_to(1000<<4); - motor->wait_while_active(); - - while (true) -{ - motor->go_to(500<<4); - motor->wait_while_active(); - - motor->go_to(0); - motor->wait_while_active(); - while (button) + // CAN Initialization + can1.reset(); + can1.frequency(500000); + can1.attach(&can_rx_handler); + + printf("DONE: CAN Init\n\r"); + + motor->run(StepperMotor::FWD); + + printf("Running!\n\r"); + + while(true) { - - } -} - - - /* Waiting while the motor is active. */ - motor->wait_while_active(); - - wait_ms(5000); - motor->disable(); + wait(1000); + } }