Diego Hamilton / Mbed 2 deprecated Robo

fon7

Dependencies:   MMA8451Q TextLCD mbed

main.cpp@0:18c127542958, 2017-06-21 (annotated)

Committer:
sid26
Date:
Wed Jun 21 18:15:49 2017 +0000
Revision:
0:18c127542958
Afonso idiota

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sid26 0:18c127542958 1 #include "mbed.h"
sid26 0:18c127542958 2 #include "MMA8451Q.h"
sid26 0:18c127542958 3 //#include "TextLCD.h"
sid26 0:18c127542958 4 #define MMA8451_I2C_ADDRESS (0x1d<<1)
sid26 0:18c127542958 5
sid26 0:18c127542958 6 Serial pc(USBTX, USBRX); //tx, rx (tx,rx) -> (tx, rx)
sid26 0:18c127542958 7 //TextLCD display(PTC9, PTC8, PTA5, PTA4, PTA12, PTD4, TextLCD::LCD16x2); // rs, e, d4-d7
sid26 0:18c127542958 8 MMA8451Q acc(PTE25, PTE24, MMA8451_I2C_ADDRESS);
sid26 0:18c127542958 9 PwmOut RB(PTB0); //direita - ré (verde) |
sid26 0:18c127542958 10 PwmOut RF(PTB1); //direita - frente (azul) |
sid26 0:18c127542958 11 PwmOut LB(PTB2); //esquerda - ré (amarelo) ---> MUDAR PINAGEM
sid26 0:18c127542958 12 PwmOut LF(PTB3); //esquerda - frente (laranja) |
sid26 0:18c127542958 13 DigitalOut led_direita(PTD3); //
sid26 0:18c127542958 14 DigitalOut led_esquerda(PTD2); // INSERT LEDS
sid26 0:18c127542958 15 DigitalOut led_frente(PTD0); //
sid26 0:18c127542958 16 DigitalOut led_tras(PTD5); //
sid26 0:18c127542958 17
sid26 0:18c127542958 18 float x1, x2; //
sid26 0:18c127542958 19 float y, y2; // acelerações
sid26 0:18c127542958 20 float z1, z2; //
sid26 0:18c127542958 21
sid26 0:18c127542958 22 float v = 0; //velocidade do motor
sid26 0:18c127542958 23 char c; //serial buffer
sid26 0:18c127542958 24
sid26 0:18c127542958 25 /*void LCD(TextLCD lcd){
sid26 0:18c127542958 26 const char equipe[10][16] = {" ULISSES"," FURA-OLHO2.0", "Afonso", "Angelica", "Diego", "Jonatas",
sid26 0:18c127542958 27 "Natalia", "Pedro", "Wanderley", "Welton"};
sid26 0:18c127542958 28
sid26 0:18c127542958 29 for(int i = 0; i < 10; i += 2){
sid26 0:18c127542958 30 lcd.cls();
sid26 0:18c127542958 31 lcd.locate(0,0);
sid26 0:18c127542958 32 lcd.printf(equipe[i]);
sid26 0:18c127542958 33 lcd.locate(0,1);
sid26 0:18c127542958 34 lcd.printf(equipe[i+1]);
sid26 0:18c127542958 35 wait(1.5);
sid26 0:18c127542958 36 }
sid26 0:18c127542958 37 }*/
sid26 0:18c127542958 38
sid26 0:18c127542958 39 bool upsideDown() {
sid26 0:18c127542958 40 //Checa se o robô está de cabeça para baixo, indicando através de um led.
sid26 0:18c127542958 41
sid26 0:18c127542958 42 z1 = acc.getAccZ();
sid26 0:18c127542958 43 wait(5e-3); //debounce
sid26 0:18c127542958 44 z2 = acc.getAccZ();
sid26 0:18c127542958 45
sid26 0:18c127542958 46 if(z1 < -0.3 && z2 < -0.3) {
sid26 0:18c127542958 47 led_direita = 1;
sid26 0:18c127542958 48 led_esquerda = 1;
sid26 0:18c127542958 49 led_frente = 1;
sid26 0:18c127542958 50 led_tras = 1;
sid26 0:18c127542958 51 return true;
sid26 0:18c127542958 52 }
sid26 0:18c127542958 53 else if(z1 > 0.3 && z2 > 0.3) {
sid26 0:18c127542958 54 led_direita = 1;
sid26 0:18c127542958 55 led_esquerda = 1;
sid26 0:18c127542958 56 led_frente = 1;
sid26 0:18c127542958 57 led_tras = 1;
sid26 0:18c127542958 58 return false;
sid26 0:18c127542958 59 }
sid26 0:18c127542958 60 }
sid26 0:18c127542958 61
sid26 0:18c127542958 62
sid26 0:18c127542958 63 void xyTranslation() {
sid26 0:18c127542958 64 //Checa se o robô está indo para frente ou para trás, indicando o sentido através de leds.
sid26 0:18c127542958 65
sid26 0:18c127542958 66 x1 = acc.getAccX();
sid26 0:18c127542958 67 wait(5e-3); //debounce
sid26 0:18c127542958 68 x2 = acc.getAccX();
sid26 0:18c127542958 69
sid26 0:18c127542958 70 if(x1 < -0.07 && x2 < -0.07) {
sid26 0:18c127542958 71 led_frente = 0;
sid26 0:18c127542958 72 led_tras = 1;
sid26 0:18c127542958 73 }
sid26 0:18c127542958 74 else if(x1 > 0.07 && x2 > 0.07) {
sid26 0:18c127542958 75 led_frente = 1;
sid26 0:18c127542958 76 led_tras = 0;
sid26 0:18c127542958 77 }
sid26 0:18c127542958 78 else {
sid26 0:18c127542958 79 led_frente = 0;
sid26 0:18c127542958 80 led_tras = 0;
sid26 0:18c127542958 81 }
sid26 0:18c127542958 82 }
sid26 0:18c127542958 83
sid26 0:18c127542958 84
sid26 0:18c127542958 85 void xyRotation() {
sid26 0:18c127542958 86 //Checa se o robô está fazendo curva, indicando o sentido através de leds.
sid26 0:18c127542958 87
sid26 0:18c127542958 88 y = acc.getAccY();
sid26 0:18c127542958 89 wait(5e-3); //debounce
sid26 0:18c127542958 90 y2 = acc.getAccY();
sid26 0:18c127542958 91
sid26 0:18c127542958 92 pc.printf("Y1: %.4f;Y2: %.4f;\t\t\t", y, y2);
sid26 0:18c127542958 93
sid26 0:18c127542958 94 if(y < -0.07 && y2 < -0.07) {
sid26 0:18c127542958 95 led_direita = 1;
sid26 0:18c127542958 96 led_esquerda = 0;
sid26 0:18c127542958 97 }
sid26 0:18c127542958 98 else if(y > 0.07 && y2 > 0.07) {
sid26 0:18c127542958 99 led_direita = 0;
sid26 0:18c127542958 100 led_esquerda = 1;
sid26 0:18c127542958 101 }
sid26 0:18c127542958 102 else {
sid26 0:18c127542958 103 led_direita = 0;
sid26 0:18c127542958 104 led_esquerda = 0;
sid26 0:18c127542958 105 }
sid26 0:18c127542958 106 }
sid26 0:18c127542958 107
sid26 0:18c127542958 108
sid26 0:18c127542958 109 void controleMovimentos() {
sid26 0:18c127542958 110 if(upsideDown());
sid26 0:18c127542958 111 else{
sid26 0:18c127542958 112 xyTranslation();
sid26 0:18c127542958 113 xyRotation();
sid26 0:18c127542958 114 }
sid26 0:18c127542958 115 }
sid26 0:18c127542958 116
sid26 0:18c127542958 117
sid26 0:18c127542958 118 void main() {
sid26 0:18c127542958 119 // LCD(display);
sid26 0:18c127542958 120
sid26 0:18c127542958 121 while (1) {
sid26 0:18c127542958 122 c = pc.getc();
sid26 0:18c127542958 123 //display.cls();
sid26 0:18c127542958 124 //display.locate(0,0);
sid26 0:18c127542958 125 //display.printf("COMANDO: %c", c);
sid26 0:18c127542958 126
sid26 0:18c127542958 127 //Checando movimento com o acelerômetro:
sid26 0:18c127542958 128 controleMovimentos();
sid26 0:18c127542958 129
sid26 0:18c127542958 130 //Controle de velocidade:
sid26 0:18c127542958 131 if (c >= '0' && c <= '9' || c == 'q') {
sid26 0:18c127542958 132 if (c == 'q')
sid26 0:18c127542958 133 c = '9' + 1;
sid26 0:18c127542958 134 v = (c - '0') / 10.0;
sid26 0:18c127542958 135 }
sid26 0:18c127542958 136
sid26 0:18c127542958 137 //Controle de direção:
sid26 0:18c127542958 138 switch (c) {
sid26 0:18c127542958 139 case 'F':
sid26 0:18c127542958 140 RF = v;
sid26 0:18c127542958 141 LF = v;
sid26 0:18c127542958 142 RB = 0;
sid26 0:18c127542958 143 LB = 0;
sid26 0:18c127542958 144 break;
sid26 0:18c127542958 145 case 'B':
sid26 0:18c127542958 146 RF = 0;
sid26 0:18c127542958 147 LF = 0;
sid26 0:18c127542958 148 RB = v;
sid26 0:18c127542958 149 LB = v;
sid26 0:18c127542958 150 break;
sid26 0:18c127542958 151 case 'R':
sid26 0:18c127542958 152 RF = 0;
sid26 0:18c127542958 153 LF = v;
sid26 0:18c127542958 154 RB = v;
sid26 0:18c127542958 155 LB = 0;
sid26 0:18c127542958 156 break;
sid26 0:18c127542958 157 case 'L':
sid26 0:18c127542958 158 RF = v;
sid26 0:18c127542958 159 LF = 0;
sid26 0:18c127542958 160 RB = 0;
sid26 0:18c127542958 161 LB = v;
sid26 0:18c127542958 162 break;
sid26 0:18c127542958 163 case 'G':
sid26 0:18c127542958 164 RF = v;
sid26 0:18c127542958 165 LF = v;
sid26 0:18c127542958 166 RB = 0;
sid26 0:18c127542958 167 LB = 0;
sid26 0:18c127542958 168 break;
sid26 0:18c127542958 169 case 'I':
sid26 0:18c127542958 170 RF = v;
sid26 0:18c127542958 171 LF = v;
sid26 0:18c127542958 172 RB = 0;
sid26 0:18c127542958 173 LB = 0;
sid26 0:18c127542958 174 break;
sid26 0:18c127542958 175 case 'H':
sid26 0:18c127542958 176 RF = 0;
sid26 0:18c127542958 177 LF = 0;
sid26 0:18c127542958 178 RB = v;
sid26 0:18c127542958 179 LB = v;
sid26 0:18c127542958 180 break;
sid26 0:18c127542958 181 case 'J':
sid26 0:18c127542958 182 RF = 0;
sid26 0:18c127542958 183 LF = 0;
sid26 0:18c127542958 184 RB = v;
sid26 0:18c127542958 185 LB = v;
sid26 0:18c127542958 186 break;
sid26 0:18c127542958 187 case 'S':
sid26 0:18c127542958 188 RF = 0;
sid26 0:18c127542958 189 LF = 0;
sid26 0:18c127542958 190 RB = 0;
sid26 0:18c127542958 191 LB = 0;
sid26 0:18c127542958 192 break;
sid26 0:18c127542958 193 }
sid26 0:18c127542958 194 }
sid26 0:18c127542958 195 }