UC-GRUPO DE ESTUDIO-INTELIGENCIA ARTIFICIAL / Mbed 2 deprecated motoresMicromouse

asd

Dependencies:   Motordriver mbed

main.cpp@2:3204cde81db7, 2017-07-27 (annotated)

Committer:
jhony
Date:
Thu Jul 27 00:25:59 2017 +0000
Revision:
2:3204cde81db7
Parent:
1:1a1df03a2dc1 
...;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Janice_Rafael 0:2db5466698f9 1 #include "mbed.h"
Janice_Rafael 0:2db5466698f9 2 #include "motordriver.h"
jhony 2:3204cde81db7 3 //Esto no es un comentario pero parece comentario
Janice_Rafael 0:2db5466698f9 4 DigitalOut STBY(p15);//p7
Janice_Rafael 0:2db5466698f9 5 PwmOut PWMA(p26);//p21
Janice_Rafael 0:2db5466698f9 6 DigitalOut AIN1(p30);//p5
Janice_Rafael 0:2db5466698f9 7 DigitalOut AIN2(p29);//p6
Janice_Rafael 0:2db5466698f9 8 PwmOut PWMB(p25);//p22
Janice_Rafael 0:2db5466698f9 9 DigitalOut BIN1(p14);//p8
Janice_Rafael 0:2db5466698f9 10 DigitalOut BIN2(p13);//p9
Janice_Rafael 0:2db5466698f9 11
Janice_Rafael 0:2db5466698f9 12 Serial pc(USBTX,USBRX); //Configura comunicación serial en Bus USB
Janice_Rafael 0:2db5466698f9 13 //Configuración de interrupciones
Janice_Rafael 0:2db5466698f9 14 //p10,p11,p12,p13
Janice_Rafael 0:2db5466698f9 15 InterruptIn INT0(p22);//10 //Interrupción externa 0 CHA1
Janice_Rafael 0:2db5466698f9 16 InterruptIn INT1(p21);//11 //Interrupción externa 1 CHB1
Janice_Rafael 0:2db5466698f9 17 InterruptIn INT2(p23);//12 //Interrupción externa 2 CHA2
Janice_Rafael 0:2db5466698f9 18 InterruptIn INT3(p24);//13 //Interrupción externa 3 CHB2
Janice_Rafael 0:2db5466698f9 19 int CNT1, CNT2; //Contadores de posición para encoders
Janice_Rafael 0:2db5466698f9 20 bool flagCHA1, flagCHB1, flagCHA2, flagCHB2;
Janice_Rafael 0:2db5466698f9 21 void printCNT1(void){
Janice_Rafael 0:2db5466698f9 22 //pc.printf("Conteo :%ld ",CNT1);
Janice_Rafael 0:2db5466698f9 23 }
Janice_Rafael 0:2db5466698f9 24 void printCNT2(void){
Janice_Rafael 0:2db5466698f9 25 //pc.printf("Conteo_2 :%ld ",CNT2);
Janice_Rafael 0:2db5466698f9 26 }
Janice_Rafael 0:2db5466698f9 27 void risingINT0()
Janice_Rafael 0:2db5466698f9 28 {
Janice_Rafael 0:2db5466698f9 29 flagCHA1=1;
Janice_Rafael 0:2db5466698f9 30 if (flagCHB1 == 0) {
Janice_Rafael 0:2db5466698f9 31 CNT1 = CNT1 - 1; //Lectura de encoder en cuadratura
Janice_Rafael 0:2db5466698f9 32 } else {
Janice_Rafael 0:2db5466698f9 33 CNT1 = CNT1 + 1; //Decremento en cuadratura
Janice_Rafael 0:2db5466698f9 34 }
Janice_Rafael 0:2db5466698f9 35 printCNT1();
Janice_Rafael 0:2db5466698f9 36 }
Janice_Rafael 0:2db5466698f9 37 void fallingINT0()
Janice_Rafael 0:2db5466698f9 38 {
Janice_Rafael 0:2db5466698f9 39 flagCHA1 = 0;
Janice_Rafael 0:2db5466698f9 40 if (flagCHB1 == 1) {
Janice_Rafael 0:2db5466698f9 41 CNT1 = CNT1 - 1; //Incremento en cuadratura
Janice_Rafael 0:2db5466698f9 42 } else {
Janice_Rafael 0:2db5466698f9 43 CNT1 = CNT1 + 1; //Decremento en cuadratura
Janice_Rafael 0:2db5466698f9 44 }
Janice_Rafael 0:2db5466698f9 45 printCNT1();
Janice_Rafael 0:2db5466698f9 46 }
Janice_Rafael 0:2db5466698f9 47 void risingINT1()
Janice_Rafael 0:2db5466698f9 48 {
Janice_Rafael 0:2db5466698f9 49 flagCHB1=1;
Janice_Rafael 0:2db5466698f9 50 if (flagCHA1 == 0) {
Janice_Rafael 0:2db5466698f9 51 CNT1 = CNT1 + 1; //Lectura de encoder en cuadratura
Janice_Rafael 0:2db5466698f9 52 } else {
Janice_Rafael 0:2db5466698f9 53 CNT1 = CNT1 - 1; //Decremento en cuadratura
Janice_Rafael 0:2db5466698f9 54 }
Janice_Rafael 0:2db5466698f9 55 printCNT1();
Janice_Rafael 0:2db5466698f9 56 }
Janice_Rafael 0:2db5466698f9 57 void fallingINT1()
Janice_Rafael 0:2db5466698f9 58 {
Janice_Rafael 0:2db5466698f9 59 flagCHB1 = 0;
Janice_Rafael 0:2db5466698f9 60 if (flagCHA1 == 1) {
Janice_Rafael 0:2db5466698f9 61 CNT1 = CNT1 + 1; //Incremento en cuadratura
Janice_Rafael 0:2db5466698f9 62 } else {
Janice_Rafael 0:2db5466698f9 63 CNT1 = CNT1 - 1; //Decremento en cuadratura
Janice_Rafael 0:2db5466698f9 64 }
Janice_Rafael 0:2db5466698f9 65 printCNT1();
Janice_Rafael 0:2db5466698f9 66 }
Janice_Rafael 0:2db5466698f9 67 void risingINT2()
Janice_Rafael 0:2db5466698f9 68 {
Janice_Rafael 0:2db5466698f9 69 flagCHA2=1;
Janice_Rafael 0:2db5466698f9 70 if (flagCHB2 == 0) {
Janice_Rafael 0:2db5466698f9 71 CNT2 = CNT2 - 1; //Lectura de encoder en cuadratura
Janice_Rafael 0:2db5466698f9 72 } else {
Janice_Rafael 0:2db5466698f9 73 CNT2 = CNT2 + 1; //Decremento en cuadratura
Janice_Rafael 0:2db5466698f9 74 }
Janice_Rafael 0:2db5466698f9 75 printCNT2();
Janice_Rafael 0:2db5466698f9 76 }
Janice_Rafael 0:2db5466698f9 77 void fallingINT2()
Janice_Rafael 0:2db5466698f9 78 {
Janice_Rafael 0:2db5466698f9 79 flagCHA2 = 0;
Janice_Rafael 0:2db5466698f9 80 if (flagCHB2 == 1) {
Janice_Rafael 0:2db5466698f9 81 CNT2 = CNT2 - 1; //Incremento en cuadratura
Janice_Rafael 0:2db5466698f9 82 } else {
Janice_Rafael 0:2db5466698f9 83 CNT2 = CNT2 + 1; //Decremento en cuadratura
Janice_Rafael 0:2db5466698f9 84 }
Janice_Rafael 0:2db5466698f9 85 printCNT2();
Janice_Rafael 0:2db5466698f9 86 }
Janice_Rafael 0:2db5466698f9 87 void risingINT3()
Janice_Rafael 0:2db5466698f9 88 {
Janice_Rafael 0:2db5466698f9 89 flagCHB2=1;
Janice_Rafael 0:2db5466698f9 90 if (flagCHA2 == 0) {
Janice_Rafael 0:2db5466698f9 91 CNT2 = CNT2 + 1; //Lectura de encoder en cuadratura
Janice_Rafael 0:2db5466698f9 92 } else {
Janice_Rafael 0:2db5466698f9 93 CNT2 = CNT2 - 1; //Decremento en cuadratura
Janice_Rafael 0:2db5466698f9 94 }
Janice_Rafael 0:2db5466698f9 95 printCNT2();
Janice_Rafael 0:2db5466698f9 96 }
Janice_Rafael 0:2db5466698f9 97 void fallingINT3()
Janice_Rafael 0:2db5466698f9 98 {
Janice_Rafael 0:2db5466698f9 99 flagCHB2 = 0;
Janice_Rafael 0:2db5466698f9 100 if (flagCHA2 == 1) {
Janice_Rafael 0:2db5466698f9 101 CNT2 = CNT2 + 1; //Incremento en cuadratura
Janice_Rafael 0:2db5466698f9 102 } else {
Janice_Rafael 0:2db5466698f9 103 CNT2 = CNT2 - 1; //Decremento en cuadratura
Janice_Rafael 0:2db5466698f9 104 }
Janice_Rafael 0:2db5466698f9 105 printCNT2();
Janice_Rafael 0:2db5466698f9 106 }
Janice_Rafael 0:2db5466698f9 107 void initialize()
Janice_Rafael 0:2db5466698f9 108 {
Janice_Rafael 0:2db5466698f9 109 //Configuración de Interrupciones
Janice_Rafael 0:2db5466698f9 110 INT0.rise(&risingINT0);
Janice_Rafael 0:2db5466698f9 111 INT0.fall(&fallingINT0);
Janice_Rafael 0:2db5466698f9 112 INT1.rise(&risingINT1);
Janice_Rafael 0:2db5466698f9 113 INT1.fall(&fallingINT1);
Janice_Rafael 0:2db5466698f9 114 INT2.rise(&risingINT2);
Janice_Rafael 0:2db5466698f9 115 INT2.fall(&fallingINT2);
Janice_Rafael 0:2db5466698f9 116 INT3.rise(&risingINT3);
Janice_Rafael 0:2db5466698f9 117 INT3.fall(&fallingINT3);
Janice_Rafael 0:2db5466698f9 118 }
Janice_Rafael 0:2db5466698f9 119 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
Janice_Rafael 0:2db5466698f9 120 //velocidad: desde 0 a 255
Janice_Rafael 0:2db5466698f9 121 STBY = 1; //deshabilitar standby para mover
Janice_Rafael 0:2db5466698f9 122 AIN1 = 1;
Janice_Rafael 0:2db5466698f9 123 AIN2 = 0;
Janice_Rafael 0:2db5466698f9 124 PWMA = velocidad;
Janice_Rafael 0:2db5466698f9 125 BIN1 = 1;
Janice_Rafael 0:2db5466698f9 126 BIN2 = 0;
Janice_Rafael 0:2db5466698f9 127 PWMB = velocidad;
Janice_Rafael 0:2db5466698f9 128 }
Janice_Rafael 0:2db5466698f9 129 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
Janice_Rafael 0:2db5466698f9 130
Janice_Rafael 0:2db5466698f9 131 STBY = 1;
Janice_Rafael 0:2db5466698f9 132 AIN1 = 1;
Janice_Rafael 0:2db5466698f9 133 AIN2 = 0;
Janice_Rafael 0:2db5466698f9 134 PWMA = velocidad;
Janice_Rafael 0:2db5466698f9 135 BIN1 = 0;
Janice_Rafael 0:2db5466698f9 136 BIN2 = 1;
Janice_Rafael 0:2db5466698f9 137 PWMB = velocidad;
Janice_Rafael 0:2db5466698f9 138 }
Janice_Rafael 0:2db5466698f9 139
Janice_Rafael 0:2db5466698f9 140 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
Janice_Rafael 0:2db5466698f9 141
Janice_Rafael 0:2db5466698f9 142 STBY = 1;
Janice_Rafael 0:2db5466698f9 143 AIN1 = 0;
Janice_Rafael 0:2db5466698f9 144 AIN2 = 1;
Janice_Rafael 0:2db5466698f9 145 PWMA = velocidad;
Janice_Rafael 0:2db5466698f9 146 BIN1 = 1;
Janice_Rafael 0:2db5466698f9 147 BIN2 = 0;
Janice_Rafael 0:2db5466698f9 148 PWMB = velocidad;
Janice_Rafael 0:2db5466698f9 149 }
Janice_Rafael 0:2db5466698f9 150 void stop(){
Janice_Rafael 0:2db5466698f9 151 STBY = 0;
Janice_Rafael 0:2db5466698f9 152 }
Janice_Rafael 0:2db5466698f9 153 int main(){
Janice_Rafael 0:2db5466698f9 154
Janice_Rafael 0:2db5466698f9 155 CNT1 = 0;
Janice_Rafael 0:2db5466698f9 156 CNT2 = 0;
Janice_Rafael 0:2db5466698f9 157 //int CNT = 0;
Janice_Rafael 0:2db5466698f9 158 //int aux = 0;
Janice_Rafael 0:2db5466698f9 159
Janice_Rafael 0:2db5466698f9 160 while(1){
Janice_Rafael 0:2db5466698f9 161 BIN1 = 1;
Janice_Rafael 0:2db5466698f9 162 BIN2 = 0;
Janice_Rafael 0:2db5466698f9 163 PWMA = 2;
Janice_Rafael 0:2db5466698f9 164 AIN1 = 1;
Janice_Rafael 0:2db5466698f9 165 AIN2 = 0;
Janice_Rafael 0:2db5466698f9 166 PWMA = 2;
Janice_Rafael 0:2db5466698f9 167 }
Janice_Rafael 0:2db5466698f9 168
Janice_Rafael 0:2db5466698f9 169 //pc.baud(115200);
Janice_Rafael 0:2db5466698f9 170 //pc.printf("Hello World from FRDM-K64F board.\n");
Janice_Rafael 0:2db5466698f9 171 /*
Janice_Rafael 0:2db5466698f9 172 while(CNT < 4){
Janice_Rafael 0:2db5466698f9 173 adelante(1);
Janice_Rafael 0:2db5466698f9 174 wait_ms(1000);
Janice_Rafael 0:2db5466698f9 175 //pc.printf("adelante");
Janice_Rafael 0:2db5466698f9 176 aux = CNT1;
Janice_Rafael 0:2db5466698f9 177
Janice_Rafael 0:2db5466698f9 178 //pc.printf("giro");
Janice_Rafael 0:2db5466698f9 179 while(aux+90 > CNT1)
Janice_Rafael 0:2db5466698f9 180 {
Janice_Rafael 0:2db5466698f9 181 initialize();
Janice_Rafael 0:2db5466698f9 182 giro_izquierda(2);
Janice_Rafael 0:2db5466698f9 183 //pc.printf("Giro derecha");
Janice_Rafael 0:2db5466698f9 184 CNT1++;
Janice_Rafael 0:2db5466698f9 185 }
Janice_Rafael 0:2db5466698f9 186 wait_ms(1000);
Janice_Rafael 0:2db5466698f9 187
Janice_Rafael 0:2db5466698f9 188 //pc.printf("giro");
Janice_Rafael 0:2db5466698f9 189 stop();
Janice_Rafael 0:2db5466698f9 190 aux = CNT1;
Janice_Rafael 0:2db5466698f9 191 while(aux+90 > CNT1) // una vuelta
Janice_Rafael 0:2db5466698f9 192 {
Janice_Rafael 0:2db5466698f9 193 initialize();
Janice_Rafael 0:2db5466698f9 194 giro_izquierda(2);
Janice_Rafael 0:2db5466698f9 195 pc.printf("Giro izquierda");
Janice_Rafael 0:2db5466698f9 196 //CNT1++;
Janice_Rafael 0:2db5466698f9 197 }
Janice_Rafael 0:2db5466698f9 198 aux = CNT1;
Janice_Rafael 0:2db5466698f9 199 wait_ms(1000);
Janice_Rafael 0:2db5466698f9 200 CNT++;
Janice_Rafael 0:2db5466698f9 201 stop(); */
Janice_Rafael 0:2db5466698f9 202 }