Team DIANA / Mbed 2 deprecated Arm_stepper_BASE_serial

hbjhjb

Dependencies:   mbed X-NUCLEO-IHM05A1

main.cpp@33:c1cefad6d338, 2019-09-15 (annotated)

Committer:
gidiana
Date:
Sun Sep 15 08:47:18 2019 +0000
Revision:
33:c1cefad6d338
Parent:
32:465e41868fe4 
aaa

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"
gidiana 33:c1cefad6d338 3
stebonicelli 25:281c8e913db4 4 #define VREFA_PWM_PIN D3
stebonicelli 25:281c8e913db4 5 #define VREFB_PWM_PIN D9
gidiana 33:c1cefad6d338 6 #define BAUDRATE 9600
gidiana 33:c1cefad6d338 7 #define JOINT 2
stebonicelli 25:281c8e913db4 8 l6208_init_t init =
stebonicelli 25:281c8e913db4 9 {
stebonicelli 25:281c8e913db4 10 8000, //Acceleration rate in step/s^2 or (1/16)th step/s^2 for microstep modes
stebonicelli 25:281c8e913db4 11 80, //Acceleration current torque in % (from 0 to 100)
stebonicelli 25:281c8e913db4 12 8000, //Deceleration rate in step/s^2 or (1/16)th step/s^2 for microstep modes
stebonicelli 25:281c8e913db4 13 80, //Deceleration current torque in % (from 0 to 100)
stebonicelli 25:281c8e913db4 14 8000, //Running speed in step/s or (1/16)th step/s for microstep modes
stebonicelli 25:281c8e913db4 15 80, //Running current torque in % (from 0 to 100)
stebonicelli 25:281c8e913db4 16 40, //Holding current torque in % (from 0 to 100)
stebonicelli 25:281c8e913db4 17 STEP_MODE_1_16, //Step mode via enum motorStepMode_t
stebonicelli 25:281c8e913db4 18 FAST_DECAY, //Decay mode via enum motorDecayMode_t
stebonicelli 25:281c8e913db4 19 0, //Dwelling time in ms
stebonicelli 25:281c8e913db4 20 FALSE, //Automatic HIZ STOP
stebonicelli 25:281c8e913db4 21 100000 //VREFA and VREFB PWM frequency (Hz)
stebonicelli 25:281c8e913db4 22 };
gidiana 33:c1cefad6d338 23
gidiana 33:c1cefad6d338 24
stebonicelli 25:281c8e913db4 25 // Utility
gidiana 30:c40b060795a2 26 //InterruptIn button(USER_BUTTON);
stebonicelli 25:281c8e913db4 27 DigitalOut led(LED1);
gidiana 33:c1cefad6d338 28
stebonicelli 25:281c8e913db4 29 // Motor Control
stebonicelli 25:281c8e913db4 30 L6208 *motor;
gidiana 33:c1cefad6d338 31
gidiana 30:c40b060795a2 32 InterruptIn end1(USER_BUTTON, PullUp);
gidiana 30:c40b060795a2 33 DigitalIn end0(PA_5, PullUp);
gidiana 33:c1cefad6d338 34 Serial serial(PA_2, PA_3);
gidiana 33:c1cefad6d338 35
gidiana 33:c1cefad6d338 36
gidiana 33:c1cefad6d338 37
gidiana 30:c40b060795a2 38 float pose, current_pose;
gidiana 30:c40b060795a2 39 float speed, current_speed;
gidiana 30:c40b060795a2 40 void zero()
gidiana 30:c40b060795a2 41 {
gidiana 33:c1cefad6d338 42 printf("zero");
gidiana 33:c1cefad6d338 43 motor->run(StepperMotor::BWD);
gidiana 33:c1cefad6d338 44 while(!end0){
gidiana 33:c1cefad6d338 45 }
gidiana 33:c1cefad6d338 46 motor->hard_stop();
gidiana 33:c1cefad6d338 47 motor->set_home();
gidiana 33:c1cefad6d338 48 motor->go_to(0);
gidiana 33:c1cefad6d338 49 printf("END0: Pressed\n\rPOSITION: %d\n\r", motor->get_position());
gidiana 30:c40b060795a2 50 }
gidiana 33:c1cefad6d338 51
gidiana 33:c1cefad6d338 52 void motor_error_handler(uint16_t error)
stebonicelli 17:dc1b04f0b55d 53 {
gidiana 29:f888a2394027 54 printf("ERROR: Motor Runtime\n\r");
gidiana 33:c1cefad6d338 55
nucleosam 0:36aa6787d4f9 56 }
gidiana 33:c1cefad6d338 57
gidiana 29:f888a2394027 58 void end1_int_handler()
stebonicelli 17:dc1b04f0b55d 59 {
gidiana 33:c1cefad6d338 60 // motor->hard_stop();
gidiana 33:c1cefad6d338 61
gidiana 30:c40b060795a2 62 motor->run(StepperMotor::FWD);
gidiana 33:c1cefad6d338 63
gidiana 29:f888a2394027 64 printf("END1: Pressed\n\rPOSITION: %d\n\r", motor->get_position());
stebonicelli 17:dc1b04f0b55d 65 }
gidiana 33:c1cefad6d338 66
gidiana 33:c1cefad6d338 67
gidiana 33:c1cefad6d338 68 void serialrx()
gidiana 29:f888a2394027 69 {
gidiana 33:c1cefad6d338 70
gidiana 33:c1cefad6d338 71 int id, speed;
gidiana 33:c1cefad6d338 72
gidiana 33:c1cefad6d338 73
gidiana 33:c1cefad6d338 74 if(serial.readable() )
gidiana 30:c40b060795a2 75 {
gidiana 33:c1cefad6d338 76 serial.scanf("%d %d", &id, &speed );
gidiana 33:c1cefad6d338 77 printf("%d %d\n", id, speed);
gidiana 33:c1cefad6d338 78 if (id==JOINT)
gidiana 33:c1cefad6d338 79 {
gidiana 33:c1cefad6d338 80 led=!led;
gidiana 33:c1cefad6d338 81 current_speed=speed;
gidiana 33:c1cefad6d338 82 }
gidiana 30:c40b060795a2 83
gidiana 30:c40b060795a2 84
gidiana 33:c1cefad6d338 85 }
gidiana 29:f888a2394027 86 }
gidiana 33:c1cefad6d338 87
gidiana 33:c1cefad6d338 88 void fmotor()
gidiana 29:f888a2394027 89 {
gidiana 33:c1cefad6d338 90
gidiana 33:c1cefad6d338 91
gidiana 29:f888a2394027 92 if (current_speed>0)
stebonicelli 21:533d014f09e0 93 {
gidiana 33:c1cefad6d338 94 printf("run FWD\n");
gidiana 33:c1cefad6d338 95 motor->set_max_speed(abs(current_speed*80));
gidiana 33:c1cefad6d338 96 motor->run(StepperMotor::BWD);
gidiana 29:f888a2394027 97 }
gidiana 29:f888a2394027 98 else if (current_speed<0)
gidiana 29:f888a2394027 99 {
gidiana 33:c1cefad6d338 100 printf("run BWD\n");
gidiana 33:c1cefad6d338 101 motor->set_max_speed(abs(current_speed*80));
gidiana 33:c1cefad6d338 102 motor->run(StepperMotor::FWD);
gidiana 29:f888a2394027 103 }
gidiana 33:c1cefad6d338 104
gidiana 29:f888a2394027 105 else
gidiana 29:f888a2394027 106 {
gidiana 33:c1cefad6d338 107 motor->hard_stop();
gidiana 29:f888a2394027 108 current_pose= motor->get_position();
gidiana 29:f888a2394027 109 motor->go_to(current_pose);
stebonicelli 21:533d014f09e0 110 }
gidiana 33:c1cefad6d338 111
gidiana 33:c1cefad6d338 112
gidiana 33:c1cefad6d338 113
gidiana 29:f888a2394027 114
stebonicelli 25:281c8e913db4 115 }
gidiana 33:c1cefad6d338 116
gidiana 33:c1cefad6d338 117
gidiana 33:c1cefad6d338 118 /* Main ----------------------------------------------------------------------*/
gidiana 30:c40b060795a2 119
gidiana 33:c1cefad6d338 120 int main()
stebonicelli 17:dc1b04f0b55d 121 {
gidiana 33:c1cefad6d338 122 led=1;
gidiana 33:c1cefad6d338 123 serial.baud(BAUDRATE);
gidiana 33:c1cefad6d338 124
gidiana 33:c1cefad6d338 125 // Motor Initialization
gidiana 33:c1cefad6d338 126 motor = new L6208(D2, D8, D7, D4, D5, D6, VREFA_PWM_PIN, VREFB_PWM_PIN);
gidiana 33:c1cefad6d338 127 motor->set_step_mode(StepperMotor::STEP_MODE_1_16);
gidiana 33:c1cefad6d338 128 if (motor->init(&init) != COMPONENT_OK)
gidiana 33:c1cefad6d338 129 {
gidiana 33:c1cefad6d338 130 printf("ERROR: vvMotor Init\n\r");
gidiana 33:c1cefad6d338 131 exit(EXIT_FAILURE);
gidiana 33:c1cefad6d338 132 }
gidiana 33:c1cefad6d338 133
gidiana 33:c1cefad6d338 134 motor->attach_error_handler(&motor_error_handler);
gidiana 31:5d6a97adae07 135
gidiana 30:c40b060795a2 136
gidiana 33:c1cefad6d338 137 // end1.rise(&end1_int_handler);
gidiana 33:c1cefad6d338 138
gidiana 33:c1cefad6d338 139 printf("DONE: Motor Init\n\r");
gidiana 33:c1cefad6d338 140
gidiana 33:c1cefad6d338 141
gidiana 33:c1cefad6d338 142 printf("Running!\n\r");
gidiana 33:c1cefad6d338 143
gidiana 33:c1cefad6d338 144 //zero();
gidiana 33:c1cefad6d338 145
gidiana 33:c1cefad6d338 146 while(true)
gidiana 33:c1cefad6d338 147 {
gidiana 33:c1cefad6d338 148 serialrx();
gidiana 33:c1cefad6d338 149 //wait (0.001);
gidiana 33:c1cefad6d338 150 fmotor();
gidiana 33:c1cefad6d338 151
gidiana 33:c1cefad6d338 152 }
gidiana 33:c1cefad6d338 153 }