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@2:73d24891eef6, 2018-04-25 (annotated)

Committer:
claraluques
Date:
Wed Apr 25 11:42:40 2018 +0000
Revision:
2:73d24891eef6
Parent:
1:b2e395e50a45
Child:
3:eb9bd4131e95
OIE;

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