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?
User | Revision | Line number | New 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 | } |