asd
Dependencies: Motordriver mbed
Diff: main.cpp
- Revision:
- 0:2db5466698f9
- Child:
- 1:1a1df03a2dc1
diff -r 000000000000 -r 2db5466698f9 main.cpp --- /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