Team DIANA / Mbed OS Arm_stepper_DEVEL

a version to test

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?

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
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 }