Team DIANA / Mbed OS arm_linear_can_2

Linear driver with st driver

Dependencies:   X_NUCLEO_IHM04A1

Dependents:   Basic_DC_Control Basic_DC_Control1 DC_Serial

main.cpp@23:3110010d98a0, 2019-04-06 (annotated)

Committer:
gidiana
Date:
Sat Apr 06 14:51:15 2019 +0000
Revision:
23:3110010d98a0
Parent:
22:137d1ef8d22d
Child:
24:37f139e067b2
fixx

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 21:533d014f09e0 9 8000, //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 21:533d014f09e0 11 8000, //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 21:533d014f09e0 13 8000, //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 };
stebonicelli 21:533d014f09e0 22
gidiana 18:65707db67191 23 Thread canrxa;
stebonicelli 17:dc1b04f0b55d 24
stebonicelli 17:dc1b04f0b55d 25 // Utility
stebonicelli 17:dc1b04f0b55d 26 InterruptIn button(USER_BUTTON);
stebonicelli 17:dc1b04f0b55d 27 DigitalOut led(LED1);
stebonicelli 17:dc1b04f0b55d 28
stebonicelli 17:dc1b04f0b55d 29 // Motor Control
nucleosam 0:36aa6787d4f9 30 L6208 *motor;
stebonicelli 17:dc1b04f0b55d 31
stebonicelli 17:dc1b04f0b55d 32 InterruptIn end0(PC_10, PullUp);
stebonicelli 17:dc1b04f0b55d 33 InterruptIn end1(PC_11, PullUp);
stebonicelli 17:dc1b04f0b55d 34 InterruptIn enc(PC_12, PullUp);
stebonicelli 17:dc1b04f0b55d 35
stebonicelli 17:dc1b04f0b55d 36 int current_pose = 0;
stebonicelli 17:dc1b04f0b55d 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 17:dc1b04f0b55d 42 while(1){}
stebonicelli 17:dc1b04f0b55d 43 }
stebonicelli 17:dc1b04f0b55d 44
stebonicelli 21:533d014f09e0 45 void motor_zero()
stebonicelli 21:533d014f09e0 46 {
stebonicelli 21:533d014f09e0 47 motor->run(StepperMotor::FWD);
stebonicelli 21:533d014f09e0 48 }
stebonicelli 21:533d014f09e0 49
stebonicelli 17:dc1b04f0b55d 50 void button_int_handler()
nucleosam 0:36aa6787d4f9 51 {
stebonicelli 17:dc1b04f0b55d 52 printf("POSITION: %d\n\r", motor->get_position());
stebonicelli 21:533d014f09e0 53 motor_zero();
stebonicelli 17:dc1b04f0b55d 54 }
stebonicelli 17:dc1b04f0b55d 55
stebonicelli 17:dc1b04f0b55d 56 void end0_int_handler()
stebonicelli 17:dc1b04f0b55d 57 {
stebonicelli 17:dc1b04f0b55d 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 17:dc1b04f0b55d 63 motor->hard_stop();
stebonicelli 17:dc1b04f0b55d 64
stebonicelli 17:dc1b04f0b55d 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 17:dc1b04f0b55d 70 void motor_set_home()
stebonicelli 17:dc1b04f0b55d 71 {
stebonicelli 17:dc1b04f0b55d 72 motor->hard_stop();
stebonicelli 17:dc1b04f0b55d 73 motor->set_home();
stebonicelli 17:dc1b04f0b55d 74 motor->go_to(0);
stebonicelli 17:dc1b04f0b55d 75
stebonicelli 17:dc1b04f0b55d 76 current_pose = 0;
stebonicelli 17:dc1b04f0b55d 77 pose = 0;
stebonicelli 17:dc1b04f0b55d 78 }
stebonicelli 17:dc1b04f0b55d 79
stebonicelli 17:dc1b04f0b55d 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 {
gidiana 23:3110010d98a0 92 if(can1.read(messageIn,filter)&&messageIn.id==0x010)
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 21:533d014f09e0 96
gidiana 23:3110010d98a0 97
stebonicelli 22:137d1ef8d22d 98 if (pose == 1)
stebonicelli 21:533d014f09e0 99 {
stebonicelli 21:533d014f09e0 100 motor->run(StepperMotor::FWD);
stebonicelli 21:533d014f09e0 101 }
stebonicelli 22:137d1ef8d22d 102 else if (pose == 0)
stebonicelli 21:533d014f09e0 103 {
stebonicelli 21:533d014f09e0 104 motor->soft_stop();
stebonicelli 21:533d014f09e0 105 current_pose= motor->get_position();
stebonicelli 21:533d014f09e0 106 motor->go_to(current_pose);
stebonicelli 21:533d014f09e0 107 }
stebonicelli 21:533d014f09e0 108 else if (pose == 2)
stebonicelli 21:533d014f09e0 109 {
stebonicelli 21:533d014f09e0 110 motor->run(StepperMotor::BWD);
stebonicelli 21:533d014f09e0 111 }
stebonicelli 21:533d014f09e0 112 else
stebonicelli 21:533d014f09e0 113 {
stebonicelli 21:533d014f09e0 114 motor->soft_stop();
stebonicelli 21:533d014f09e0 115 current_pose= motor->get_position();
stebonicelli 21:533d014f09e0 116 motor->go_to(current_pose);
stebonicelli 21:533d014f09e0 117 }
gidiana 23:3110010d98a0 118
stebonicelli 21:533d014f09e0 119
gidiana 23:3110010d98a0 120 // motor->go_to(pose);
stebonicelli 19:9680ebe86f4a 121 }
stebonicelli 17:dc1b04f0b55d 122 }
nucleosam 0:36aa6787d4f9 123 }
nucleosam 0:36aa6787d4f9 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 21:533d014f09e0 131
stebonicelli 17:dc1b04f0b55d 132 // Motor Initialization
nucleosam 3:a6e155687c6a 133 motor = new L6208(D2, D8, D7, D4, D5, D6, VREFA_PWM_PIN, VREFB_PWM_PIN);
stebonicelli 17:dc1b04f0b55d 134
stebonicelli 17:dc1b04f0b55d 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 17:dc1b04f0b55d 141 motor->attach_error_handler(&motor_error_handler);
stebonicelli 17:dc1b04f0b55d 142
stebonicelli 17:dc1b04f0b55d 143 end0.rise(&end0_int_handler);
stebonicelli 17:dc1b04f0b55d 144 end1.rise(&end1_int_handler);
stebonicelli 17:dc1b04f0b55d 145 end1.fall(&motor_set_home);
stebonicelli 17:dc1b04f0b55d 146
stebonicelli 17:dc1b04f0b55d 147 button.rise(&button_int_handler);
stebonicelli 21:533d014f09e0 148
gidiana 20:8e5dd30b1b59 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 }