Grupo T / Mbed OS GRUPOT

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

main.cpp@1:b2e395e50a45, 2018-04-25 (annotated)

Committer:
lucasfontenla
Date:
Wed Apr 25 11:39:47 2018 +0000
Revision:
1:b2e395e50a45
Parent:
0:cc562f78f889
Child:
2:73d24891eef6
CODIGO

Who changed what in which revision?

UserRevisionLine numberNew contents of line
lucasfontenla 1:b2e395e50a45 1 /* Includes ------------------------------------------------------------------*/
lucasfontenla 1:b2e395e50a45 2
lucasfontenla 1:b2e395e50a45 3 /* mbed specific header files. */
lucasfontenla 0:cc562f78f889 4 #include "mbed.h"
lucasfontenla 1:b2e395e50a45 5 #include "rtos.h"
lucasfontenla 1:b2e395e50a45 6
lucasfontenla 1:b2e395e50a45 7 /* Helper header files. */
lucasfontenla 1:b2e395e50a45 8 #include "DevSPI.h"
lucasfontenla 1:b2e395e50a45 9
lucasfontenla 1:b2e395e50a45 10 /* Component specific header files. */
lucasfontenla 1:b2e395e50a45 11 #include "L6474.h"
lucasfontenla 1:b2e395e50a45 12
lucasfontenla 1:b2e395e50a45 13
lucasfontenla 1:b2e395e50a45 14 /* Definitions ---------------------------------------------------------------*/
lucasfontenla 1:b2e395e50a45 15
lucasfontenla 1:b2e395e50a45 16 /* Number of steps. */
lucasfontenla 1:b2e395e50a45 17 #define STEPS_1 (400 * 8) /* 1 revolution given a 400 steps motor configured at 1/8 microstep mode. */
lucasfontenla 1:b2e395e50a45 18
lucasfontenla 1:b2e395e50a45 19 /* Delay in milliseconds. */
lucasfontenla 1:b2e395e50a45 20 #define DELAY_1 1000
lucasfontenla 1:b2e395e50a45 21 #define DELAY_2 2000
lucasfontenla 1:b2e395e50a45 22 #define DELAY_3 2000
lucasfontenla 1:b2e395e50a45 23 #define DELAY_4 2000
lucasfontenla 1:b2e395e50a45 24
lucasfontenla 1:b2e395e50a45 25 /* Speed in pps (Pulses Per Second).
lucasfontenla 1:b2e395e50a45 26 In Full Step mode: 1 pps = 1 step/s).
lucasfontenla 1:b2e395e50a45 27 In 1/N Step Mode: N pps = 1 step/s). */
lucasfontenla 1:b2e395e50a45 28 #define SPEED_1 2400
lucasfontenla 1:b2e395e50a45 29 #define SPEED_2 1200
lucasfontenla 1:b2e395e50a45 30
lucasfontenla 1:b2e395e50a45 31 AnalogIn joyx(A5);
lucasfontenla 1:b2e395e50a45 32 AnalogIn joyy(A4);
lucasfontenla 1:b2e395e50a45 33
lucasfontenla 1:b2e395e50a45 34 DigitalIn bup(D12);
lucasfontenla 1:b2e395e50a45 35 DigitalIn bdown(D13);
lucasfontenla 1:b2e395e50a45 36
lucasfontenla 1:b2e395e50a45 37 /* Variables -----------------------------------------------------------------*/
lucasfontenla 0:cc562f78f889 38
lucasfontenla 1:b2e395e50a45 39 /* Initialization parameters. */
lucasfontenla 1:b2e395e50a45 40 L6474_init_t init = {
lucasfontenla 1:b2e395e50a45 41 300, /* Acceleration rate in pps^2. Range: (0..+inf). */
lucasfontenla 1:b2e395e50a45 42 300, /* Deceleration rate in pps^2. Range: (0..+inf). */
lucasfontenla 1:b2e395e50a45 43 2400, /* Maximum speed in pps. Range: (30..10000]. */
lucasfontenla 1:b2e395e50a45 44 30, /* Minimum speed in pps. Range: [30..10000). */
lucasfontenla 1:b2e395e50a45 45 250, /* Torque regulation current in mA. Range: 31.25mA to 4000mA. */
lucasfontenla 1:b2e395e50a45 46 L6474_OCD_TH_2250mA, /* Overcurrent threshold (OCD_TH register). */
lucasfontenla 1:b2e395e50a45 47 L6474_CONFIG_OC_SD_ENABLE, /* Overcurrent shutwdown (OC_SD field of CONFIG register). */
lucasfontenla 1:b2e395e50a45 48 L6474_CONFIG_EN_TQREG_TVAL_USED, /* Torque regulation method (EN_TQREG field of CONFIG register). */
lucasfontenla 1:b2e395e50a45 49 L6474_STEP_SEL_1_8, /* Step selection (STEP_SEL field of STEP_MODE register). */
lucasfontenla 1:b2e395e50a45 50 L6474_SYNC_SEL_1_2, /* Sync selection (SYNC_SEL field of STEP_MODE register). */
lucasfontenla 1:b2e395e50a45 51 L6474_FAST_STEP_12us, /* Fall time value (T_FAST field of T_FAST register). Range: 2us to 32us. */
lucasfontenla 1:b2e395e50a45 52 L6474_TOFF_FAST_8us, /* Maximum fast decay time (T_OFF field of T_FAST register). Range: 2us to 32us. */
lucasfontenla 1:b2e395e50a45 53 3, /* Minimum ON time in us (TON_MIN register). Range: 0.5us to 64us. */
lucasfontenla 1:b2e395e50a45 54 21, /* Minimum OFF time in us (TOFF_MIN register). Range: 0.5us to 64us. */
lucasfontenla 1:b2e395e50a45 55 L6474_CONFIG_TOFF_044us, /* Target Swicthing Period (field TOFF of CONFIG register). */
lucasfontenla 1:b2e395e50a45 56 L6474_CONFIG_SR_320V_us, /* Slew rate (POW_SR field of CONFIG register). */
lucasfontenla 1:b2e395e50a45 57 L6474_CONFIG_INT_16MHZ, /* Clock setting (OSC_CLK_SEL field of CONFIG register). */
lucasfontenla 1:b2e395e50a45 58 L6474_ALARM_EN_OVERCURRENT |
lucasfontenla 1:b2e395e50a45 59 L6474_ALARM_EN_THERMAL_SHUTDOWN |
lucasfontenla 1:b2e395e50a45 60 L6474_ALARM_EN_THERMAL_WARNING |
lucasfontenla 1:b2e395e50a45 61 L6474_ALARM_EN_UNDERVOLTAGE |
lucasfontenla 1:b2e395e50a45 62 L6474_ALARM_EN_SW_TURN_ON |
lucasfontenla 1:b2e395e50a45 63 L6474_ALARM_EN_WRONG_NPERF_CMD /* Alarm (ALARM_EN register). */
lucasfontenla 1:b2e395e50a45 64 };
lucasfontenla 1:b2e395e50a45 65
lucasfontenla 1:b2e395e50a45 66 float value_x = 0;
lucasfontenla 1:b2e395e50a45 67 float value_y = 0;
lucasfontenla 1:b2e395e50a45 68
lucasfontenla 1:b2e395e50a45 69 int value_bup = 0;
lucasfontenla 1:b2e395e50a45 70 int value_bdown = 0;
lucasfontenla 1:b2e395e50a45 71
lucasfontenla 1:b2e395e50a45 72 unsigned int z_speed = 2000;
lucasfontenla 1:b2e395e50a45 73
lucasfontenla 1:b2e395e50a45 74 int read = 0;
lucasfontenla 1:b2e395e50a45 75 int previous = 0;
lucasfontenla 1:b2e395e50a45 76 int speed_read;
lucasfontenla 1:b2e395e50a45 77
lucasfontenla 1:b2e395e50a45 78 unsigned int maxSpeed = 2000;
lucasfontenla 1:b2e395e50a45 79 unsigned int minSpeed = 30;
lucasfontenla 1:b2e395e50a45 80
lucasfontenla 1:b2e395e50a45 81 unsigned int speedFWDX = 0;
lucasfontenla 1:b2e395e50a45 82 unsigned int speedBWDX = 0;
lucasfontenla 1:b2e395e50a45 83
lucasfontenla 1:b2e395e50a45 84 unsigned int speedFWDY = 0;
lucasfontenla 1:b2e395e50a45 85 unsigned int speedBWDY = 0;
lucasfontenla 1:b2e395e50a45 86
lucasfontenla 1:b2e395e50a45 87 /* Motor Control Component. */
lucasfontenla 1:b2e395e50a45 88 L6474 *motor;
lucasfontenla 1:b2e395e50a45 89 L6474 *motor2;
lucasfontenla 1:b2e395e50a45 90 L6474 *motor3;
lucasfontenla 1:b2e395e50a45 91
lucasfontenla 1:b2e395e50a45 92
lucasfontenla 1:b2e395e50a45 93 /* Functions -----------------------------------------------------------------*/
lucasfontenla 1:b2e395e50a45 94 void flag_irq_handler(void)
lucasfontenla 1:b2e395e50a45 95 {
lucasfontenla 1:b2e395e50a45 96 /* Set ISR flag. */
lucasfontenla 1:b2e395e50a45 97 motor->isr_flag = TRUE;
lucasfontenla 1:b2e395e50a45 98
lucasfontenla 1:b2e395e50a45 99 /* Get the value of the status register. */
lucasfontenla 1:b2e395e50a45 100 unsigned int status = motor->get_status();
lucasfontenla 1:b2e395e50a45 101
lucasfontenla 1:b2e395e50a45 102 /* Check NOTPERF_CMD flag: if set, the command received by SPI can't be performed. */
lucasfontenla 1:b2e395e50a45 103 /* This often occures when a command is sent to the L6474 while it is not in HiZ state. */
lucasfontenla 1:b2e395e50a45 104 if ((status & L6474_STATUS_NOTPERF_CMD) == L6474_STATUS_NOTPERF_CMD) {
lucasfontenla 1:b2e395e50a45 105 printf(" WARNING: \"FLAG\" interrupt triggered. Non-performable command detected when updating L6474's registers while not in HiZ state.\r\n");
lucasfontenla 1:b2e395e50a45 106 }
lucasfontenla 1:b2e395e50a45 107
lucasfontenla 1:b2e395e50a45 108 /* Reset ISR flag. */
lucasfontenla 1:b2e395e50a45 109 motor->isr_flag = FALSE;
lucasfontenla 1:b2e395e50a45 110 }
lucasfontenla 1:b2e395e50a45 111
lucasfontenla 1:b2e395e50a45 112 /* Main ----------------------------------------------------------------------*/
lucasfontenla 1:b2e395e50a45 113
lucasfontenla 1:b2e395e50a45 114 float upper = 0.5;
lucasfontenla 1:b2e395e50a45 115 float lower = 0.42;
lucasfontenla 1:b2e395e50a45 116
lucasfontenla 1:b2e395e50a45 117 int main(){
lucasfontenla 1:b2e395e50a45 118 /*----- Initialization. -----*/
lucasfontenla 1:b2e395e50a45 119
lucasfontenla 1:b2e395e50a45 120 /* Initializing SPI bus. */
lucasfontenla 1:b2e395e50a45 121 DevSPI dev_spi(D11, D12, D13);
lucasfontenla 0:cc562f78f889 122
lucasfontenla 1:b2e395e50a45 123 // MOTOR 1 INIT ------------------------------------------------------------
lucasfontenla 1:b2e395e50a45 124 motor = new L6474(D2, D8, D7, D9, D10, dev_spi);
lucasfontenla 1:b2e395e50a45 125
lucasfontenla 1:b2e395e50a45 126 if (motor->init(&init) != COMPONENT_OK) {
lucasfontenla 1:b2e395e50a45 127 exit(EXIT_FAILURE);
lucasfontenla 1:b2e395e50a45 128 printf("Motor Error");
lucasfontenla 1:b2e395e50a45 129 }
lucasfontenla 1:b2e395e50a45 130 else {
lucasfontenla 1:b2e395e50a45 131 printf("Motor 1 started");
lucasfontenla 1:b2e395e50a45 132 }
lucasfontenla 1:b2e395e50a45 133
lucasfontenla 1:b2e395e50a45 134 /* Attaching and enabling interrupt handlers. */
lucasfontenla 1:b2e395e50a45 135 motor->attach_flag_irq(&flag_irq_handler);
lucasfontenla 1:b2e395e50a45 136 motor->enable_flag_irq();
lucasfontenla 1:b2e395e50a45 137
lucasfontenla 1:b2e395e50a45 138 /* Printing to the console. */
lucasfontenla 1:b2e395e50a45 139 printf("--> Setting Torque Regulation Current to 500[mA].\r\n");
lucasfontenla 1:b2e395e50a45 140
lucasfontenla 1:b2e395e50a45 141 /* Increasing the torque regulation current to 500[mA]. */
lucasfontenla 1:b2e395e50a45 142 motor->set_parameter(L6474_TVAL, 500);
lucasfontenla 1:b2e395e50a45 143
lucasfontenla 1:b2e395e50a45 144 /* Doubling the microsteps. */
lucasfontenla 1:b2e395e50a45 145 if (!motor->set_step_mode((StepperMotor::step_mode_t) STEP_MODE_FULL)) {
lucasfontenla 1:b2e395e50a45 146 printf("Motor 1 Step Mode not allowed.\r\n");
lucasfontenla 1:b2e395e50a45 147 }
lucasfontenla 1:b2e395e50a45 148
lucasfontenla 1:b2e395e50a45 149 // MOTOR 2 INIT ------------------------------------------------------------
lucasfontenla 1:b2e395e50a45 150 motor2 = new L6474(D2, D8, D4, D3, D10, dev_spi);
lucasfontenla 1:b2e395e50a45 151
lucasfontenla 1:b2e395e50a45 152 if (motor2->init(&init) != COMPONENT_OK) {
lucasfontenla 1:b2e395e50a45 153 exit(EXIT_FAILURE);
lucasfontenla 1:b2e395e50a45 154 printf("Motor 2 Error");
lucasfontenla 1:b2e395e50a45 155 } else {
lucasfontenla 1:b2e395e50a45 156 printf("Motor 2 started");
lucasfontenla 1:b2e395e50a45 157 }
lucasfontenla 1:b2e395e50a45 158
lucasfontenla 1:b2e395e50a45 159 /* Attaching and enabling interrupt handlers. */
lucasfontenla 1:b2e395e50a45 160 motor2->attach_flag_irq(&flag_irq_handler);
lucasfontenla 1:b2e395e50a45 161 motor2->enable_flag_irq();
lucasfontenla 1:b2e395e50a45 162
lucasfontenla 1:b2e395e50a45 163 /* Printing to the console. */
lucasfontenla 1:b2e395e50a45 164 printf("--> Setting Torque Regulation Current to 500[mA].\r\n");
lucasfontenla 1:b2e395e50a45 165
lucasfontenla 1:b2e395e50a45 166 /* Increasing the torque regulation current to 500[mA]. */
lucasfontenla 1:b2e395e50a45 167 motor2->set_parameter(L6474_TVAL, 500);
lucasfontenla 1:b2e395e50a45 168
lucasfontenla 1:b2e395e50a45 169 /* Doubling the microsteps. */
lucasfontenla 1:b2e395e50a45 170 if (!motor2->set_step_mode((StepperMotor::step_mode_t) STEP_MODE_FULL)) {
lucasfontenla 1:b2e395e50a45 171 printf(" Motor 2 Step Mode not allowed.\r\n");
lucasfontenla 1:b2e395e50a45 172 }
lucasfontenla 1:b2e395e50a45 173
lucasfontenla 1:b2e395e50a45 174 // MOTOR 1 INIT ------------------------------------------------------------
lucasfontenla 1:b2e395e50a45 175 motor3 = new L6474(D2, D8, D5, D6, D10, dev_spi);
lucasfontenla 1:b2e395e50a45 176
lucasfontenla 1:b2e395e50a45 177 if (motor3->init(&init) != COMPONENT_OK) {
lucasfontenla 1:b2e395e50a45 178 exit(EXIT_FAILURE);
lucasfontenla 1:b2e395e50a45 179 printf("Motor 3 Error");
lucasfontenla 1:b2e395e50a45 180 }
lucasfontenla 1:b2e395e50a45 181 else {
lucasfontenla 1:b2e395e50a45 182 printf("Motor 3 started");
lucasfontenla 1:b2e395e50a45 183 }
lucasfontenla 1:b2e395e50a45 184
lucasfontenla 1:b2e395e50a45 185 /* Attaching and enabling interrupt handlers. */
lucasfontenla 1:b2e395e50a45 186 motor3->attach_flag_irq(&flag_irq_handler);
lucasfontenla 1:b2e395e50a45 187 motor3->enable_flag_irq();
lucasfontenla 1:b2e395e50a45 188
lucasfontenla 1:b2e395e50a45 189 /* Printing to the console. */
lucasfontenla 1:b2e395e50a45 190 printf("--> Setting Torque Regulation Current to 500[mA].\r\n");
lucasfontenla 1:b2e395e50a45 191
lucasfontenla 1:b2e395e50a45 192 /* Increasing the torque regulation current to 500[mA]. */
lucasfontenla 1:b2e395e50a45 193 motor3->set_parameter(L6474_TVAL, 500);
lucasfontenla 1:b2e395e50a45 194
lucasfontenla 1:b2e395e50a45 195 /* Doubling the microsteps. */
lucasfontenla 1:b2e395e50a45 196 if (!motor3->set_step_mode((StepperMotor::step_mode_t) STEP_MODE_FULL)) {
lucasfontenla 1:b2e395e50a45 197 printf("Motor 3 Step Mode not allowed.\r\n");
lucasfontenla 1:b2e395e50a45 198 }
lucasfontenla 1:b2e395e50a45 199
lucasfontenla 1:b2e395e50a45 200 //------------------------------------------------------------------------------------------------------------------------
lucasfontenla 1:b2e395e50a45 201
lucasfontenla 1:b2e395e50a45 202 while(1){
lucasfontenla 1:b2e395e50a45 203 value_x = joyx;
lucasfontenla 1:b2e395e50a45 204 value_y = joyy;
lucasfontenla 1:b2e395e50a45 205
lucasfontenla 1:b2e395e50a45 206 wait(0.05);
lucasfontenla 1:b2e395e50a45 207
lucasfontenla 1:b2e395e50a45 208 value_bup = !bup;
lucasfontenla 1:b2e395e50a45 209 value_bdown = !bdown;
lucasfontenla 1:b2e395e50a45 210
lucasfontenla 1:b2e395e50a45 211 //motor1
lucasfontenla 1:b2e395e50a45 212
lucasfontenla 1:b2e395e50a45 213 if(value_x < upper && value_x > lower){
lucasfontenla 1:b2e395e50a45 214 motor->hard_stop();
lucasfontenla 1:b2e395e50a45 215 }
lucasfontenla 1:b2e395e50a45 216 else if(value_x > upper){
lucasfontenla 1:b2e395e50a45 217 speedFWDX = (int)((float)maxSpeed-(float)minSpeed)*(value_x-upper)/((float)1-upper)+(float)minSpeed;
lucasfontenla 1:b2e395e50a45 218 motor->set_min_speed(speedFWDX);
lucasfontenla 1:b2e395e50a45 219 motor->run(StepperMotor::FWD);
lucasfontenla 1:b2e395e50a45 220 printf("upper X %d\r\n",motor->get_speed());
lucasfontenla 1:b2e395e50a45 221 }
lucasfontenla 1:b2e395e50a45 222 else if(value_x < lower){
lucasfontenla 1:b2e395e50a45 223 speedBWDX = (int)((float)maxSpeed - ((float)maxSpeed-(float)minSpeed)*value_x/lower);
lucasfontenla 1:b2e395e50a45 224 motor->set_min_speed(speedBWDX);
lucasfontenla 1:b2e395e50a45 225 motor->run(StepperMotor::BWD);
lucasfontenla 1:b2e395e50a45 226 printf("lower X %d\r\n", motor->get_speed());
lucasfontenla 1:b2e395e50a45 227 }
lucasfontenla 1:b2e395e50a45 228
lucasfontenla 1:b2e395e50a45 229 //motor2
lucasfontenla 1:b2e395e50a45 230
lucasfontenla 1:b2e395e50a45 231 if(value_y < upper && value_y > lower){
lucasfontenla 1:b2e395e50a45 232 motor2->hard_stop();
lucasfontenla 1:b2e395e50a45 233 }
lucasfontenla 1:b2e395e50a45 234 else if(value_y > upper){
lucasfontenla 1:b2e395e50a45 235 speedFWDY = (int)((float)maxSpeed-(float)minSpeed)*(value_y-upper)/((float)1-upper)+(float)minSpeed;
lucasfontenla 1:b2e395e50a45 236 motor2->set_min_speed(speedFWDY);
lucasfontenla 1:b2e395e50a45 237 motor2->run(StepperMotor::FWD);
lucasfontenla 1:b2e395e50a45 238 printf("upper Y %d\r\n",motor2->get_speed());
lucasfontenla 1:b2e395e50a45 239 }
lucasfontenla 1:b2e395e50a45 240 else if(value_y < lower){
lucasfontenla 1:b2e395e50a45 241 speedBWDY = (int)((float)maxSpeed - ((float)maxSpeed-(float)minSpeed)*value_y/lower);
lucasfontenla 1:b2e395e50a45 242 motor2->set_min_speed(speedBWDY);
lucasfontenla 1:b2e395e50a45 243 motor2->run(StepperMotor::BWD);
lucasfontenla 1:b2e395e50a45 244 printf("lower Y %d\r\n", motor2->get_speed());
lucasfontenla 1:b2e395e50a45 245 }
lucasfontenla 1:b2e395e50a45 246
lucasfontenla 1:b2e395e50a45 247 //motor 3
lucasfontenla 1:b2e395e50a45 248 if(value_bup && value_bdown){
lucasfontenla 1:b2e395e50a45 249 motor3->hard_stop();
lucasfontenla 1:b2e395e50a45 250 }
lucasfontenla 1:b2e395e50a45 251 else if(!value_bup && !value_bdown){
lucasfontenla 1:b2e395e50a45 252 motor3->hard_stop();
lucasfontenla 1:b2e395e50a45 253 }
lucasfontenla 1:b2e395e50a45 254 else if(value_bup && !value_bdown){
lucasfontenla 1:b2e395e50a45 255 motor3->set_min_speed(z_speed);
lucasfontenla 1:b2e395e50a45 256 motor3->run(StepperMotor::FWD);
lucasfontenla 1:b2e395e50a45 257 printf("upper Z %d\r\n",motor3->get_speed());
lucasfontenla 1:b2e395e50a45 258 }
lucasfontenla 1:b2e395e50a45 259 else if(value_bdown && !value_bup){
lucasfontenla 1:b2e395e50a45 260 motor3->set_min_speed(z_speed);
lucasfontenla 1:b2e395e50a45 261 motor3->run(StepperMotor::BWD);
lucasfontenla 1:b2e395e50a45 262 printf("lower Z %d\r\n", motor3->get_speed());
lucasfontenla 1:b2e395e50a45 263 }
lucasfontenla 1:b2e395e50a45 264
lucasfontenla 0:cc562f78f889 265 }
lucasfontenla 0:cc562f78f889 266 }