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?
User | Revision | Line number | New 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 | } |