Joy_Buttons versao 2
Dependencies: X_NUCLEO_IHM01A1 mbed
Fork of HelloWorld_IHM01A1_buttons by
botoesnh_default.cpp@40:13d2c943dfe9, 2018-04-27 (annotated)
- Committer:
- Bitelli
- Date:
- Fri Apr 27 17:52:39 2018 +0000
- Revision:
- 40:13d2c943dfe9
fiz_tudo;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Bitelli | 40:13d2c943dfe9 | 1 | /* Includes ------------------------------------------------------------------*/ |
Bitelli | 40:13d2c943dfe9 | 2 | |
Bitelli | 40:13d2c943dfe9 | 3 | /* mbed specific header files. */ |
Bitelli | 40:13d2c943dfe9 | 4 | #include "mbed.h" |
Bitelli | 40:13d2c943dfe9 | 5 | |
Bitelli | 40:13d2c943dfe9 | 6 | /* Helper header files. */ |
Bitelli | 40:13d2c943dfe9 | 7 | #include "DevSPI.h" |
Bitelli | 40:13d2c943dfe9 | 8 | |
Bitelli | 40:13d2c943dfe9 | 9 | /* Component specific header files. */ |
Bitelli | 40:13d2c943dfe9 | 10 | #include "L6474.h" |
Bitelli | 40:13d2c943dfe9 | 11 | #include "new_file.cpp" |
Bitelli | 40:13d2c943dfe9 | 12 | |
Bitelli | 40:13d2c943dfe9 | 13 | /* Definitions ---------------------------------------------------------------*/ |
Bitelli | 40:13d2c943dfe9 | 14 | |
Bitelli | 40:13d2c943dfe9 | 15 | /* Number of steps. */ |
Bitelli | 40:13d2c943dfe9 | 16 | #define STEPS_1 (200 * 2) /* 1 revolution given a 200 steps motor configured at 1/2 microstep mode. */ |
Bitelli | 40:13d2c943dfe9 | 17 | #define safe (600) |
Bitelli | 40:13d2c943dfe9 | 18 | |
Bitelli | 40:13d2c943dfe9 | 19 | /* Delay in milliseconds. */ |
Bitelli | 40:13d2c943dfe9 | 20 | #define DELAY_1 1000 |
Bitelli | 40:13d2c943dfe9 | 21 | |
Bitelli | 40:13d2c943dfe9 | 22 | |
Bitelli | 40:13d2c943dfe9 | 23 | /* Speed in pps (Pulses Per Second). |
Bitelli | 40:13d2c943dfe9 | 24 | In Full Step mode: 1 pps = 1 step/s). |
Bitelli | 40:13d2c943dfe9 | 25 | In 1/N Step Mode: N pps = 1 step/s). */ |
Bitelli | 40:13d2c943dfe9 | 26 | #define SPEED_1 2400 |
Bitelli | 40:13d2c943dfe9 | 27 | #define SPEED_2 1200 |
Bitelli | 40:13d2c943dfe9 | 28 | |
Bitelli | 40:13d2c943dfe9 | 29 | DigitalOut myled(LED1); |
Bitelli | 40:13d2c943dfe9 | 30 | Serial pc(USBTX, USBRX); |
Bitelli | 40:13d2c943dfe9 | 31 | |
Bitelli | 40:13d2c943dfe9 | 32 | |
Bitelli | 40:13d2c943dfe9 | 33 | /* Variables -----------------------------------------------------------------*/ |
Bitelli | 40:13d2c943dfe9 | 34 | |
Bitelli | 40:13d2c943dfe9 | 35 | /* Initialization parameters. */ |
Bitelli | 40:13d2c943dfe9 | 36 | |
Bitelli | 40:13d2c943dfe9 | 37 | L6474_init_t init = { |
Bitelli | 40:13d2c943dfe9 | 38 | 160, /* Acceleration rate in pps^2. Range: (0..+inf). */ |
Bitelli | 40:13d2c943dfe9 | 39 | 160, /* Deceleration rate in pps^2. Range: (0..+inf). */ |
Bitelli | 40:13d2c943dfe9 | 40 | 2600, /* Maximum speed in pps. Range: (30..10000]. */ |
Bitelli | 40:13d2c943dfe9 | 41 | 1300, /* Minimum speed in pps. Range: [30..10000). */ |
Bitelli | 40:13d2c943dfe9 | 42 | 250, /* Torque regulation current in mA. Range: 31.25mA to 4000mA. */ |
Bitelli | 40:13d2c943dfe9 | 43 | L6474_OCD_TH_750mA, /* Overcurrent threshold (OCD_TH register). */ |
Bitelli | 40:13d2c943dfe9 | 44 | L6474_CONFIG_OC_SD_ENABLE, /* Overcurrent shutwdown (OC_SD field of CONFIG register). */ |
Bitelli | 40:13d2c943dfe9 | 45 | L6474_CONFIG_EN_TQREG_TVAL_USED, /* Torque regulation method (EN_TQREG field of CONFIG register). */ |
Bitelli | 40:13d2c943dfe9 | 46 | L6474_STEP_SEL_1_16, /* Step selection (STEP_SEL field of STEP_MODE register). */ |
Bitelli | 40:13d2c943dfe9 | 47 | L6474_SYNC_SEL_1_2, /* Sync selection (SYNC_SEL field of STEP_MODE register). */ |
Bitelli | 40:13d2c943dfe9 | 48 | L6474_FAST_STEP_12us, /* Fall time value (T_FAST field of T_FAST register). Range: 2us to 32us. */ |
Bitelli | 40:13d2c943dfe9 | 49 | L6474_TOFF_FAST_8us, /* Maximum fast decay time (T_OFF field of T_FAST register). Range: 2us to 32us. */ |
Bitelli | 40:13d2c943dfe9 | 50 | 3, /* Minimum ON time in us (TON_MIN register). Range: 0.5us to 64us. */ |
Bitelli | 40:13d2c943dfe9 | 51 | 21, /* Minimum OFF time in us (TOFF_MIN register). Range: 0.5us to 64us. */ |
Bitelli | 40:13d2c943dfe9 | 52 | L6474_CONFIG_TOFF_044us, /* Target Swicthing Period (field TOFF of CONFIG register). */ |
Bitelli | 40:13d2c943dfe9 | 53 | L6474_CONFIG_SR_320V_us, /* Slew rate (POW_SR field of CONFIG register). */ |
Bitelli | 40:13d2c943dfe9 | 54 | L6474_CONFIG_INT_16MHZ, /* Clock setting (OSC_CLK_SEL field of CONFIG register). */ |
Bitelli | 40:13d2c943dfe9 | 55 | L6474_ALARM_EN_OVERCURRENT | |
Bitelli | 40:13d2c943dfe9 | 56 | L6474_ALARM_EN_THERMAL_SHUTDOWN | |
Bitelli | 40:13d2c943dfe9 | 57 | L6474_ALARM_EN_THERMAL_WARNING | |
Bitelli | 40:13d2c943dfe9 | 58 | L6474_ALARM_EN_UNDERVOLTAGE | |
Bitelli | 40:13d2c943dfe9 | 59 | L6474_ALARM_EN_SW_TURN_ON | |
Bitelli | 40:13d2c943dfe9 | 60 | L6474_ALARM_EN_WRONG_NPERF_CMD /* Alarm (ALARM_EN register). */ |
Bitelli | 40:13d2c943dfe9 | 61 | }; |
Bitelli | 40:13d2c943dfe9 | 62 | |
Bitelli | 40:13d2c943dfe9 | 63 | /* Motor Control Component. */ |
Bitelli | 40:13d2c943dfe9 | 64 | L6474 *motor1; |
Bitelli | 40:13d2c943dfe9 | 65 | L6474 *motor2; |
Bitelli | 40:13d2c943dfe9 | 66 | L6474 *motor3; |
Bitelli | 40:13d2c943dfe9 | 67 | |
Bitelli | 40:13d2c943dfe9 | 68 | |
Bitelli | 40:13d2c943dfe9 | 69 | |
Bitelli | 40:13d2c943dfe9 | 70 | /*---------------------Functions -------------------*/ |
Bitelli | 40:13d2c943dfe9 | 71 | |
Bitelli | 40:13d2c943dfe9 | 72 | |
Bitelli | 40:13d2c943dfe9 | 73 | |
Bitelli | 40:13d2c943dfe9 | 74 | |
Bitelli | 40:13d2c943dfe9 | 75 | DigitalIn sUp(A0); |
Bitelli | 40:13d2c943dfe9 | 76 | DigitalIn sDown(A1); |
Bitelli | 40:13d2c943dfe9 | 77 | DigitalIn sLeft(A2); |
Bitelli | 40:13d2c943dfe9 | 78 | DigitalIn sRight(A3); |
Bitelli | 40:13d2c943dfe9 | 79 | DigitalIn YES(A4); |
Bitelli | 40:13d2c943dfe9 | 80 | DigitalIn NO(A5); |
Bitelli | 40:13d2c943dfe9 | 81 | DigitalIn zeroX(PC_8); |
Bitelli | 40:13d2c943dfe9 | 82 | DigitalIn zeroY(PC_6); |
Bitelli | 40:13d2c943dfe9 | 83 | DigitalIn zeroZ(PA_11); |
Bitelli | 40:13d2c943dfe9 | 84 | DigitalIn maxX(PC_5); |
Bitelli | 40:13d2c943dfe9 | 85 | DigitalIn maxY(PA_12); |
Bitelli | 40:13d2c943dfe9 | 86 | DigitalIn maxZ(PB_12); |
Bitelli | 40:13d2c943dfe9 | 87 | |
Bitelli | 40:13d2c943dfe9 | 88 | |
Bitelli | 40:13d2c943dfe9 | 89 | |
Bitelli | 40:13d2c943dfe9 | 90 | void fdc_inicialX(){ |
Bitelli | 40:13d2c943dfe9 | 91 | pc.printf("\n\rZerando eixo X"); |
Bitelli | 40:13d2c943dfe9 | 92 | motor1->run(StepperMotor::BWD); |
Bitelli | 40:13d2c943dfe9 | 93 | while(zeroX ==1){ |
Bitelli | 40:13d2c943dfe9 | 94 | wait(0.01); |
Bitelli | 40:13d2c943dfe9 | 95 | } |
Bitelli | 40:13d2c943dfe9 | 96 | motor1->hard_stop(); |
Bitelli | 40:13d2c943dfe9 | 97 | motor1->move(StepperMotor::FWD, safe); |
Bitelli | 40:13d2c943dfe9 | 98 | motor1->set_home(); |
Bitelli | 40:13d2c943dfe9 | 99 | pc.printf("\n\rEixo X zerado"); |
Bitelli | 40:13d2c943dfe9 | 100 | } |
Bitelli | 40:13d2c943dfe9 | 101 | |
Bitelli | 40:13d2c943dfe9 | 102 | |
Bitelli | 40:13d2c943dfe9 | 103 | void fdc_inicialY(){ |
Bitelli | 40:13d2c943dfe9 | 104 | pc.printf("\n\rZerando eixo Y"); |
Bitelli | 40:13d2c943dfe9 | 105 | motor2->run(StepperMotor::BWD); |
Bitelli | 40:13d2c943dfe9 | 106 | while(zeroY ==1){ |
Bitelli | 40:13d2c943dfe9 | 107 | wait(0.01); |
Bitelli | 40:13d2c943dfe9 | 108 | } |
Bitelli | 40:13d2c943dfe9 | 109 | motor2->hard_stop(); |
Bitelli | 40:13d2c943dfe9 | 110 | motor2->move(StepperMotor::FWD, safe); |
Bitelli | 40:13d2c943dfe9 | 111 | motor2->set_home(); |
Bitelli | 40:13d2c943dfe9 | 112 | pc.printf("\n\rEixo Y zerado"); |
Bitelli | 40:13d2c943dfe9 | 113 | } |
Bitelli | 40:13d2c943dfe9 | 114 | |
Bitelli | 40:13d2c943dfe9 | 115 | |
Bitelli | 40:13d2c943dfe9 | 116 | void fdc_inicialZ(){ |
Bitelli | 40:13d2c943dfe9 | 117 | pc.printf("\n\rZerando eixo Z"); |
Bitelli | 40:13d2c943dfe9 | 118 | motor3->run(StepperMotor::BWD); |
Bitelli | 40:13d2c943dfe9 | 119 | while(zeroZ ==1){ |
Bitelli | 40:13d2c943dfe9 | 120 | wait(0.01); |
Bitelli | 40:13d2c943dfe9 | 121 | } |
Bitelli | 40:13d2c943dfe9 | 122 | motor3->hard_stop(); |
Bitelli | 40:13d2c943dfe9 | 123 | motor3->move(StepperMotor::FWD, safe); |
Bitelli | 40:13d2c943dfe9 | 124 | motor3->set_home(); |
Bitelli | 40:13d2c943dfe9 | 125 | pc.printf("\n\rEixo Z zerado"); |
Bitelli | 40:13d2c943dfe9 | 126 | } |
Bitelli | 40:13d2c943dfe9 | 127 | |
Bitelli | 40:13d2c943dfe9 | 128 | |
Bitelli | 40:13d2c943dfe9 | 129 | void fdc_zeroX(){ |
Bitelli | 40:13d2c943dfe9 | 130 | motor1->move(StepperMotor::FWD, safe); |
Bitelli | 40:13d2c943dfe9 | 131 | pc.printf ("\n\rLimite do Eixo X"); |
Bitelli | 40:13d2c943dfe9 | 132 | pc.printf ("\n\rY: %d", motor2->get_position()); |
Bitelli | 40:13d2c943dfe9 | 133 | wait(2); |
Bitelli | 40:13d2c943dfe9 | 134 | } |
Bitelli | 40:13d2c943dfe9 | 135 | |
Bitelli | 40:13d2c943dfe9 | 136 | void fdc_zeroY(){ |
Bitelli | 40:13d2c943dfe9 | 137 | motor2->move(StepperMotor::FWD, safe); |
Bitelli | 40:13d2c943dfe9 | 138 | pc.printf ("\n\rX: %d", motor1->get_position()); |
Bitelli | 40:13d2c943dfe9 | 139 | pc.printf ("\n\rLimite do Eixo Y"); |
Bitelli | 40:13d2c943dfe9 | 140 | wait(2); |
Bitelli | 40:13d2c943dfe9 | 141 | } |
Bitelli | 40:13d2c943dfe9 | 142 | |
Bitelli | 40:13d2c943dfe9 | 143 | void fdc_zeroZ(){ |
Bitelli | 40:13d2c943dfe9 | 144 | motor3->move(StepperMotor::FWD, safe); |
Bitelli | 40:13d2c943dfe9 | 145 | pc.printf ("\n\rZ: %d", motor3->get_position()); |
Bitelli | 40:13d2c943dfe9 | 146 | pc.printf ("\n\rLimite do Eixo Z"); |
Bitelli | 40:13d2c943dfe9 | 147 | wait(2); |
Bitelli | 40:13d2c943dfe9 | 148 | } |
Bitelli | 40:13d2c943dfe9 | 149 | |
Bitelli | 40:13d2c943dfe9 | 150 | void fdc_maxX(){ |
Bitelli | 40:13d2c943dfe9 | 151 | motor1->move(StepperMotor::BWD, safe); |
Bitelli | 40:13d2c943dfe9 | 152 | pc.printf ("\n\rLimite do Eixo X"); |
Bitelli | 40:13d2c943dfe9 | 153 | pc.printf ("\n\rY: %d", motor2->get_position()); |
Bitelli | 40:13d2c943dfe9 | 154 | wait(2); |
Bitelli | 40:13d2c943dfe9 | 155 | } |
Bitelli | 40:13d2c943dfe9 | 156 | |
Bitelli | 40:13d2c943dfe9 | 157 | void fdc_maxY(){ |
Bitelli | 40:13d2c943dfe9 | 158 | motor2->move(StepperMotor::BWD, safe); |
Bitelli | 40:13d2c943dfe9 | 159 | pc.printf ("\n\rX: %d", motor1->get_position()); |
Bitelli | 40:13d2c943dfe9 | 160 | pc.printf ("\n\rLimite do Eixo Y"); |
Bitelli | 40:13d2c943dfe9 | 161 | wait(2); |
Bitelli | 40:13d2c943dfe9 | 162 | } |
Bitelli | 40:13d2c943dfe9 | 163 | |
Bitelli | 40:13d2c943dfe9 | 164 | |
Bitelli | 40:13d2c943dfe9 | 165 | void fdc_maxZ(){ |
Bitelli | 40:13d2c943dfe9 | 166 | motor3->move(StepperMotor::BWD, safe); |
Bitelli | 40:13d2c943dfe9 | 167 | pc.printf ("\n\rZ: %d", motor3->get_position()); |
Bitelli | 40:13d2c943dfe9 | 168 | pc.printf ("\n\rLimite do Eixo Z"); |
Bitelli | 40:13d2c943dfe9 | 169 | wait(2); |
Bitelli | 40:13d2c943dfe9 | 170 | } |
Bitelli | 40:13d2c943dfe9 | 171 | |
Bitelli | 40:13d2c943dfe9 | 172 | void zeramento(){ |
Bitelli | 40:13d2c943dfe9 | 173 | fdc_inicialX(); |
Bitelli | 40:13d2c943dfe9 | 174 | fdc_inicialY(); |
Bitelli | 40:13d2c943dfe9 | 175 | fdc_inicialZ(); |
Bitelli | 40:13d2c943dfe9 | 176 | } |
Bitelli | 40:13d2c943dfe9 | 177 | |
Bitelli | 40:13d2c943dfe9 | 178 | int referenciar_z(int referencia_z){ |
Bitelli | 40:13d2c943dfe9 | 179 | pc.printf("\n\rZ: %d", motor3->get_position()); |
Bitelli | 40:13d2c943dfe9 | 180 | while(NO!=0){ |
Bitelli | 40:13d2c943dfe9 | 181 | if (sUp==0){ |
Bitelli | 40:13d2c943dfe9 | 182 | motor3->move(StepperMotor::FWD,600); |
Bitelli | 40:13d2c943dfe9 | 183 | pc.printf("\n\rZ: %d", motor3->get_position()); |
Bitelli | 40:13d2c943dfe9 | 184 | } |
Bitelli | 40:13d2c943dfe9 | 185 | if (sDown==0){ |
Bitelli | 40:13d2c943dfe9 | 186 | motor3->move(StepperMotor::BWD,600); |
Bitelli | 40:13d2c943dfe9 | 187 | pc.printf("\n\rZ: %d", motor3->get_position()); |
Bitelli | 40:13d2c943dfe9 | 188 | } |
Bitelli | 40:13d2c943dfe9 | 189 | if (zeroZ ==0){ |
Bitelli | 40:13d2c943dfe9 | 190 | fdc_zeroZ(); |
Bitelli | 40:13d2c943dfe9 | 191 | } |
Bitelli | 40:13d2c943dfe9 | 192 | if (maxZ ==0){ |
Bitelli | 40:13d2c943dfe9 | 193 | fdc_maxZ(); |
Bitelli | 40:13d2c943dfe9 | 194 | } |
Bitelli | 40:13d2c943dfe9 | 195 | wait(0.1); |
Bitelli | 40:13d2c943dfe9 | 196 | pc.printf ("\n\rZ: %d", motor3->get_position()); |
Bitelli | 40:13d2c943dfe9 | 197 | if (YES==0) { |
Bitelli | 40:13d2c943dfe9 | 198 | pc.printf ("\n\rEixo Z referenciado!"); |
Bitelli | 40:13d2c943dfe9 | 199 | referencia_z=motor3->get_position(); |
Bitelli | 40:13d2c943dfe9 | 200 | wait(1); |
Bitelli | 40:13d2c943dfe9 | 201 | } |
Bitelli | 40:13d2c943dfe9 | 202 | } |
Bitelli | 40:13d2c943dfe9 | 203 | return referencia_z; |
Bitelli | 40:13d2c943dfe9 | 204 | } |
Bitelli | 40:13d2c943dfe9 | 205 | int adicionar_pontos(int j,int positionX[], int positionY[]){ |
Bitelli | 40:13d2c943dfe9 | 206 | while(NO!=0) { |
Bitelli | 40:13d2c943dfe9 | 207 | if (sUp==0) { |
Bitelli | 40:13d2c943dfe9 | 208 | pc.printf ("\n\rX: %d", motor1->get_position()); |
Bitelli | 40:13d2c943dfe9 | 209 | pc.printf ("\n\rY: %d", motor2->get_position()); |
Bitelli | 40:13d2c943dfe9 | 210 | motor2->move(StepperMotor::FWD, 600); |
Bitelli | 40:13d2c943dfe9 | 211 | } |
Bitelli | 40:13d2c943dfe9 | 212 | if (sRight==0) { |
Bitelli | 40:13d2c943dfe9 | 213 | pc.printf ("\n\rX: %d", motor1->get_position()); |
Bitelli | 40:13d2c943dfe9 | 214 | pc.printf ("\n\rY: %d", motor2->get_position()); |
Bitelli | 40:13d2c943dfe9 | 215 | motor1->move(StepperMotor::FWD, 600); |
Bitelli | 40:13d2c943dfe9 | 216 | } |
Bitelli | 40:13d2c943dfe9 | 217 | if (sDown==0) { |
Bitelli | 40:13d2c943dfe9 | 218 | pc.printf ("\n\rX: %d", motor1->get_position()); |
Bitelli | 40:13d2c943dfe9 | 219 | pc.printf ("\n\rY: %d", motor2->get_position()); |
Bitelli | 40:13d2c943dfe9 | 220 | motor2->move(StepperMotor::BWD, 600); |
Bitelli | 40:13d2c943dfe9 | 221 | } |
Bitelli | 40:13d2c943dfe9 | 222 | if (sLeft==0) { |
Bitelli | 40:13d2c943dfe9 | 223 | pc.printf ("\n\rX: %d", motor1->get_position()); |
Bitelli | 40:13d2c943dfe9 | 224 | pc.printf ("\n\rY: %d", motor2->get_position()); |
Bitelli | 40:13d2c943dfe9 | 225 | motor1->move(StepperMotor::BWD, 600); |
Bitelli | 40:13d2c943dfe9 | 226 | } |
Bitelli | 40:13d2c943dfe9 | 227 | if (zeroX ==0){ |
Bitelli | 40:13d2c943dfe9 | 228 | fdc_zeroX(); |
Bitelli | 40:13d2c943dfe9 | 229 | } |
Bitelli | 40:13d2c943dfe9 | 230 | if (zeroY ==0){ |
Bitelli | 40:13d2c943dfe9 | 231 | fdc_zeroY(); |
Bitelli | 40:13d2c943dfe9 | 232 | } |
Bitelli | 40:13d2c943dfe9 | 233 | if (maxX ==0){ |
Bitelli | 40:13d2c943dfe9 | 234 | fdc_maxX(); |
Bitelli | 40:13d2c943dfe9 | 235 | } |
Bitelli | 40:13d2c943dfe9 | 236 | if (maxY ==0){ |
Bitelli | 40:13d2c943dfe9 | 237 | fdc_maxY(); |
Bitelli | 40:13d2c943dfe9 | 238 | } |
Bitelli | 40:13d2c943dfe9 | 239 | wait(0.2); |
Bitelli | 40:13d2c943dfe9 | 240 | if (YES==0) { |
Bitelli | 40:13d2c943dfe9 | 241 | pc.printf ("\n\rPonto Adicionado!"); |
Bitelli | 40:13d2c943dfe9 | 242 | positionX[j]=motor1->get_position(); |
Bitelli | 40:13d2c943dfe9 | 243 | positionY[j]=motor2->get_position(); |
Bitelli | 40:13d2c943dfe9 | 244 | j++; |
Bitelli | 40:13d2c943dfe9 | 245 | wait(1); |
Bitelli | 40:13d2c943dfe9 | 246 | } |
Bitelli | 40:13d2c943dfe9 | 247 | } |
Bitelli | 40:13d2c943dfe9 | 248 | } |
Bitelli | 40:13d2c943dfe9 | 249 | |
Bitelli | 40:13d2c943dfe9 | 250 | void home (){ |
Bitelli | 40:13d2c943dfe9 | 251 | motor1->go_home(); |
Bitelli | 40:13d2c943dfe9 | 252 | motor2->go_home(); |
Bitelli | 40:13d2c943dfe9 | 253 | motor3->go_home(); |
Bitelli | 40:13d2c943dfe9 | 254 | } |
Bitelli | 40:13d2c943dfe9 | 255 | |
Bitelli | 40:13d2c943dfe9 | 256 | void move_xy(int i,int x[],int y[]){ |
Bitelli | 40:13d2c943dfe9 | 257 | int xi=x[i]; |
Bitelli | 40:13d2c943dfe9 | 258 | int yi=y[i]; |
Bitelli | 40:13d2c943dfe9 | 259 | motor1->go_to(xi); |
Bitelli | 40:13d2c943dfe9 | 260 | motor2->go_to(yi); |
Bitelli | 40:13d2c943dfe9 | 261 | } |
Bitelli | 40:13d2c943dfe9 | 262 | |
Bitelli | 40:13d2c943dfe9 | 263 | /*-------------------------Variables---------------------------*/ |
Bitelli | 40:13d2c943dfe9 | 264 | |
Bitelli | 40:13d2c943dfe9 | 265 | int j=0; |
Bitelli | 40:13d2c943dfe9 | 266 | int positionX[20]; |
Bitelli | 40:13d2c943dfe9 | 267 | int positionY[20]; |
Bitelli | 40:13d2c943dfe9 | 268 | int referencia_z=0; |
Bitelli | 40:13d2c943dfe9 | 269 | |
Bitelli | 40:13d2c943dfe9 | 270 | /*---------------------- Initialization. --------------------*/ |
Bitelli | 40:13d2c943dfe9 | 271 | int main() { |
Bitelli | 40:13d2c943dfe9 | 272 | |
Bitelli | 40:13d2c943dfe9 | 273 | /* Initializing SPI bus. */ |
Bitelli | 40:13d2c943dfe9 | 274 | DevSPI dev_spi(D11, D12, D13); |
Bitelli | 40:13d2c943dfe9 | 275 | |
Bitelli | 40:13d2c943dfe9 | 276 | /* Initializing Motor Control Component. */ |
Bitelli | 40:13d2c943dfe9 | 277 | motor1 = new L6474(D2, D8, D7, D9, D10, dev_spi); |
Bitelli | 40:13d2c943dfe9 | 278 | motor2 = new L6474(D2, D8, D4, D3, D10, dev_spi); |
Bitelli | 40:13d2c943dfe9 | 279 | motor3 = new L6474(D2, D8, D5, D6, D10, dev_spi); |
Bitelli | 40:13d2c943dfe9 | 280 | if (motor1->init(&init) != COMPONENT_OK) { |
Bitelli | 40:13d2c943dfe9 | 281 | exit(EXIT_FAILURE); |
Bitelli | 40:13d2c943dfe9 | 282 | } |
Bitelli | 40:13d2c943dfe9 | 283 | if (motor2->init(&init) != COMPONENT_OK) { |
Bitelli | 40:13d2c943dfe9 | 284 | exit(EXIT_FAILURE); |
Bitelli | 40:13d2c943dfe9 | 285 | } |
Bitelli | 40:13d2c943dfe9 | 286 | if (motor3->init(&init) != COMPONENT_OK) { |
Bitelli | 40:13d2c943dfe9 | 287 | exit(EXIT_FAILURE); |
Bitelli | 40:13d2c943dfe9 | 288 | } |
Bitelli | 40:13d2c943dfe9 | 289 | |
Bitelli | 40:13d2c943dfe9 | 290 | |
Bitelli | 40:13d2c943dfe9 | 291 | pc.baud(9600); |
Bitelli | 40:13d2c943dfe9 | 292 | |
Bitelli | 40:13d2c943dfe9 | 293 | while(YES!=0){ |
Bitelli | 40:13d2c943dfe9 | 294 | pc.printf("\n\rZerar a mesa?"); |
Bitelli | 40:13d2c943dfe9 | 295 | } |
Bitelli | 40:13d2c943dfe9 | 296 | |
Bitelli | 40:13d2c943dfe9 | 297 | |
Bitelli | 40:13d2c943dfe9 | 298 | /*----------------------------Zerando a mesa-----------------*/ |
Bitelli | 40:13d2c943dfe9 | 299 | |
Bitelli | 40:13d2c943dfe9 | 300 | zeramento(); |
Bitelli | 40:13d2c943dfe9 | 301 | pc.printf("\n\rReferenciar eixo Z"); |
Bitelli | 40:13d2c943dfe9 | 302 | wait (2); |
Bitelli | 40:13d2c943dfe9 | 303 | int referencia_z=referenciar_z(referencia_z); |
Bitelli | 40:13d2c943dfe9 | 304 | pc.printf ("%d",referencia_z); |
Bitelli | 40:13d2c943dfe9 | 305 | home(); |
Bitelli | 40:13d2c943dfe9 | 306 | /*-------------------------Menu "Inicial"-----------------*/ |
Bitelli | 40:13d2c943dfe9 | 307 | |
Bitelli | 40:13d2c943dfe9 | 308 | pc.printf("\n\rMenu"); |
Bitelli | 40:13d2c943dfe9 | 309 | wait(1); |
Bitelli | 40:13d2c943dfe9 | 310 | |
Bitelli | 40:13d2c943dfe9 | 311 | |
Bitelli | 40:13d2c943dfe9 | 312 | /*-----------------------------Adiciontar Pontos------------------------*/ |
Bitelli | 40:13d2c943dfe9 | 313 | |
Bitelli | 40:13d2c943dfe9 | 314 | pc.printf ("\n\rX: %d", motor1->get_position()); |
Bitelli | 40:13d2c943dfe9 | 315 | pc.printf ("\n\rY: %d", motor2->get_position()); |
Bitelli | 40:13d2c943dfe9 | 316 | |
Bitelli | 40:13d2c943dfe9 | 317 | while(!=NO){ |
Bitelli | 40:13d2c943dfe9 | 318 | |
Bitelli | 40:13d2c943dfe9 | 319 | |
Bitelli | 40:13d2c943dfe9 | 320 | adicionar_pontos(j, positionX, positionY); |
Bitelli | 40:13d2c943dfe9 | 321 | |
Bitelli | 40:13d2c943dfe9 | 322 | |
Bitelli | 40:13d2c943dfe9 | 323 | /*-----------------------------Colar Pontos----------------------------*/ |
Bitelli | 40:13d2c943dfe9 | 324 | |
Bitelli | 40:13d2c943dfe9 | 325 | pc.printf ("\n\rIniciando..."); |
Bitelli | 40:13d2c943dfe9 | 326 | |
Bitelli | 40:13d2c943dfe9 | 327 | home(); |
Bitelli | 40:13d2c943dfe9 | 328 | |
Bitelli | 40:13d2c943dfe9 | 329 | pc.printf ("\n\rColando..."); |
Bitelli | 40:13d2c943dfe9 | 330 | |
Bitelli | 40:13d2c943dfe9 | 331 | for (int i=0;i<20;i++){ |
Bitelli | 40:13d2c943dfe9 | 332 | move_xy(i, positionX, positionY); |
Bitelli | 40:13d2c943dfe9 | 333 | motor3->go_to(referencia_z); |
Bitelli | 40:13d2c943dfe9 | 334 | //depositar(); |
Bitelli | 40:13d2c943dfe9 | 335 | motor3->go_home(); |
Bitelli | 40:13d2c943dfe9 | 336 | } |
Bitelli | 40:13d2c943dfe9 | 337 | |
Bitelli | 40:13d2c943dfe9 | 338 | /*------------------------Finalizar Operação/Zerar----------------------*/ |
Bitelli | 40:13d2c943dfe9 | 339 | home(); |
Bitelli | 40:13d2c943dfe9 | 340 | } |
Bitelli | 40:13d2c943dfe9 | 341 |