main.cpp

00001 #include "mbed.h"
00002 #include "motordriver.h"
00003 //Esto no es un comentario pero parece comentario
00004 DigitalOut STBY(p15);//p7
00005 PwmOut PWMA(p26);//p21
00006 DigitalOut AIN1(p30);//p5
00007 DigitalOut AIN2(p29);//p6
00008 PwmOut PWMB(p25);//p22
00009 DigitalOut BIN1(p14);//p8
00010 DigitalOut BIN2(p13);//p9
00011 
00012 Serial pc(USBTX,USBRX);   //Configura comunicación serial en Bus USB
00013 //Configuración de interrupciones
00014 //p10,p11,p12,p13
00015 InterruptIn INT0(p22);//10     //Interrupción externa 0  CHA1
00016 InterruptIn INT1(p21);//11 //Interrupción externa 1  CHB1
00017 InterruptIn INT2(p23);//12     //Interrupción externa 2  CHA2
00018 InterruptIn INT3(p24);//13     //Interrupción externa 3  CHB2
00019 int CNT1, CNT2; //Contadores de posición para encoders
00020 bool flagCHA1, flagCHB1, flagCHA2, flagCHB2;
00021 void printCNT1(void){
00022     //pc.printf("Conteo :%ld ",CNT1);
00023 }
00024 void printCNT2(void){
00025     //pc.printf("Conteo_2 :%ld ",CNT2);
00026 }
00027 void risingINT0()
00028 {
00029     flagCHA1=1;
00030     if (flagCHB1 == 0) {
00031         CNT1 = CNT1 - 1;    //Lectura de encoder en cuadratura
00032     } else {
00033         CNT1 = CNT1 + 1;    //Decremento en cuadratura
00034     }
00035     printCNT1();
00036 }
00037 void fallingINT0()
00038 {
00039     flagCHA1 = 0;
00040     if (flagCHB1 == 1) {
00041         CNT1 = CNT1 - 1;    //Incremento en cuadratura
00042     } else {
00043         CNT1 = CNT1 + 1;    //Decremento en cuadratura
00044     }
00045     printCNT1();
00046 }
00047 void risingINT1()
00048 {
00049     flagCHB1=1;
00050     if (flagCHA1 == 0) {
00051         CNT1 = CNT1 + 1;    //Lectura de encoder en cuadratura
00052     } else {
00053         CNT1 = CNT1 - 1;    //Decremento en cuadratura
00054     }
00055     printCNT1();
00056 }
00057 void fallingINT1()
00058 {
00059     flagCHB1 = 0;
00060     if (flagCHA1 == 1) {
00061         CNT1 = CNT1 + 1;    //Incremento en cuadratura
00062     } else {
00063         CNT1 = CNT1 - 1;    //Decremento en cuadratura
00064     }
00065     printCNT1();
00066 }
00067 void risingINT2()
00068 {
00069     flagCHA2=1;
00070     if (flagCHB2 == 0) {
00071         CNT2 = CNT2 - 1;    //Lectura de encoder en cuadratura
00072     } else {
00073         CNT2 = CNT2 + 1;    //Decremento en cuadratura
00074     }
00075     printCNT2();
00076 }
00077 void fallingINT2()
00078 {
00079     flagCHA2 = 0;
00080     if (flagCHB2 == 1) {
00081         CNT2 = CNT2 - 1;    //Incremento en cuadratura
00082     } else {
00083         CNT2 = CNT2 + 1;    //Decremento en cuadratura
00084     }
00085     printCNT2();
00086 }
00087 void risingINT3()
00088 {
00089     flagCHB2=1;
00090     if (flagCHA2 == 0) {
00091         CNT2 = CNT2 + 1;    //Lectura de encoder en cuadratura
00092     } else {
00093         CNT2 = CNT2 - 1;    //Decremento en cuadratura
00094     }
00095     printCNT2();
00096 }
00097 void fallingINT3()
00098 {
00099     flagCHB2 = 0;
00100     if (flagCHA2 == 1) {
00101         CNT2 = CNT2 + 1;    //Incremento en cuadratura
00102     } else {
00103         CNT2 = CNT2 - 1;    //Decremento en cuadratura
00104     }
00105     printCNT2();
00106 }
00107 void initialize()
00108 {
00109     //Configuración de Interrupciones
00110     INT0.rise(&risingINT0);
00111     INT0.fall(&fallingINT0);
00112     INT1.rise(&risingINT1);
00113     INT1.fall(&fallingINT1);
00114     INT2.rise(&risingINT2);
00115     INT2.fall(&fallingINT2);
00116     INT3.rise(&risingINT3);
00117     INT3.fall(&fallingINT3); 
00118 }
00119 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
00120                                           //velocidad: desde 0 a 255
00121   STBY = 1;           //deshabilitar standby para mover
00122   AIN1 = 1;
00123   AIN2 = 0;
00124   PWMA = velocidad;
00125   BIN1 = 1;
00126   BIN2 = 0;
00127   PWMB = velocidad;
00128 }
00129 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
00130           
00131   STBY = 1;        
00132   AIN1 = 1;
00133   AIN2 = 0;
00134   PWMA = velocidad;
00135   BIN1 = 0;
00136   BIN2 = 1;
00137   PWMB = velocidad;
00138 }
00139 
00140 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
00141            
00142   STBY = 1;        
00143   AIN1 = 0;
00144   AIN2 = 1;
00145   PWMA = velocidad;
00146   BIN1 = 1;
00147   BIN2 = 0;
00148   PWMB = velocidad;
00149 }
00150 void stop(){                                        
00151   STBY = 0;
00152 }
00153 int main(){
00154   
00155     CNT1 = 0;
00156     CNT2 = 0;
00157     //int CNT = 0;
00158     //int aux = 0;
00159     
00160     while(1){
00161     BIN1 = 1;
00162     BIN2 = 0;
00163     PWMA = 2;
00164     AIN1 = 1;
00165     AIN2 = 0;
00166     PWMA = 2;
00167     }
00168     
00169     //pc.baud(115200);
00170     //pc.printf("Hello World from FRDM-K64F board.\n");
00171     /*
00172     while(CNT < 4){
00173     adelante(1);
00174     wait_ms(1000); 
00175     //pc.printf("adelante");
00176     aux = CNT1;  
00177     
00178     //pc.printf("giro");
00179     while(aux+90 > CNT1)
00180     {
00181         initialize();
00182         giro_izquierda(2);
00183         //pc.printf("Giro derecha");
00184         CNT1++;
00185     }
00186     wait_ms(1000);
00187     
00188     //pc.printf("giro");
00189     stop();  
00190     aux = CNT1;   
00191     while(aux+90 > CNT1) // una vuelta
00192     {
00193         initialize();
00194         giro_izquierda(2);
00195         pc.printf("Giro izquierda");
00196         //CNT1++;
00197     }
00198     aux = CNT1;  
00199     wait_ms(1000);
00200     CNT++;
00201     stop(); */ 
00202 }