Team DIANA / Mbed 2 deprecated Arm_stepper_BASE_Serial_01

testing

Dependencies:   mbed X-NUCLEO-IHM05A1

main.cpp@1:6cca05643958, 2019-10-04 (annotated)

Committer:
edoardoVacchetto
Date:
Fri Oct 04 15:10:46 2019 +0000
Revision:
1:6cca05643958
Parent:
0:2c5d9f3acfb8 
ttf

Who changed what in which revision?

UserRevisionLine numberNew contents of line
edoardoVacchetto 0:2c5d9f3acfb8 1 #include "mbed.h"
edoardoVacchetto 0:2c5d9f3acfb8 2 #include "L6208.h"
edoardoVacchetto 0:2c5d9f3acfb8 3
edoardoVacchetto 0:2c5d9f3acfb8 4 #define Rx_Buff_Dim 12
edoardoVacchetto 0:2c5d9f3acfb8 5
edoardoVacchetto 0:2c5d9f3acfb8 6 #define VREFA_PWM_PIN D3
edoardoVacchetto 0:2c5d9f3acfb8 7 #define VREFB_PWM_PIN D9
edoardoVacchetto 0:2c5d9f3acfb8 8 #define BAUDRATE 9600
edoardoVacchetto 0:2c5d9f3acfb8 9 #define JOINT 2
edoardoVacchetto 0:2c5d9f3acfb8 10
edoardoVacchetto 0:2c5d9f3acfb8 11 l6208_init_t init =
edoardoVacchetto 0:2c5d9f3acfb8 12 {
edoardoVacchetto 0:2c5d9f3acfb8 13 8100, //Acceleration rate in step/s^2 or (1/16)th step/s^2 for microstep modes
edoardoVacchetto 0:2c5d9f3acfb8 14 80, //Acceleration current torque in % (from 0 to 100)
edoardoVacchetto 0:2c5d9f3acfb8 15 8100, //Deceleration rate in step/s^2 or (1/16)th step/s^2 for microstep modes
edoardoVacchetto 0:2c5d9f3acfb8 16 80, //Deceleration current torque in % (from 0 to 100)
edoardoVacchetto 0:2c5d9f3acfb8 17 8000, //Running speed in step/s or (1/16)th step/s for microstep modes
edoardoVacchetto 0:2c5d9f3acfb8 18 80, //Running current torque in % (from 0 to 100)
edoardoVacchetto 0:2c5d9f3acfb8 19 40, //Holding current torque in % (from 0 to 100)
edoardoVacchetto 0:2c5d9f3acfb8 20 STEP_MODE_1_16, //Step mode via enum motorStepMode_t
edoardoVacchetto 0:2c5d9f3acfb8 21 FAST_DECAY, //Decay mode via enum motorDecayMode_t
edoardoVacchetto 0:2c5d9f3acfb8 22 0, //Dwelling time in ms
edoardoVacchetto 0:2c5d9f3acfb8 23 FALSE, //Automatic HIZ STOP
edoardoVacchetto 0:2c5d9f3acfb8 24 100000 //VREFA and VREFB PWM frequency (Hz)
edoardoVacchetto 0:2c5d9f3acfb8 25 };
edoardoVacchetto 0:2c5d9f3acfb8 26
edoardoVacchetto 0:2c5d9f3acfb8 27 // Motor Control
edoardoVacchetto 0:2c5d9f3acfb8 28 L6208 *motor;
edoardoVacchetto 0:2c5d9f3acfb8 29 DigitalIn end0(PA_5); // endstop
edoardoVacchetto 0:2c5d9f3acfb8 30 Serial s_rx(PC_10, PC_11); // comunication usart
edoardoVacchetto 0:2c5d9f3acfb8 31 Serial Pc_Stat(PA_2, PA_3);// status usart
edoardoVacchetto 0:2c5d9f3acfb8 32 Timer timeOuter;
edoardoVacchetto 0:2c5d9f3acfb8 33
edoardoVacchetto 0:2c5d9f3acfb8 34 char dataRxBuffer[Rx_Buff_Dim];
edoardoVacchetto 0:2c5d9f3acfb8 35 volatile int rxBufferPtr;
edoardoVacchetto 0:2c5d9f3acfb8 36
edoardoVacchetto 0:2c5d9f3acfb8 37 float pose, current_pose;
edoardoVacchetto 1:6cca05643958 38 int speed, current_speed;
edoardoVacchetto 0:2c5d9f3acfb8 39
edoardoVacchetto 0:2c5d9f3acfb8 40 void motor_error_handler(uint16_t error);
edoardoVacchetto 0:2c5d9f3acfb8 41 void zero();
edoardoVacchetto 0:2c5d9f3acfb8 42 void fmotor();
edoardoVacchetto 0:2c5d9f3acfb8 43 void runCommand();
edoardoVacchetto 0:2c5d9f3acfb8 44 void serialrx();
edoardoVacchetto 0:2c5d9f3acfb8 45
edoardoVacchetto 0:2c5d9f3acfb8 46 bool posMode = false;
edoardoVacchetto 0:2c5d9f3acfb8 47
edoardoVacchetto 0:2c5d9f3acfb8 48 int main(){
edoardoVacchetto 0:2c5d9f3acfb8 49 s_rx.baud(BAUDRATE);
edoardoVacchetto 0:2c5d9f3acfb8 50 Pc_Stat.baud(115200);
edoardoVacchetto 0:2c5d9f3acfb8 51
edoardoVacchetto 0:2c5d9f3acfb8 52 // Motor Initialization
edoardoVacchetto 0:2c5d9f3acfb8 53 motor = new L6208(D2, D8, D7, D4, D5, D6, VREFA_PWM_PIN, VREFB_PWM_PIN);
edoardoVacchetto 0:2c5d9f3acfb8 54 motor->set_step_mode(StepperMotor::STEP_MODE_1_16);
edoardoVacchetto 0:2c5d9f3acfb8 55 if (motor->init(&init) != COMPONENT_OK)
edoardoVacchetto 0:2c5d9f3acfb8 56 {
edoardoVacchetto 0:2c5d9f3acfb8 57 Pc_Stat.printf("ERROR: vvMotor Init\n\r");
edoardoVacchetto 0:2c5d9f3acfb8 58 exit(EXIT_FAILURE);
edoardoVacchetto 0:2c5d9f3acfb8 59 }
edoardoVacchetto 0:2c5d9f3acfb8 60
edoardoVacchetto 0:2c5d9f3acfb8 61 motor->attach_error_handler(&motor_error_handler);
edoardoVacchetto 0:2c5d9f3acfb8 62
edoardoVacchetto 0:2c5d9f3acfb8 63 printf("DONE: Motor Init\n\r");
edoardoVacchetto 0:2c5d9f3acfb8 64
edoardoVacchetto 0:2c5d9f3acfb8 65 Pc_Stat.printf("Running!\n\r");
edoardoVacchetto 0:2c5d9f3acfb8 66
edoardoVacchetto 0:2c5d9f3acfb8 67 motor->set_home();
edoardoVacchetto 0:2c5d9f3acfb8 68
edoardoVacchetto 1:6cca05643958 69 s_rx.printf("19 1\r\n");
edoardoVacchetto 0:2c5d9f3acfb8 70
edoardoVacchetto 0:2c5d9f3acfb8 71 timeOuter.start();
edoardoVacchetto 0:2c5d9f3acfb8 72
edoardoVacchetto 0:2c5d9f3acfb8 73 while(true)
edoardoVacchetto 0:2c5d9f3acfb8 74 {
edoardoVacchetto 0:2c5d9f3acfb8 75 serialrx();
edoardoVacchetto 0:2c5d9f3acfb8 76 if (timeOuter.read_ms() > 2000) {
edoardoVacchetto 0:2c5d9f3acfb8 77 current_speed = 0;
edoardoVacchetto 0:2c5d9f3acfb8 78 timeOuter.reset();
edoardoVacchetto 1:6cca05643958 79 motor->hard_stop();
edoardoVacchetto 0:2c5d9f3acfb8 80 Pc_Stat.printf("Command timeout!!\n");
edoardoVacchetto 0:2c5d9f3acfb8 81 }
edoardoVacchetto 0:2c5d9f3acfb8 82 //wait (0.001);
edoardoVacchetto 1:6cca05643958 83 //if (!posMode) fmotor();
edoardoVacchetto 0:2c5d9f3acfb8 84
edoardoVacchetto 0:2c5d9f3acfb8 85 }
edoardoVacchetto 0:2c5d9f3acfb8 86 }
edoardoVacchetto 0:2c5d9f3acfb8 87
edoardoVacchetto 0:2c5d9f3acfb8 88 void motor_error_handler(uint16_t error){
edoardoVacchetto 0:2c5d9f3acfb8 89 Pc_Stat.printf("ERROR: Motor Runtime\n\r");
edoardoVacchetto 0:2c5d9f3acfb8 90 }
edoardoVacchetto 0:2c5d9f3acfb8 91
edoardoVacchetto 0:2c5d9f3acfb8 92 void zero(){
edoardoVacchetto 0:2c5d9f3acfb8 93 Pc_Stat.printf("Zeroing (%d)... ", (int) end0);
edoardoVacchetto 0:2c5d9f3acfb8 94 //motor->run(StepperMotor::BWD);
edoardoVacchetto 0:2c5d9f3acfb8 95
edoardoVacchetto 0:2c5d9f3acfb8 96 int osc_pos = 10000;
edoardoVacchetto 0:2c5d9f3acfb8 97
edoardoVacchetto 0:2c5d9f3acfb8 98 if (!end0) {
edoardoVacchetto 0:2c5d9f3acfb8 99 motor->hard_stop();
edoardoVacchetto 0:2c5d9f3acfb8 100 motor->set_home();
edoardoVacchetto 0:2c5d9f3acfb8 101
edoardoVacchetto 0:2c5d9f3acfb8 102 motor->go_to(-8000);
edoardoVacchetto 0:2c5d9f3acfb8 103 while(motor->get_position() != -8000);
edoardoVacchetto 0:2c5d9f3acfb8 104 }
edoardoVacchetto 0:2c5d9f3acfb8 105
edoardoVacchetto 0:2c5d9f3acfb8 106 motor->set_home();
edoardoVacchetto 0:2c5d9f3acfb8 107
edoardoVacchetto 0:2c5d9f3acfb8 108 for (int i = 0; i < 4; i++) {
edoardoVacchetto 0:2c5d9f3acfb8 109 motor->go_to(osc_pos);
edoardoVacchetto 0:2c5d9f3acfb8 110 while(end0 && motor->get_position() != osc_pos);
edoardoVacchetto 0:2c5d9f3acfb8 111 if (!end0) {
edoardoVacchetto 0:2c5d9f3acfb8 112 motor->hard_stop();
edoardoVacchetto 0:2c5d9f3acfb8 113 motor->set_home();
edoardoVacchetto 0:2c5d9f3acfb8 114 osc_pos = 5000 * (osc_pos > 0 ? 1 : -1);
edoardoVacchetto 0:2c5d9f3acfb8 115 motor->go_to(osc_pos);
edoardoVacchetto 0:2c5d9f3acfb8 116 while(motor->get_position() != osc_pos);
edoardoVacchetto 0:2c5d9f3acfb8 117 motor->set_home();
edoardoVacchetto 0:2c5d9f3acfb8 118 Pc_Stat.printf("OK!\n");
edoardoVacchetto 0:2c5d9f3acfb8 119 return;
edoardoVacchetto 0:2c5d9f3acfb8 120 } else {
edoardoVacchetto 0:2c5d9f3acfb8 121 osc_pos *= -2;
edoardoVacchetto 0:2c5d9f3acfb8 122 }
edoardoVacchetto 0:2c5d9f3acfb8 123 }
edoardoVacchetto 0:2c5d9f3acfb8 124
edoardoVacchetto 0:2c5d9f3acfb8 125 Pc_Stat.printf("ZERO FAILURE!\n");
edoardoVacchetto 0:2c5d9f3acfb8 126 }
edoardoVacchetto 0:2c5d9f3acfb8 127
edoardoVacchetto 1:6cca05643958 128 void fmotor(int speed)
edoardoVacchetto 0:2c5d9f3acfb8 129 {
edoardoVacchetto 1:6cca05643958 130 unsigned int sp = abs(speed*80);
edoardoVacchetto 0:2c5d9f3acfb8 131
edoardoVacchetto 1:6cca05643958 132 if (sp) {
edoardoVacchetto 1:6cca05643958 133 motor->set_max_speed(sp);
edoardoVacchetto 1:6cca05643958 134
edoardoVacchetto 1:6cca05643958 135 if(speed > 0){
edoardoVacchetto 1:6cca05643958 136 motor->run(StepperMotor::FWD );
edoardoVacchetto 1:6cca05643958 137 }
edoardoVacchetto 1:6cca05643958 138 else if (speed < 0){
edoardoVacchetto 1:6cca05643958 139 motor->run(StepperMotor::BWD );
edoardoVacchetto 1:6cca05643958 140 }
edoardoVacchetto 1:6cca05643958 141
edoardoVacchetto 1:6cca05643958 142 //motor->run(current_speed >= 0 ?
edoardoVacchetto 1:6cca05643958 143 // StepperMotor::FWD : StepperMotor::BWD);
edoardoVacchetto 0:2c5d9f3acfb8 144 } else {
edoardoVacchetto 0:2c5d9f3acfb8 145 motor->hard_stop();
edoardoVacchetto 0:2c5d9f3acfb8 146 current_pose = motor->get_position();
edoardoVacchetto 0:2c5d9f3acfb8 147 motor->go_to(current_pose);
edoardoVacchetto 0:2c5d9f3acfb8 148 }
edoardoVacchetto 0:2c5d9f3acfb8 149 wait(0.001);
edoardoVacchetto 0:2c5d9f3acfb8 150 }
edoardoVacchetto 0:2c5d9f3acfb8 151
edoardoVacchetto 0:2c5d9f3acfb8 152 void runCommand() {
edoardoVacchetto 0:2c5d9f3acfb8 153 int joint, spd;
edoardoVacchetto 0:2c5d9f3acfb8 154
edoardoVacchetto 0:2c5d9f3acfb8 155 current_speed = 0;
edoardoVacchetto 0:2c5d9f3acfb8 156 timeOuter.reset();
edoardoVacchetto 0:2c5d9f3acfb8 157
edoardoVacchetto 0:2c5d9f3acfb8 158 sscanf(dataRxBuffer, "%d %d", &joint, &spd);
edoardoVacchetto 1:6cca05643958 159 Pc_Stat.printf("joint: %d, value: %d\n", joint, spd);
edoardoVacchetto 0:2c5d9f3acfb8 160 posMode = false;
edoardoVacchetto 0:2c5d9f3acfb8 161
edoardoVacchetto 0:2c5d9f3acfb8 162 switch(joint){
edoardoVacchetto 0:2c5d9f3acfb8 163 case 10:
edoardoVacchetto 0:2c5d9f3acfb8 164 zero();
edoardoVacchetto 0:2c5d9f3acfb8 165 break;
edoardoVacchetto 0:2c5d9f3acfb8 166 case 11:
edoardoVacchetto 0:2c5d9f3acfb8 167 posMode = true;
edoardoVacchetto 1:6cca05643958 168 Pc_Stat.printf("Moving(position)... \n");
edoardoVacchetto 0:2c5d9f3acfb8 169 motor->set_max_speed(8000);
edoardoVacchetto 0:2c5d9f3acfb8 170 motor->go_to(spd);
edoardoVacchetto 0:2c5d9f3acfb8 171 Pc_Stat.printf("Done!\n");
edoardoVacchetto 0:2c5d9f3acfb8 172 break;
edoardoVacchetto 0:2c5d9f3acfb8 173 case 12:
edoardoVacchetto 1:6cca05643958 174 Pc_Stat.printf("Moving(velocity)... \n");
edoardoVacchetto 1:6cca05643958 175 fmotor(spd);
edoardoVacchetto 0:2c5d9f3acfb8 176 s_rx.printf("15 %d\n",motor->get_position());
edoardoVacchetto 0:2c5d9f3acfb8 177 break;
edoardoVacchetto 0:2c5d9f3acfb8 178 case 13:
edoardoVacchetto 1:6cca05643958 179 posMode = true;
edoardoVacchetto 0:2c5d9f3acfb8 180 motor->hard_stop();
edoardoVacchetto 0:2c5d9f3acfb8 181 Pc_Stat.printf("STOP.\n");
edoardoVacchetto 0:2c5d9f3acfb8 182 break;
edoardoVacchetto 0:2c5d9f3acfb8 183 default:
edoardoVacchetto 1:6cca05643958 184 Pc_Stat.printf("Unknown index %d - data: %d\n", joint, spd);
edoardoVacchetto 0:2c5d9f3acfb8 185 break;
edoardoVacchetto 0:2c5d9f3acfb8 186 }
edoardoVacchetto 0:2c5d9f3acfb8 187 }
edoardoVacchetto 0:2c5d9f3acfb8 188
edoardoVacchetto 0:2c5d9f3acfb8 189 void serialrx()
edoardoVacchetto 0:2c5d9f3acfb8 190 {
edoardoVacchetto 0:2c5d9f3acfb8 191 while(s_rx.readable())
edoardoVacchetto 0:2c5d9f3acfb8 192 {
edoardoVacchetto 0:2c5d9f3acfb8 193 char c = s_rx.getc();
edoardoVacchetto 1:6cca05643958 194 //Pc_Stat.printf("%c",c);
edoardoVacchetto 0:2c5d9f3acfb8 195
edoardoVacchetto 0:2c5d9f3acfb8 196 if (c == '\n' || rxBufferPtr >= Rx_Buff_Dim - 1) {
edoardoVacchetto 0:2c5d9f3acfb8 197 dataRxBuffer[rxBufferPtr] = '\0';
edoardoVacchetto 0:2c5d9f3acfb8 198 runCommand();
edoardoVacchetto 0:2c5d9f3acfb8 199 rxBufferPtr = 0;
edoardoVacchetto 0:2c5d9f3acfb8 200
edoardoVacchetto 0:2c5d9f3acfb8 201 } else {
edoardoVacchetto 0:2c5d9f3acfb8 202 dataRxBuffer[rxBufferPtr++] = c;
edoardoVacchetto 0:2c5d9f3acfb8 203 }
edoardoVacchetto 0:2c5d9f3acfb8 204 }
edoardoVacchetto 0:2c5d9f3acfb8 205 }