Team DIANA / Mbed OS Arm_stepper_BASE

BASE

Dependencies:   X-NUCLEO-IHM05A1

main.cpp@27:275ba9c137c9, 2019-06-21 (annotated)

Committer:
gidiana
Date:
Fri Jun 21 07:44:12 2019 +0000
Revision:
27:275ba9c137c9
Parent:
26:44175c51a820
Child:
28:8878dd50b7e1
Paolone;

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