asd
Dependencies: Motordriver mbed
main.cpp
- Committer:
- Janice_Rafael
- Date:
- 2017-07-27
- Revision:
- 0:2db5466698f9
- Child:
- 1:1a1df03a2dc1
File content as of revision 0:2db5466698f9:
#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(); */ }