Diff: botoesnh_default.cpp

Revision:

40:13d2c943dfe9

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/botoesnh_default.cpp	Fri Apr 27 17:52:39 2018 +0000
@@ -0,0 +1,341 @@
+/* Includes ------------------------------------------------------------------*/
+
+/* mbed specific header files. */
+#include "mbed.h"
+
+/* Helper header files. */
+#include "DevSPI.h"
+
+/* Component specific header files. */
+#include "L6474.h"
+#include "new_file.cpp"
+
+/* Definitions ---------------------------------------------------------------*/
+
+/* Number of steps. */
+#define STEPS_1 (200 * 2)   /* 1 revolution given a 200 steps motor configured at 1/2 microstep mode. */
+#define safe (600)
+
+/* Delay in milliseconds. */
+#define DELAY_1 1000
+
+
+/* Speed in pps (Pulses Per Second).
+   In Full Step mode: 1 pps = 1 step/s).
+   In 1/N Step Mode:  N pps = 1 step/s). */
+#define SPEED_1 2400
+#define SPEED_2 1200
+
+DigitalOut myled(LED1);
+Serial pc(USBTX, USBRX);
+
+
+/* Variables -----------------------------------------------------------------*/
+
+/* Initialization parameters. */
+
+L6474_init_t init = {
+    160,                              /* Acceleration rate in pps^2. Range: (0..+inf). */
+    160,                              /* Deceleration rate in pps^2. Range: (0..+inf). */
+    2600,                             /* Maximum speed in pps. Range: (30..10000]. */
+    1300,                              /* Minimum speed in pps. Range: [30..10000). */
+    250,                              /* Torque regulation current in mA. Range: 31.25mA to 4000mA. */
+    L6474_OCD_TH_750mA,               /* Overcurrent threshold (OCD_TH register). */
+    L6474_CONFIG_OC_SD_ENABLE,        /* Overcurrent shutwdown (OC_SD field of CONFIG register). */
+    L6474_CONFIG_EN_TQREG_TVAL_USED,  /* Torque regulation method (EN_TQREG field of CONFIG register). */
+    L6474_STEP_SEL_1_16,              /* Step selection (STEP_SEL field of STEP_MODE register). */
+    L6474_SYNC_SEL_1_2,               /* Sync selection (SYNC_SEL field of STEP_MODE register). */
+    L6474_FAST_STEP_12us,             /* Fall time value (T_FAST field of T_FAST register). Range: 2us to 32us. */
+    L6474_TOFF_FAST_8us,              /* Maximum fast decay time (T_OFF field of T_FAST register). Range: 2us to 32us. */
+    3,                                /* Minimum ON time in us (TON_MIN register). Range: 0.5us to 64us. */
+    21,                               /* Minimum OFF time in us (TOFF_MIN register). Range: 0.5us to 64us. */
+    L6474_CONFIG_TOFF_044us,          /* Target Swicthing Period (field TOFF of CONFIG register). */
+    L6474_CONFIG_SR_320V_us,          /* Slew rate (POW_SR field of CONFIG register). */
+    L6474_CONFIG_INT_16MHZ,           /* Clock setting (OSC_CLK_SEL field of CONFIG register). */
+    L6474_ALARM_EN_OVERCURRENT |
+    L6474_ALARM_EN_THERMAL_SHUTDOWN |
+    L6474_ALARM_EN_THERMAL_WARNING |
+    L6474_ALARM_EN_UNDERVOLTAGE |
+    L6474_ALARM_EN_SW_TURN_ON |
+    L6474_ALARM_EN_WRONG_NPERF_CMD    /* Alarm (ALARM_EN register). */
+};
+
+/* Motor Control Component. */
+L6474 *motor1;
+L6474 *motor2;
+L6474 *motor3;
+
+
+
+/*---------------------Functions -------------------*/
+
+
+
+
+DigitalIn sUp(A0);
+DigitalIn sDown(A1);
+DigitalIn sLeft(A2);
+DigitalIn sRight(A3);
+DigitalIn YES(A4);
+DigitalIn NO(A5);
+DigitalIn zeroX(PC_8);
+DigitalIn zeroY(PC_6);
+DigitalIn zeroZ(PA_11);
+DigitalIn maxX(PC_5);
+DigitalIn maxY(PA_12);
+DigitalIn maxZ(PB_12);
+
+
+
+void fdc_inicialX(){
+    pc.printf("\n\rZerando eixo X");
+    motor1->run(StepperMotor::BWD);
+    while(zeroX ==1){
+        wait(0.01);
+        }
+    motor1->hard_stop();
+    motor1->move(StepperMotor::FWD, safe);
+    motor1->set_home();
+    pc.printf("\n\rEixo X zerado");
+}
+
+
+void fdc_inicialY(){
+    pc.printf("\n\rZerando eixo Y");
+    motor2->run(StepperMotor::BWD);
+    while(zeroY ==1){
+        wait(0.01);
+        }
+    motor2->hard_stop();
+    motor2->move(StepperMotor::FWD, safe);
+    motor2->set_home();
+    pc.printf("\n\rEixo Y zerado");    
+}
+
+
+void fdc_inicialZ(){
+    pc.printf("\n\rZerando eixo Z");
+    motor3->run(StepperMotor::BWD);
+    while(zeroZ ==1){
+        wait(0.01);
+        }
+    motor3->hard_stop();
+    motor3->move(StepperMotor::FWD, safe);
+    motor3->set_home(); 
+    pc.printf("\n\rEixo Z zerado");   
+}
+
+
+void fdc_zeroX(){
+    motor1->move(StepperMotor::FWD, safe);
+    pc.printf ("\n\rLimite do Eixo X");
+    pc.printf ("\n\rY: %d", motor2->get_position());
+    wait(2);
+}
+
+void fdc_zeroY(){
+    motor2->move(StepperMotor::FWD, safe);
+    pc.printf ("\n\rX: %d", motor1->get_position());
+    pc.printf ("\n\rLimite do Eixo Y");
+    wait(2);
+}
+
+void fdc_zeroZ(){
+    motor3->move(StepperMotor::FWD, safe);
+    pc.printf ("\n\rZ: %d", motor3->get_position());
+    pc.printf ("\n\rLimite do Eixo Z");
+    wait(2);
+}
+
+void fdc_maxX(){
+    motor1->move(StepperMotor::BWD, safe);
+    pc.printf ("\n\rLimite do Eixo X");
+    pc.printf ("\n\rY: %d", motor2->get_position());
+    wait(2);
+}
+
+void fdc_maxY(){
+    motor2->move(StepperMotor::BWD, safe);
+    pc.printf ("\n\rX: %d", motor1->get_position());
+    pc.printf ("\n\rLimite do Eixo Y");
+    wait(2);
+}
+
+
+void fdc_maxZ(){
+    motor3->move(StepperMotor::BWD, safe);
+    pc.printf ("\n\rZ: %d", motor3->get_position());
+    pc.printf ("\n\rLimite do Eixo Z");
+    wait(2);
+}                    
+                
+void zeramento(){
+    fdc_inicialX();
+    fdc_inicialY();
+    fdc_inicialZ();         
+}
+
+int referenciar_z(int referencia_z){
+    pc.printf("\n\rZ: %d", motor3->get_position());
+    while(NO!=0){
+        if (sUp==0){
+            motor3->move(StepperMotor::FWD,600);
+            pc.printf("\n\rZ: %d", motor3->get_position()); 
+        }
+        if (sDown==0){
+            motor3->move(StepperMotor::BWD,600);
+            pc.printf("\n\rZ: %d", motor3->get_position());
+        }
+        if (zeroZ ==0){
+            fdc_zeroZ();
+        }
+        if (maxZ ==0){
+            fdc_maxZ();
+        }
+        wait(0.1);
+        pc.printf ("\n\rZ: %d", motor3->get_position());
+        if (YES==0) {
+            pc.printf ("\n\rEixo Z referenciado!");
+            referencia_z=motor3->get_position();
+            wait(1);
+        }
+    }
+    return referencia_z;
+}
+int adicionar_pontos(int j,int positionX[], int positionY[]){
+    while(NO!=0) {
+        if (sUp==0) {
+            pc.printf ("\n\rX: %d", motor1->get_position());
+            pc.printf ("\n\rY: %d", motor2->get_position());
+            motor2->move(StepperMotor::FWD, 600);
+        }
+        if (sRight==0) {
+            pc.printf ("\n\rX: %d", motor1->get_position());
+            pc.printf ("\n\rY: %d", motor2->get_position());
+            motor1->move(StepperMotor::FWD, 600);
+        }       
+        if (sDown==0) {
+            pc.printf ("\n\rX: %d", motor1->get_position());
+            pc.printf ("\n\rY: %d", motor2->get_position());
+            motor2->move(StepperMotor::BWD, 600);
+        }
+        if (sLeft==0) {
+            pc.printf ("\n\rX: %d", motor1->get_position());
+            pc.printf ("\n\rY: %d", motor2->get_position());
+            motor1->move(StepperMotor::BWD, 600);
+        }
+        if (zeroX ==0){
+            fdc_zeroX();
+        }
+        if (zeroY ==0){
+            fdc_zeroY();
+        }
+        if (maxX ==0){
+            fdc_maxX();
+        }
+        if (maxY ==0){
+            fdc_maxY();
+        }    
+        wait(0.2);
+        if (YES==0) {
+            pc.printf ("\n\rPonto Adicionado!");
+            positionX[j]=motor1->get_position();
+            positionY[j]=motor2->get_position();
+            j++;
+            wait(1);
+        }
+    }
+}
+
+void home (){
+    motor1->go_home();
+    motor2->go_home();
+    motor3->go_home();
+}
+
+void move_xy(int i,int x[],int y[]){
+    int xi=x[i];
+    int yi=y[i];
+    motor1->go_to(xi);
+    motor2->go_to(yi);
+}
+
+/*-------------------------Variables---------------------------*/
+
+int j=0;
+int positionX[20];
+int positionY[20];
+int referencia_z=0;
+   
+/*---------------------- Initialization. --------------------*/
+int main() {
+
+    /* Initializing SPI bus. */
+    DevSPI dev_spi(D11, D12, D13);
+
+    /* Initializing Motor Control Component. */
+    motor1 = new L6474(D2, D8, D7, D9, D10, dev_spi);
+    motor2 = new L6474(D2, D8, D4, D3, D10, dev_spi);
+    motor3 = new L6474(D2, D8, D5, D6, D10, dev_spi);
+    if (motor1->init(&init) != COMPONENT_OK) {
+        exit(EXIT_FAILURE);
+    }
+    if (motor2->init(&init) != COMPONENT_OK) {
+        exit(EXIT_FAILURE);
+    }
+    if (motor3->init(&init) != COMPONENT_OK) {
+        exit(EXIT_FAILURE);
+    }
+ 
+  
+    pc.baud(9600);
+  
+    while(YES!=0){
+        pc.printf("\n\rZerar a mesa?");
+    }
+    
+    
+/*----------------------------Zerando a mesa-----------------*/
+    
+    zeramento();
+    pc.printf("\n\rReferenciar eixo Z");
+    wait (2);
+    int referencia_z=referenciar_z(referencia_z);  
+    pc.printf ("%d",referencia_z);  
+    home();         
+/*-------------------------Menu "Inicial"-----------------*/
+
+    pc.printf("\n\rMenu");
+    wait(1);
+    
+
+/*-----------------------------Adiciontar Pontos------------------------*/
+    
+    pc.printf ("\n\rX: %d", motor1->get_position());
+    pc.printf ("\n\rY: %d", motor2->get_position());
+    
+    while(!=NO){
+        
+        
+    adicionar_pontos(j, positionX, positionY);
+    
+
+/*-----------------------------Colar Pontos----------------------------*/
+    
+    pc.printf ("\n\rIniciando...");
+    
+    home();
+    
+    pc.printf ("\n\rColando...");
+    
+    for (int i=0;i<20;i++){
+        move_xy(i, positionX, positionY);
+        motor3->go_to(referencia_z);
+        //depositar();
+        motor3->go_home();
+    }  
+
+/*------------------------Finalizar Operação/Zerar----------------------*/
+    home();
+}
+