BASE
Dependencies: X-NUCLEO-IHM05A1
main.cpp@26:44175c51a820, 2019-06-18 (annotated)
- Committer:
- stebonicelli
- Date:
- Tue Jun 18 16:28:38 2019 +0000
- Revision:
- 26:44175c51a820
- Parent:
- 25:281c8e913db4
- Child:
- 27:275ba9c137c9
Can protocol implemented
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
stebonicelli | 17:dc1b04f0b55d | 1 | #include "mbed.h" |
stebonicelli | 25:281c8e913db4 | 2 | #include "L6208.h" |
stebonicelli | 25:281c8e913db4 | 3 | |
stebonicelli | 25:281c8e913db4 | 4 | #define VREFA_PWM_PIN D3 |
stebonicelli | 25:281c8e913db4 | 5 | #define VREFB_PWM_PIN D9 |
danielfpq | 24:37f139e067b2 | 6 | |
stebonicelli | 26:44175c51a820 | 7 | #define JOINT_SET_SPEED 20 |
stebonicelli | 26:44175c51a820 | 8 | |
stebonicelli | 26:44175c51a820 | 9 | #define JOINT_ID 1 |
stebonicelli | 26:44175c51a820 | 10 | |
stebonicelli | 25:281c8e913db4 | 11 | l6208_init_t init = |
stebonicelli | 25:281c8e913db4 | 12 | { |
stebonicelli | 25:281c8e913db4 | 13 | 8000, //Acceleration rate in step/s^2 or (1/16)th step/s^2 for microstep modes |
stebonicelli | 25:281c8e913db4 | 14 | 80, //Acceleration current torque in % (from 0 to 100) |
stebonicelli | 25:281c8e913db4 | 15 | 8000, //Deceleration rate in step/s^2 or (1/16)th step/s^2 for microstep modes |
stebonicelli | 25:281c8e913db4 | 16 | 80, //Deceleration current torque in % (from 0 to 100) |
stebonicelli | 25:281c8e913db4 | 17 | 8000, //Running speed in step/s or (1/16)th step/s for microstep modes |
stebonicelli | 25:281c8e913db4 | 18 | 80, //Running current torque in % (from 0 to 100) |
stebonicelli | 25:281c8e913db4 | 19 | 40, //Holding current torque in % (from 0 to 100) |
stebonicelli | 25:281c8e913db4 | 20 | STEP_MODE_1_16, //Step mode via enum motorStepMode_t |
stebonicelli | 25:281c8e913db4 | 21 | FAST_DECAY, //Decay mode via enum motorDecayMode_t |
stebonicelli | 25:281c8e913db4 | 22 | 0, //Dwelling time in ms |
stebonicelli | 25:281c8e913db4 | 23 | FALSE, //Automatic HIZ STOP |
stebonicelli | 25:281c8e913db4 | 24 | 100000 //VREFA and VREFB PWM frequency (Hz) |
stebonicelli | 25:281c8e913db4 | 25 | }; |
nucleosam | 0:36aa6787d4f9 | 26 | |
stebonicelli | 25:281c8e913db4 | 27 | Thread canrxa; |
stebonicelli | 25:281c8e913db4 | 28 | |
stebonicelli | 25:281c8e913db4 | 29 | // Utility |
stebonicelli | 25:281c8e913db4 | 30 | InterruptIn button(USER_BUTTON); |
stebonicelli | 25:281c8e913db4 | 31 | DigitalOut led(LED1); |
stebonicelli | 25:281c8e913db4 | 32 | |
stebonicelli | 25:281c8e913db4 | 33 | // Motor Control |
stebonicelli | 25:281c8e913db4 | 34 | L6208 *motor; |
stebonicelli | 25:281c8e913db4 | 35 | |
stebonicelli | 25:281c8e913db4 | 36 | InterruptIn end0(PC_10, PullUp); |
stebonicelli | 25:281c8e913db4 | 37 | InterruptIn end1(PC_11, PullUp); |
stebonicelli | 25:281c8e913db4 | 38 | InterruptIn enc(PC_12, PullUp); |
stebonicelli | 17:dc1b04f0b55d | 39 | |
stebonicelli | 26:44175c51a820 | 40 | int32_t speed = 0; |
nucleosam | 0:36aa6787d4f9 | 41 | |
stebonicelli | 17:dc1b04f0b55d | 42 | void motor_error_handler(uint16_t error) |
stebonicelli | 17:dc1b04f0b55d | 43 | { |
stebonicelli | 26:44175c51a820 | 44 | printf("ERROR: Motor Runtime\n\r"); |
stebonicelli | 26:44175c51a820 | 45 | while(1){} |
stebonicelli | 17:dc1b04f0b55d | 46 | } |
stebonicelli | 17:dc1b04f0b55d | 47 | |
stebonicelli | 21:533d014f09e0 | 48 | void motor_zero() |
stebonicelli | 21:533d014f09e0 | 49 | { |
stebonicelli | 26:44175c51a820 | 50 | motor->run(StepperMotor::FWD); |
stebonicelli | 21:533d014f09e0 | 51 | } |
stebonicelli | 21:533d014f09e0 | 52 | |
stebonicelli | 25:281c8e913db4 | 53 | void button_int_handler() |
nucleosam | 0:36aa6787d4f9 | 54 | { |
stebonicelli | 25:281c8e913db4 | 55 | printf("POSITION: %d\n\r", motor->get_position()); |
stebonicelli | 21:533d014f09e0 | 56 | motor_zero(); |
stebonicelli | 17:dc1b04f0b55d | 57 | } |
stebonicelli | 17:dc1b04f0b55d | 58 | |
stebonicelli | 25:281c8e913db4 | 59 | void end0_int_handler() |
stebonicelli | 17:dc1b04f0b55d | 60 | { |
stebonicelli | 26:44175c51a820 | 61 | printf("END0: Pressed\n\rPOSITION: %d\n\r", motor->get_position()); |
nucleosam | 0:36aa6787d4f9 | 62 | } |
nucleosam | 0:36aa6787d4f9 | 63 | |
stebonicelli | 17:dc1b04f0b55d | 64 | void end1_int_handler() |
stebonicelli | 17:dc1b04f0b55d | 65 | { |
stebonicelli | 26:44175c51a820 | 66 | motor->hard_stop(); |
stebonicelli | 26:44175c51a820 | 67 | motor->run(StepperMotor::BWD); |
stebonicelli | 17:dc1b04f0b55d | 68 | |
stebonicelli | 26:44175c51a820 | 69 | printf("END1: Pressed\n\r"); |
stebonicelli | 17:dc1b04f0b55d | 70 | } |
stebonicelli | 17:dc1b04f0b55d | 71 | |
stebonicelli | 25:281c8e913db4 | 72 | void motor_set_home() |
stebonicelli | 25:281c8e913db4 | 73 | { |
stebonicelli | 25:281c8e913db4 | 74 | motor->hard_stop(); |
stebonicelli | 25:281c8e913db4 | 75 | motor->set_home(); |
stebonicelli | 25:281c8e913db4 | 76 | motor->go_to(0); |
stebonicelli | 25:281c8e913db4 | 77 | } |
stebonicelli | 17:dc1b04f0b55d | 78 | |
stebonicelli | 25:281c8e913db4 | 79 | // CAN |
gidiana | 23:3110010d98a0 | 80 | CAN can1(PB_12, PB_13); // RX, TX |
stebonicelli | 17:dc1b04f0b55d | 81 | |
stebonicelli | 17:dc1b04f0b55d | 82 | CANMessage messageIn; |
stebonicelli | 17:dc1b04f0b55d | 83 | CANMessage messageOut; |
stebonicelli | 17:dc1b04f0b55d | 84 | |
gidiana | 18:65707db67191 | 85 | void canrx() |
stebonicelli | 17:dc1b04f0b55d | 86 | { |
stebonicelli | 19:9680ebe86f4a | 87 | while(1) |
stebonicelli | 21:533d014f09e0 | 88 | { |
stebonicelli | 26:44175c51a820 | 89 | if(can1.read(messageIn)) |
stebonicelli | 19:9680ebe86f4a | 90 | { |
stebonicelli | 26:44175c51a820 | 91 | if(messageIn.id == ((JOINT_SET_SPEED << 8) + JOINT_ID)) |
stebonicelli | 21:533d014f09e0 | 92 | { |
stebonicelli | 26:44175c51a820 | 93 | speed = 0; |
stebonicelli | 26:44175c51a820 | 94 | speed = (messageIn.data[0] << 24) | (messageIn.data[1] << 16) | (messageIn.data[2] << 8) | (messageIn.data[3]); |
stebonicelli | 26:44175c51a820 | 95 | |
stebonicelli | 26:44175c51a820 | 96 | motor->set_max_speed(speed); |
stebonicelli | 26:44175c51a820 | 97 | (speed > 0) ? motor->run(StepperMotor::BWD) : motor->run(StepperMotor::FWD); |
stebonicelli | 26:44175c51a820 | 98 | |
stebonicelli | 26:44175c51a820 | 99 | printf("CAN: mess %d\n\r", speed); |
stebonicelli | 21:533d014f09e0 | 100 | } |
stebonicelli | 25:281c8e913db4 | 101 | } |
stebonicelli | 26:44175c51a820 | 102 | |
stebonicelli | 26:44175c51a820 | 103 | wait(0.1); |
stebonicelli | 25:281c8e913db4 | 104 | } |
nucleosam | 0:36aa6787d4f9 | 105 | } |
stebonicelli | 25:281c8e913db4 | 106 | |
stebonicelli | 17:dc1b04f0b55d | 107 | |
nucleosam | 0:36aa6787d4f9 | 108 | /* Main ----------------------------------------------------------------------*/ |
nucleosam | 0:36aa6787d4f9 | 109 | |
nucleosam | 0:36aa6787d4f9 | 110 | int main() |
nucleosam | 0:36aa6787d4f9 | 111 | { |
stebonicelli | 26:44175c51a820 | 112 | can1.frequency(500000); |
stebonicelli | 17:dc1b04f0b55d | 113 | |
stebonicelli | 25:281c8e913db4 | 114 | // Motor Initialization |
stebonicelli | 25:281c8e913db4 | 115 | motor = new L6208(D2, D8, D7, D4, D5, D6, VREFA_PWM_PIN, VREFB_PWM_PIN); |
stebonicelli | 25:281c8e913db4 | 116 | |
stebonicelli | 25:281c8e913db4 | 117 | if (motor->init(&init) != COMPONENT_OK) |
stebonicelli | 17:dc1b04f0b55d | 118 | { |
gidiana | 18:65707db67191 | 119 | printf("ERROR: vvMotor Init\n\r"); |
davide.aliprandi@st.com | 5:bc710d77d801 | 120 | exit(EXIT_FAILURE); |
davide.aliprandi@st.com | 5:bc710d77d801 | 121 | } |
nucleosam | 0:36aa6787d4f9 | 122 | |
stebonicelli | 25:281c8e913db4 | 123 | motor->attach_error_handler(&motor_error_handler); |
stebonicelli | 25:281c8e913db4 | 124 | |
stebonicelli | 25:281c8e913db4 | 125 | end0.rise(&end0_int_handler); |
stebonicelli | 25:281c8e913db4 | 126 | end1.rise(&end1_int_handler); |
stebonicelli | 25:281c8e913db4 | 127 | end1.fall(&motor_set_home); |
stebonicelli | 17:dc1b04f0b55d | 128 | |
stebonicelli | 17:dc1b04f0b55d | 129 | button.rise(&button_int_handler); |
stebonicelli | 21:533d014f09e0 | 130 | |
stebonicelli | 25:281c8e913db4 | 131 | motor->set_step_mode(StepperMotor::STEP_MODE_1_16); |
stebonicelli | 19:9680ebe86f4a | 132 | printf("DONE: Motor Init\n\r"); |
nucleosam | 0:36aa6787d4f9 | 133 | |
stebonicelli | 17:dc1b04f0b55d | 134 | // CAN Initialization |
stebonicelli | 21:533d014f09e0 | 135 | |
stebonicelli | 19:9680ebe86f4a | 136 | canrxa.start(canrx); |
stebonicelli | 17:dc1b04f0b55d | 137 | |
stebonicelli | 17:dc1b04f0b55d | 138 | printf("DONE: CAN Init\n\r"); |
stebonicelli | 17:dc1b04f0b55d | 139 | |
stebonicelli | 17:dc1b04f0b55d | 140 | printf("Running!\n\r"); |
stebonicelli | 17:dc1b04f0b55d | 141 | |
stebonicelli | 17:dc1b04f0b55d | 142 | while(true) |
gidiana | 13:08617f604d55 | 143 | { |
stebonicelli | 26:44175c51a820 | 144 | wait(1); |
stebonicelli | 17:dc1b04f0b55d | 145 | } |
nucleosam | 0:36aa6787d4f9 | 146 | } |