Stepper controller with can interface
Dependencies: X-NUCLEO-IHM05A1
main.cpp
- Committer:
- gidiana
- Date:
- 2019-04-06
- Revision:
- 23:3110010d98a0
- Parent:
- 22:137d1ef8d22d
- Child:
- 24:37f139e067b2
File content as of revision 23:3110010d98a0:
#include "mbed.h" #include "L6208.h" #define VREFA_PWM_PIN D3 #define VREFB_PWM_PIN D9 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) }; 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 pose = 0; void motor_error_handler(uint16_t error) { printf("ERROR: Motor Runtime\n\r"); while(1){} } void motor_zero() { motor->run(StepperMotor::FWD); } void button_int_handler() { printf("POSITION: %d\n\r", motor->get_position()); motor_zero(); } void end0_int_handler() { printf("END0: Pressed\n\rPOSITION: %d\n\r", motor->get_position()); } 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; } // 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==0x010) { pose=messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24); printf("CAN: mess %d\n\r", pose); if (pose == 1) { motor->run(StepperMotor::FWD); } else if (pose == 0) { motor->soft_stop(); current_pose= motor->get_position(); motor->go_to(current_pose); } else if (pose == 2) { motor->run(StepperMotor::BWD); } else { motor->soft_stop(); current_pose= motor->get_position(); motor->go_to(current_pose); } // motor->go_to(pose); } } } /* Main ----------------------------------------------------------------------*/ int main() { can1.frequency(125000); // 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_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 canrxa.start(canrx); printf("DONE: CAN Init\n\r"); printf("Running!\n\r"); while(true) { wait(1000); } }