Team DIANA / Mbed OS arm_dcmotor_can

__

Dependencies:   X_NUCLEO_IHM04A1

main.cpp@20:8e5dd30b1b59, 2019-02-08 (annotated)

Committer:
gidiana
Date:
Fri Feb 08 19:48:07 2019 +0000
Revision:
20:8e5dd30b1b59
Parent:
19:9680ebe86f4a
Child:
21:533d014f09e0
speed mode

Who changed what in which revision?

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