Diff: main.cpp

Revision:

0:2db5466698f9

Child:

1:1a1df03a2dc1

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Jul 27 00:19:58 2017 +0000
@@ -0,0 +1,202 @@
+#include "mbed.h"
+#include "motordriver.h"
+
+DigitalOut STBY(p15);//p7
+PwmOut PWMA(p26);//p21
+DigitalOut AIN1(p30);//p5
+DigitalOut AIN2(p29);//p6
+PwmOut PWMB(p25);//p22
+DigitalOut BIN1(p14);//p8
+DigitalOut BIN2(p13);//p9
+
+Serial pc(USBTX,USBRX);   //Configura comunicación serial en Bus USB
+//Configuración de interrupciones
+//p10,p11,p12,p13
+InterruptIn INT0(p22);//10     //Interrupción externa 0  CHA1
+InterruptIn INT1(p21);//11 //Interrupción externa 1  CHB1
+InterruptIn INT2(p23);//12     //Interrupción externa 2  CHA2
+InterruptIn INT3(p24);//13     //Interrupción externa 3  CHB2
+int CNT1, CNT2; //Contadores de posición para encoders
+bool flagCHA1, flagCHB1, flagCHA2, flagCHB2;
+void printCNT1(void){
+    //pc.printf("Conteo :%ld ",CNT1);
+}
+void printCNT2(void){
+    //pc.printf("Conteo_2 :%ld ",CNT2);
+}
+void risingINT0()
+{
+    flagCHA1=1;
+    if (flagCHB1 == 0) {
+        CNT1 = CNT1 - 1;    //Lectura de encoder en cuadratura
+    } else {
+        CNT1 = CNT1 + 1;    //Decremento en cuadratura
+    }
+    printCNT1();
+}
+void fallingINT0()
+{
+    flagCHA1 = 0;
+    if (flagCHB1 == 1) {
+        CNT1 = CNT1 - 1;    //Incremento en cuadratura
+    } else {
+        CNT1 = CNT1 + 1;    //Decremento en cuadratura
+    }
+    printCNT1();
+}
+void risingINT1()
+{
+    flagCHB1=1;
+    if (flagCHA1 == 0) {
+        CNT1 = CNT1 + 1;    //Lectura de encoder en cuadratura
+    } else {
+        CNT1 = CNT1 - 1;    //Decremento en cuadratura
+    }
+    printCNT1();
+}
+void fallingINT1()
+{
+    flagCHB1 = 0;
+    if (flagCHA1 == 1) {
+        CNT1 = CNT1 + 1;    //Incremento en cuadratura
+    } else {
+        CNT1 = CNT1 - 1;    //Decremento en cuadratura
+    }
+    printCNT1();
+}
+void risingINT2()
+{
+    flagCHA2=1;
+    if (flagCHB2 == 0) {
+        CNT2 = CNT2 - 1;    //Lectura de encoder en cuadratura
+    } else {
+        CNT2 = CNT2 + 1;    //Decremento en cuadratura
+    }
+    printCNT2();
+}
+void fallingINT2()
+{
+    flagCHA2 = 0;
+    if (flagCHB2 == 1) {
+        CNT2 = CNT2 - 1;    //Incremento en cuadratura
+    } else {
+        CNT2 = CNT2 + 1;    //Decremento en cuadratura
+    }
+    printCNT2();
+}
+void risingINT3()
+{
+    flagCHB2=1;
+    if (flagCHA2 == 0) {
+        CNT2 = CNT2 + 1;    //Lectura de encoder en cuadratura
+    } else {
+        CNT2 = CNT2 - 1;    //Decremento en cuadratura
+    }
+    printCNT2();
+}
+void fallingINT3()
+{
+    flagCHB2 = 0;
+    if (flagCHA2 == 1) {
+        CNT2 = CNT2 + 1;    //Incremento en cuadratura
+    } else {
+        CNT2 = CNT2 - 1;    //Decremento en cuadratura
+    }
+    printCNT2();
+}
+void initialize()
+{
+    //Configuración de Interrupciones
+    INT0.rise(&risingINT0);
+    INT0.fall(&fallingINT0);
+    INT1.rise(&risingINT1);
+    INT1.fall(&fallingINT1);
+    INT2.rise(&risingINT2);
+    INT2.fall(&fallingINT2);
+    INT3.rise(&risingINT3);
+    INT3.fall(&fallingINT3); 
+}
+void adelante(int velocidad ){    //Vamos a definir la función mover, que va a acciona un motor, fijar su velocidad y el                                                                                            sentido de giro. Definimos:    //motor:                                                                                                    llamaremos 1 al motor A, y 2 al motor B
+                                          //velocidad: desde 0 a 255
+  STBY = 1;           //deshabilitar standby para mover
+  AIN1 = 1;
+  AIN2 = 0;
+  PWMA = velocidad;
+  BIN1 = 1;
+  BIN2 = 0;
+  PWMB = velocidad;
+}
+void giro_derecha(int velocidad){    //Vamos a definir la función mover, que va a acciona un motor, fijar su velocidad y el                                                                                            sentido de giro. Definimos:    //motor:                                                                                                    llamaremos 1 al motor A, y 2 al motor B
+          
+  STBY = 1;        
+  AIN1 = 1;
+  AIN2 = 0;
+  PWMA = velocidad;
+  BIN1 = 0;
+  BIN2 = 1;
+  PWMB = velocidad;
+}
+
+void giro_izquierda(int velocidad){    //Vamos a definir la función mover, que va a acciona un motor, fijar su velocidad y el                                                                                            sentido de giro. Definimos:    //motor:                                                                                                    llamaremos 1 al motor A, y 2 al motor B
+           
+  STBY = 1;        
+  AIN1 = 0;
+  AIN2 = 1;
+  PWMA = velocidad;
+  BIN1 = 1;
+  BIN2 = 0;
+  PWMB = velocidad;
+}
+void stop(){                                        
+  STBY = 0;
+}
+int main(){
+  
+    CNT1 = 0;
+    CNT2 = 0;
+    //int CNT = 0;
+    //int aux = 0;
+    
+    while(1){
+    BIN1 = 1;
+    BIN2 = 0;
+    PWMA = 2;
+    AIN1 = 1;
+    AIN2 = 0;
+    PWMA = 2;
+    }
+    
+    //pc.baud(115200);
+    //pc.printf("Hello World from FRDM-K64F board.\n");
+    /*
+    while(CNT < 4){
+    adelante(1);
+    wait_ms(1000); 
+    //pc.printf("adelante");
+    aux = CNT1;  
+    
+    //pc.printf("giro");
+    while(aux+90 > CNT1)
+    {
+        initialize();
+        giro_izquierda(2);
+        //pc.printf("Giro derecha");
+        CNT1++;
+    }
+    wait_ms(1000);
+    
+    //pc.printf("giro");
+    stop();  
+    aux = CNT1;   
+    while(aux+90 > CNT1) // una vuelta
+    {
+        initialize();
+        giro_izquierda(2);
+        pc.printf("Giro izquierda");
+        //CNT1++;
+    }
+    aux = CNT1;  
+    wait_ms(1000);
+    CNT++;
+    stop(); */ 
+}
\ No newline at end of file