Team DIANA / Mbed OS Arm_stepper_SHOULDER

SHOULDER

Dependencies:   X-NUCLEO-IHM05A1

main.cpp@16:1dc772fba09f, 2018-12-07 (annotated)

Committer:
gidiana
Date:
Fri Dec 07 17:40:15 2018 +0000
Revision:
16:1dc772fba09f
Parent:
15:c781dda7e44c
Child:
17:dc1b04f0b55d
end stop

Who changed what in which revision?

UserRevisionLine numberNew contents of line
nucleosam 0:36aa6787d4f9 1
nucleosam 0:36aa6787d4f9 2 /* Includes ------------------------------------------------------------------*/
nucleosam 0:36aa6787d4f9 3 /* mbed specific header files. */
nucleosam 0:36aa6787d4f9 4 #include "mbed.h"
nucleosam 0:36aa6787d4f9 5 /* Component specific header files. */
davide.aliprandi@st.com 5:bc710d77d801 6 #include "L6208.h"
nucleosam 0:36aa6787d4f9 7 /* Definitions ---------------------------------------------------------------*/
nucleosam 0:36aa6787d4f9 8 #define VREFA_PWM_PIN D3
nucleosam 3:a6e155687c6a 9 #define VREFB_PWM_PIN D9
nucleosam 0:36aa6787d4f9 10 /* Variables -----------------------------------------------------------------*/
nucleosam 0:36aa6787d4f9 11 /* Initialization parameters of the motor connected to the expansion board. */
davide.aliprandi@st.com 5:bc710d77d801 12 l6208_init_t init =
nucleosam 0:36aa6787d4f9 13 {
gidiana 15:c781dda7e44c 14 1500, //Acceleration rate in step/s^2 or (1/16)th step/s^2 for microstep modes
gidiana 13:08617f604d55 15 80, //Acceleration current torque in % (from 0 to 100)
gidiana 15:c781dda7e44c 16 1500, //Deceleration rate in step/s^2 or (1/16)th step/s^2 for microstep modes
gidiana 13:08617f604d55 17 80, //Deceleration current torque in % (from 0 to 100)
gidiana 13:08617f604d55 18 1000, //Running speed in step/s or (1/16)th step/s for microstep modes
gidiana 15:c781dda7e44c 19 80, //Running current torque in % (from 0 to 100)
gidiana 15:c781dda7e44c 20 40, //Holding current torque in % (from 0 to 100)
nucleosam 0:36aa6787d4f9 21 STEP_MODE_1_16, //Step mode via enum motorStepMode_t
nucleosam 0:36aa6787d4f9 22 FAST_DECAY, //Decay mode via enum motorDecayMode_t
nucleosam 0:36aa6787d4f9 23 0, //Dwelling time in ms
nucleosam 0:36aa6787d4f9 24 FALSE, //Automatic HIZ STOP
nucleosam 0:36aa6787d4f9 25 100000 //VREFA and VREFB PWM frequency (Hz)
nucleosam 0:36aa6787d4f9 26 };
nucleosam 0:36aa6787d4f9 27
nucleosam 0:36aa6787d4f9 28 /* Motor Control Component. */
nucleosam 0:36aa6787d4f9 29 L6208 *motor;
gidiana 14:1b70207bf922 30 DigitalIn button (USER_BUTTON);
gidiana 16:1dc772fba09f 31 DigitalIn end(PC_10);
gidiana 16:1dc772fba09f 32 DigitalOut led(LED1);
nucleosam 0:36aa6787d4f9 33 /* Functions -----------------------------------------------------------------*/
nucleosam 0:36aa6787d4f9 34
nucleosam 0:36aa6787d4f9 35 /**
nucleosam 0:36aa6787d4f9 36 * @brief This is an example of user handler for the flag interrupt.
nucleosam 0:36aa6787d4f9 37 * @param None
nucleosam 0:36aa6787d4f9 38 * @retval None
nucleosam 0:36aa6787d4f9 39 * @note If needed, implement it, and then attach and enable it:
davide.aliprandi@st.com 5:bc710d77d801 40 * + motor->attach_flag_irq(&my_flag_irq_handler);
davide.aliprandi@st.com 5:bc710d77d801 41 * + motor->enable_flag_irq();
nucleosam 0:36aa6787d4f9 42 * To disable it:
nucleosam 0:36aa6787d4f9 43 * + motor->DisbleFlagIRQ();
nucleosam 0:36aa6787d4f9 44 */
davide.aliprandi@st.com 5:bc710d77d801 45 void my_flag_irq_handler(void)
nucleosam 0:36aa6787d4f9 46 {
nucleosam 0:36aa6787d4f9 47 printf(" WARNING: \"FLAG\" interrupt triggered:\r\n");
Davidroid 6:6fb625ef867a 48 motor->disable();
nucleosam 0:36aa6787d4f9 49 printf(" Motor disabled.\r\n\n");
nucleosam 0:36aa6787d4f9 50 }
nucleosam 0:36aa6787d4f9 51
nucleosam 0:36aa6787d4f9 52 /**
nucleosam 0:36aa6787d4f9 53 * @brief This is an example of error handler.
nucleosam 0:36aa6787d4f9 54 * @param[in] error Number of the error
nucleosam 0:36aa6787d4f9 55 * @retval None
nucleosam 0:36aa6787d4f9 56 * @note If needed, implement it, and then attach it:
davide.aliprandi@st.com 5:bc710d77d801 57 * + motor->attach_error_handler(&my_error_handler);
nucleosam 0:36aa6787d4f9 58 */
davide.aliprandi@st.com 5:bc710d77d801 59 void my_error_handler(uint16_t error)
nucleosam 0:36aa6787d4f9 60 {
nucleosam 0:36aa6787d4f9 61 /* Printing to the console. */
nucleosam 0:36aa6787d4f9 62 printf("Error %d detected\r\n\n", error);
nucleosam 0:36aa6787d4f9 63
nucleosam 0:36aa6787d4f9 64 /* Infinite loop */
davide.aliprandi@st.com 5:bc710d77d801 65 while (true) {
nucleosam 0:36aa6787d4f9 66 }
nucleosam 0:36aa6787d4f9 67 }
nucleosam 0:36aa6787d4f9 68
nucleosam 0:36aa6787d4f9 69 /* Main ----------------------------------------------------------------------*/
nucleosam 0:36aa6787d4f9 70
nucleosam 0:36aa6787d4f9 71 int main()
nucleosam 0:36aa6787d4f9 72 {
gidiana 9:16f134d983bb 73
nucleosam 0:36aa6787d4f9 74 //----- Initialization
nucleosam 0:36aa6787d4f9 75 /* Initializing Motor Control Component. */
nucleosam 3:a6e155687c6a 76 motor = new L6208(D2, D8, D7, D4, D5, D6, VREFA_PWM_PIN, VREFB_PWM_PIN);
davide.aliprandi@st.com 5:bc710d77d801 77 if (motor->init(&init) != COMPONENT_OK) {
gidiana 15:c781dda7e44c 78 printf("error init\n\r");
davide.aliprandi@st.com 5:bc710d77d801 79 exit(EXIT_FAILURE);
davide.aliprandi@st.com 5:bc710d77d801 80 }
nucleosam 0:36aa6787d4f9 81
nucleosam 0:36aa6787d4f9 82 /* Attaching and enabling an interrupt handler. */
davide.aliprandi@st.com 5:bc710d77d801 83 motor->attach_flag_irq(&my_flag_irq_handler);
davide.aliprandi@st.com 5:bc710d77d801 84 motor->enable_flag_irq();
nucleosam 0:36aa6787d4f9 85
nucleosam 0:36aa6787d4f9 86 /* Attaching an error handler */
davide.aliprandi@st.com 5:bc710d77d801 87 motor->attach_error_handler(&my_error_handler);
gidiana 13:08617f604d55 88
gidiana 16:1dc772fba09f 89 motor->set_step_mode(StepperMotor::STEP_MODE_1_8);
nucleosam 0:36aa6787d4f9 90
nucleosam 0:36aa6787d4f9 91 /* Set speed, acceleration and deceleration to scale with microstep mode */
gidiana 15:c781dda7e44c 92 //motor->set_max_speed(2000);
gidiana 15:c781dda7e44c 93 //motor->set_acceleration(2400);
gidiana 15:c781dda7e44c 94 //motor->set_deceleration(2400);
gidiana 15:c781dda7e44c 95 motor->set_max_speed(motor->get_max_speed()<<2);
gidiana 15:c781dda7e44c 96 motor->set_acceleration(motor->get_acceleration()<<2);
gidiana 15:c781dda7e44c 97 motor->set_deceleration(motor->get_deceleration()<<2);
gidiana 16:1dc772fba09f 98 printf(" init\n\r");
gidiana 16:1dc772fba09f 99 end.mode(PullUp);
gidiana 16:1dc772fba09f 100 /* Request to go position 9000 (quarter steps) */
gidiana 16:1dc772fba09f 101 motor->run(StepperMotor::BWD);
gidiana 16:1dc772fba09f 102 while(1)
gidiana 16:1dc772fba09f 103 {
gidiana 16:1dc772fba09f 104 // motor->wait_while_active();
gidiana 16:1dc772fba09f 105 printf("while");
gidiana 16:1dc772fba09f 106 if(end) break;
gidiana 16:1dc772fba09f 107 }
gidiana 16:1dc772fba09f 108 motor->hard_stop();
gidiana 16:1dc772fba09f 109 led=1;
nucleosam 0:36aa6787d4f9 110
gidiana 16:1dc772fba09f 111 int pos=motor->get_position();
gidiana 16:1dc772fba09f 112 printf("pos:%d\n\r", pos);
gidiana 16:1dc772fba09f 113 motor->go_to(pos+200);
gidiana 16:1dc772fba09f 114 motor->wait_while_active();
gidiana 16:1dc772fba09f 115 led=0;
gidiana 16:1dc772fba09f 116 wait(0.5);
gidiana 16:1dc772fba09f 117
gidiana 16:1dc772fba09f 118 motor->set_home();
gidiana 16:1dc772fba09f 119 wait(1);
gidiana 16:1dc772fba09f 120
gidiana 16:1dc772fba09f 121 motor->go_to(1000<<4);
gidiana 16:1dc772fba09f 122 motor->wait_while_active();
gidiana 16:1dc772fba09f 123
gidiana 13:08617f604d55 124 while (true)
gidiana 13:08617f604d55 125 {
gidiana 16:1dc772fba09f 126 motor->go_to(500<<4);
davide.aliprandi@st.com 5:bc710d77d801 127 motor->wait_while_active();
gidiana 13:08617f604d55 128
gidiana 13:08617f604d55 129 motor->go_to(0);
gidiana 13:08617f604d55 130 motor->wait_while_active();
gidiana 13:08617f604d55 131 while (button)
gidiana 13:08617f604d55 132 {
gidiana 16:1dc772fba09f 133
gidiana 13:08617f604d55 134 }
gidiana 13:08617f604d55 135 }
gidiana 13:08617f604d55 136
gidiana 13:08617f604d55 137
nucleosam 0:36aa6787d4f9 138 /* Waiting while the motor is active. */
davide.aliprandi@st.com 5:bc710d77d801 139 motor->wait_while_active();
gidiana 13:08617f604d55 140
gidiana 9:16f134d983bb 141 wait_ms(5000);
gidiana 9:16f134d983bb 142 motor->disable();
nucleosam 0:36aa6787d4f9 143 }
davide.aliprandi@st.com 5:bc710d77d801 144
gidiana 9:16f134d983bb 145