Team DIANA / Mbed OS Arm_stepper_can

Stepper controller with can interface

Dependencies:   X-NUCLEO-IHM05A1

main.cpp@25:281c8e913db4, 2019-06-12 (annotated)

Committer:
stebonicelli
Date:
Wed Jun 12 16:10:21 2019 +0000
Revision:
25:281c8e913db4
Parent:
24:37f139e067b2
Child:
26:44175c51a820
reverted

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 25:281c8e913db4 7 l6208_init_t init =
stebonicelli 25:281c8e913db4 8 {
stebonicelli 25:281c8e913db4 9 8000, //Acceleration rate in step/s^2 or (1/16)th step/s^2 for microstep modes
stebonicelli 25:281c8e913db4 10 80, //Acceleration current torque in % (from 0 to 100)
stebonicelli 25:281c8e913db4 11 8000, //Deceleration rate in step/s^2 or (1/16)th step/s^2 for microstep modes
stebonicelli 25:281c8e913db4 12 80, //Deceleration current torque in % (from 0 to 100)
stebonicelli 25:281c8e913db4 13 8000, //Running speed in step/s or (1/16)th step/s for microstep modes
stebonicelli 25:281c8e913db4 14 80, //Running current torque in % (from 0 to 100)
stebonicelli 25:281c8e913db4 15 40, //Holding current torque in % (from 0 to 100)
stebonicelli 25:281c8e913db4 16 STEP_MODE_1_16, //Step mode via enum motorStepMode_t
stebonicelli 25:281c8e913db4 17 FAST_DECAY, //Decay mode via enum motorDecayMode_t
stebonicelli 25:281c8e913db4 18 0, //Dwelling time in ms
stebonicelli 25:281c8e913db4 19 FALSE, //Automatic HIZ STOP
stebonicelli 25:281c8e913db4 20 100000 //VREFA and VREFB PWM frequency (Hz)
stebonicelli 25:281c8e913db4 21 };
nucleosam 0:36aa6787d4f9 22
stebonicelli 25:281c8e913db4 23 Thread canrxa;
stebonicelli 25:281c8e913db4 24
stebonicelli 25:281c8e913db4 25 // Utility
stebonicelli 25:281c8e913db4 26 InterruptIn button(USER_BUTTON);
stebonicelli 25:281c8e913db4 27 DigitalOut led(LED1);
stebonicelli 25:281c8e913db4 28
stebonicelli 25:281c8e913db4 29 // Motor Control
stebonicelli 25:281c8e913db4 30 L6208 *motor;
stebonicelli 25:281c8e913db4 31
stebonicelli 25:281c8e913db4 32 InterruptIn end0(PC_10, PullUp);
stebonicelli 25:281c8e913db4 33 InterruptIn end1(PC_11, PullUp);
stebonicelli 25:281c8e913db4 34 InterruptIn enc(PC_12, PullUp);
stebonicelli 17:dc1b04f0b55d 35
danielfpq 24:37f139e067b2 36 int current_pose = 0;
stebonicelli 25:281c8e913db4 37 int pose = 0;
nucleosam 0:36aa6787d4f9 38
stebonicelli 17:dc1b04f0b55d 39 void motor_error_handler(uint16_t error)
stebonicelli 17:dc1b04f0b55d 40 {
stebonicelli 17:dc1b04f0b55d 41 printf("ERROR: Motor Runtime\n\r");
stebonicelli 25:281c8e913db4 42 while(1){}
stebonicelli 17:dc1b04f0b55d 43 }
stebonicelli 17:dc1b04f0b55d 44
stebonicelli 21:533d014f09e0 45 void motor_zero()
stebonicelli 21:533d014f09e0 46 {
stebonicelli 25:281c8e913db4 47 motor->run(StepperMotor::FWD);
stebonicelli 21:533d014f09e0 48 }
stebonicelli 21:533d014f09e0 49
stebonicelli 25:281c8e913db4 50 void button_int_handler()
nucleosam 0:36aa6787d4f9 51 {
stebonicelli 25:281c8e913db4 52 printf("POSITION: %d\n\r", motor->get_position());
stebonicelli 21:533d014f09e0 53 motor_zero();
stebonicelli 17:dc1b04f0b55d 54 }
stebonicelli 17:dc1b04f0b55d 55
stebonicelli 25:281c8e913db4 56 void end0_int_handler()
stebonicelli 17:dc1b04f0b55d 57 {
stebonicelli 25:281c8e913db4 58 printf("END0: Pressed\n\rPOSITION: %d\n\r", motor->get_position());
nucleosam 0:36aa6787d4f9 59 }
nucleosam 0:36aa6787d4f9 60
stebonicelli 17:dc1b04f0b55d 61 void end1_int_handler()
stebonicelli 17:dc1b04f0b55d 62 {
stebonicelli 25:281c8e913db4 63 motor->hard_stop();
stebonicelli 17:dc1b04f0b55d 64
stebonicelli 25:281c8e913db4 65 motor->run(StepperMotor::BWD);
stebonicelli 17:dc1b04f0b55d 66
stebonicelli 17:dc1b04f0b55d 67 printf("END1: Pressed\n\r");
stebonicelli 17:dc1b04f0b55d 68 }
stebonicelli 17:dc1b04f0b55d 69
stebonicelli 25:281c8e913db4 70 void motor_set_home()
stebonicelli 25:281c8e913db4 71 {
stebonicelli 25:281c8e913db4 72 motor->hard_stop();
stebonicelli 25:281c8e913db4 73 motor->set_home();
stebonicelli 25:281c8e913db4 74 motor->go_to(0);
stebonicelli 25:281c8e913db4 75
stebonicelli 25:281c8e913db4 76 current_pose = 0;
stebonicelli 25:281c8e913db4 77 pose = 0;
stebonicelli 25:281c8e913db4 78 }
stebonicelli 17:dc1b04f0b55d 79
stebonicelli 25:281c8e913db4 80 // CAN
gidiana 23:3110010d98a0 81 CAN can1(PB_12, PB_13); // RX, TX
stebonicelli 17:dc1b04f0b55d 82
stebonicelli 17:dc1b04f0b55d 83 CANMessage messageIn;
stebonicelli 17:dc1b04f0b55d 84 CANMessage messageOut;
stebonicelli 17:dc1b04f0b55d 85
gidiana 23:3110010d98a0 86 int filter = can1.filter(0x010, 0x4FF, CANStandard);
stebonicelli 17:dc1b04f0b55d 87
gidiana 18:65707db67191 88 void canrx()
stebonicelli 17:dc1b04f0b55d 89 {
stebonicelli 19:9680ebe86f4a 90 while(1)
stebonicelli 21:533d014f09e0 91 {
stebonicelli 25:281c8e913db4 92 if(can1.read(messageIn,filter)&&messageIn.id==0x010)
stebonicelli 19:9680ebe86f4a 93 {
stebonicelli 25:281c8e913db4 94 pose=messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24);
stebonicelli 25:281c8e913db4 95 printf("CAN: mess %d\n\r", pose);
stebonicelli 21:533d014f09e0 96
stebonicelli 25:281c8e913db4 97
stebonicelli 25:281c8e913db4 98 if (pose == 1)
stebonicelli 21:533d014f09e0 99 {
stebonicelli 25:281c8e913db4 100 motor->run(StepperMotor::FWD);
stebonicelli 21:533d014f09e0 101 }
stebonicelli 25:281c8e913db4 102 else if (pose == 0)
stebonicelli 21:533d014f09e0 103 {
stebonicelli 25:281c8e913db4 104 motor->soft_stop();
stebonicelli 25:281c8e913db4 105 current_pose= motor->get_position();
stebonicelli 25:281c8e913db4 106 motor->go_to(current_pose);
stebonicelli 25:281c8e913db4 107 }
stebonicelli 25:281c8e913db4 108 else if (pose == 2)
stebonicelli 25:281c8e913db4 109 {
stebonicelli 25:281c8e913db4 110 motor->run(StepperMotor::BWD);
stebonicelli 21:533d014f09e0 111 }
stebonicelli 21:533d014f09e0 112 else
stebonicelli 21:533d014f09e0 113 {
stebonicelli 25:281c8e913db4 114 motor->soft_stop();
stebonicelli 25:281c8e913db4 115 current_pose= motor->get_position();
stebonicelli 25:281c8e913db4 116 motor->go_to(current_pose);
danielfpq 24:37f139e067b2 117 }
stebonicelli 25:281c8e913db4 118
stebonicelli 25:281c8e913db4 119
stebonicelli 25:281c8e913db4 120 // motor->go_to(pose);
stebonicelli 25:281c8e913db4 121 }
stebonicelli 25:281c8e913db4 122 }
nucleosam 0:36aa6787d4f9 123 }
stebonicelli 25:281c8e913db4 124
stebonicelli 17:dc1b04f0b55d 125
nucleosam 0:36aa6787d4f9 126 /* Main ----------------------------------------------------------------------*/
nucleosam 0:36aa6787d4f9 127
nucleosam 0:36aa6787d4f9 128 int main()
nucleosam 0:36aa6787d4f9 129 {
stebonicelli 21:533d014f09e0 130 can1.frequency(125000);
stebonicelli 17:dc1b04f0b55d 131
stebonicelli 25:281c8e913db4 132 // Motor Initialization
stebonicelli 25:281c8e913db4 133 motor = new L6208(D2, D8, D7, D4, D5, D6, VREFA_PWM_PIN, VREFB_PWM_PIN);
stebonicelli 25:281c8e913db4 134
stebonicelli 25:281c8e913db4 135 if (motor->init(&init) != COMPONENT_OK)
stebonicelli 17:dc1b04f0b55d 136 {
gidiana 18:65707db67191 137 printf("ERROR: vvMotor Init\n\r");
davide.aliprandi@st.com 5:bc710d77d801 138 exit(EXIT_FAILURE);
davide.aliprandi@st.com 5:bc710d77d801 139 }
nucleosam 0:36aa6787d4f9 140
stebonicelli 25:281c8e913db4 141 motor->attach_error_handler(&motor_error_handler);
stebonicelli 25:281c8e913db4 142
stebonicelli 25:281c8e913db4 143 end0.rise(&end0_int_handler);
stebonicelli 25:281c8e913db4 144 end1.rise(&end1_int_handler);
stebonicelli 25:281c8e913db4 145 end1.fall(&motor_set_home);
stebonicelli 17:dc1b04f0b55d 146
stebonicelli 17:dc1b04f0b55d 147 button.rise(&button_int_handler);
stebonicelli 21:533d014f09e0 148
stebonicelli 25:281c8e913db4 149 motor->set_step_mode(StepperMotor::STEP_MODE_1_16);
stebonicelli 19:9680ebe86f4a 150 printf("DONE: Motor Init\n\r");
nucleosam 0:36aa6787d4f9 151
stebonicelli 17:dc1b04f0b55d 152 // CAN Initialization
stebonicelli 21:533d014f09e0 153
stebonicelli 19:9680ebe86f4a 154 canrxa.start(canrx);
stebonicelli 17:dc1b04f0b55d 155
stebonicelli 17:dc1b04f0b55d 156 printf("DONE: CAN Init\n\r");
stebonicelli 17:dc1b04f0b55d 157
gidiana 20:8e5dd30b1b59 158
stebonicelli 17:dc1b04f0b55d 159
stebonicelli 17:dc1b04f0b55d 160 printf("Running!\n\r");
stebonicelli 17:dc1b04f0b55d 161
stebonicelli 17:dc1b04f0b55d 162 while(true)
gidiana 13:08617f604d55 163 {
stebonicelli 17:dc1b04f0b55d 164 wait(1000);
stebonicelli 17:dc1b04f0b55d 165 }
nucleosam 0:36aa6787d4f9 166 }