Team DIANA / Mbed OS Arm_stepper_can

Stepper controller with can interface

Dependencies:   X-NUCLEO-IHM05A1

main.cpp@29:847123225e37, 2019-06-25 (annotated)

Committer:
stebonicelli
Date:
Tue Jun 25 09:02:36 2019 +0000
Revision:
29:847123225e37
Parent:
28:8878dd50b7e1 
da 16 a 8;

Who changed what in which revision?

UserRevisionLine numberNew 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
gidiana 27:275ba9c137c9 9 #define JOINT_ID 2
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
gidiana 27:275ba9c137c9 36 InterruptIn end0(PC_4, PullUp);
gidiana 27:275ba9c137c9 37 InterruptIn end1(PC_8, 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 28:8878dd50b7e1 50 motor->run(StepperMotor::BWD);
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 28:8878dd50b7e1 59 void end0_pressed()
stebonicelli 17:dc1b04f0b55d 60 {
gidiana 27:275ba9c137c9 61 motor->run(StepperMotor::BWD);
stebonicelli 26:44175c51a820 62 printf("END0: Pressed\n\rPOSITION: %d\n\r", motor->get_position());
nucleosam 0:36aa6787d4f9 63 }
nucleosam 0:36aa6787d4f9 64
stebonicelli 28:8878dd50b7e1 65 void end0_released()
stebonicelli 28:8878dd50b7e1 66 {
stebonicelli 28:8878dd50b7e1 67 motor->go_to(motor->get_position());
stebonicelli 28:8878dd50b7e1 68 }
stebonicelli 28:8878dd50b7e1 69
stebonicelli 28:8878dd50b7e1 70 void end1_pressed()
stebonicelli 17:dc1b04f0b55d 71 {
stebonicelli 26:44175c51a820 72 motor->hard_stop();
gidiana 27:275ba9c137c9 73 motor->run(StepperMotor::FWD);
stebonicelli 17:dc1b04f0b55d 74
stebonicelli 26:44175c51a820 75 printf("END1: Pressed\n\r");
stebonicelli 17:dc1b04f0b55d 76 }
stebonicelli 17:dc1b04f0b55d 77
stebonicelli 25:281c8e913db4 78 void motor_set_home()
stebonicelli 25:281c8e913db4 79 {
stebonicelli 25:281c8e913db4 80 motor->hard_stop();
stebonicelli 25:281c8e913db4 81 motor->set_home();
stebonicelli 25:281c8e913db4 82 motor->go_to(0);
stebonicelli 25:281c8e913db4 83 }
stebonicelli 17:dc1b04f0b55d 84
stebonicelli 25:281c8e913db4 85 // CAN
gidiana 27:275ba9c137c9 86 CAN can1(PB_8, PB_9); // RX, TX
stebonicelli 17:dc1b04f0b55d 87
stebonicelli 17:dc1b04f0b55d 88 CANMessage messageIn;
stebonicelli 17:dc1b04f0b55d 89 CANMessage messageOut;
stebonicelli 17:dc1b04f0b55d 90
gidiana 18:65707db67191 91 void canrx()
stebonicelli 17:dc1b04f0b55d 92 {
stebonicelli 19:9680ebe86f4a 93 while(1)
stebonicelli 21:533d014f09e0 94 {
stebonicelli 26:44175c51a820 95 if(can1.read(messageIn))
stebonicelli 19:9680ebe86f4a 96 {
stebonicelli 28:8878dd50b7e1 97 printf("CAN: message received\r\n");
stebonicelli 26:44175c51a820 98 if(messageIn.id == ((JOINT_SET_SPEED << 8) + JOINT_ID))
stebonicelli 21:533d014f09e0 99 {
stebonicelli 26:44175c51a820 100 speed = 0;
stebonicelli 26:44175c51a820 101 speed = (messageIn.data[0] << 24) | (messageIn.data[1] << 16) | (messageIn.data[2] << 8) | (messageIn.data[3]);
stebonicelli 26:44175c51a820 102
stebonicelli 26:44175c51a820 103 motor->set_max_speed(speed);
stebonicelli 26:44175c51a820 104 (speed > 0) ? motor->run(StepperMotor::BWD) : motor->run(StepperMotor::FWD);
stebonicelli 26:44175c51a820 105
stebonicelli 28:8878dd50b7e1 106 printf("CAN: setting speed -> %d\n\r", speed);
stebonicelli 21:533d014f09e0 107 }
stebonicelli 25:281c8e913db4 108 }
stebonicelli 26:44175c51a820 109
stebonicelli 28:8878dd50b7e1 110 wait(0.05);
stebonicelli 25:281c8e913db4 111 }
nucleosam 0:36aa6787d4f9 112 }
stebonicelli 25:281c8e913db4 113
stebonicelli 17:dc1b04f0b55d 114
nucleosam 0:36aa6787d4f9 115 /* Main ----------------------------------------------------------------------*/
nucleosam 0:36aa6787d4f9 116
nucleosam 0:36aa6787d4f9 117 int main()
nucleosam 0:36aa6787d4f9 118 {
gidiana 27:275ba9c137c9 119 can1.frequency(125000);
gidiana 27:275ba9c137c9 120 messageIn.format=CANExtended;
stebonicelli 17:dc1b04f0b55d 121
stebonicelli 25:281c8e913db4 122 // Motor Initialization
stebonicelli 25:281c8e913db4 123 motor = new L6208(D2, D8, D7, D4, D5, D6, VREFA_PWM_PIN, VREFB_PWM_PIN);
stebonicelli 25:281c8e913db4 124
stebonicelli 25:281c8e913db4 125 if (motor->init(&init) != COMPONENT_OK)
stebonicelli 17:dc1b04f0b55d 126 {
gidiana 18:65707db67191 127 printf("ERROR: vvMotor Init\n\r");
davide.aliprandi@st.com 5:bc710d77d801 128 exit(EXIT_FAILURE);
davide.aliprandi@st.com 5:bc710d77d801 129 }
nucleosam 0:36aa6787d4f9 130
stebonicelli 25:281c8e913db4 131 motor->attach_error_handler(&motor_error_handler);
stebonicelli 25:281c8e913db4 132
stebonicelli 28:8878dd50b7e1 133 // Limit EndStop
stebonicelli 28:8878dd50b7e1 134 end0.rise(&end0_pressed);
stebonicelli 28:8878dd50b7e1 135 end0.fall(&end0_released);
stebonicelli 28:8878dd50b7e1 136 // Zero EndStop
stebonicelli 28:8878dd50b7e1 137 end1.rise(&end1_pressed);
stebonicelli 25:281c8e913db4 138 end1.fall(&motor_set_home);
stebonicelli 17:dc1b04f0b55d 139
stebonicelli 17:dc1b04f0b55d 140 button.rise(&button_int_handler);
stebonicelli 21:533d014f09e0 141
stebonicelli 29:847123225e37 142 motor->set_step_mode(StepperMotor::STEP_MODE_1_8);
stebonicelli 28:8878dd50b7e1 143
stebonicelli 28:8878dd50b7e1 144 motor_set_home();
stebonicelli 28:8878dd50b7e1 145
stebonicelli 19:9680ebe86f4a 146 printf("DONE: Motor Init\n\r");
nucleosam 0:36aa6787d4f9 147
stebonicelli 17:dc1b04f0b55d 148 // CAN Initialization
stebonicelli 21:533d014f09e0 149
stebonicelli 19:9680ebe86f4a 150 canrxa.start(canrx);
stebonicelli 17:dc1b04f0b55d 151
stebonicelli 17:dc1b04f0b55d 152 printf("DONE: CAN Init\n\r");
stebonicelli 17:dc1b04f0b55d 153
stebonicelli 17:dc1b04f0b55d 154 printf("Running!\n\r");
stebonicelli 17:dc1b04f0b55d 155
gidiana 27:275ba9c137c9 156 // DEBUG
gidiana 27:275ba9c137c9 157 //motor->set_max_speed(8000);
gidiana 27:275ba9c137c9 158 //motor->run(StepperMotor::FWD);
gidiana 27:275ba9c137c9 159
stebonicelli 17:dc1b04f0b55d 160 while(true)
gidiana 13:08617f604d55 161 {
stebonicelli 26:44175c51a820 162 wait(1);
stebonicelli 17:dc1b04f0b55d 163 }
nucleosam 0:36aa6787d4f9 164 }