Llibreria per fer servir un HC-06 o HC-05 en mode esclau. És compatible amb les llibreries RTOS de mbed.
Dependents: projecte_v4_rtos ProjecteRobotFinal
Hc05.cpp@4:de2112ca0735, 2015-12-23 (annotated)
- Committer:
- jcabello7
- Date:
- Wed Dec 23 15:23:01 2015 +0000
- Revision:
- 4:de2112ca0735
- Parent:
- 3:c594d33c7523
- Child:
- 5:92151b9067e4
Afegida suggerencia ultrasons
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
jcabello7 | 0:acddf8d6d0cb | 1 | #include "Hc05.h" |
jcabello7 | 0:acddf8d6d0cb | 2 | #include "mbed.h" |
jcabello7 | 1:cd73654d13ed | 3 | #include "math.h" |
jcabello7 | 0:acddf8d6d0cb | 4 | |
jcabello7 | 1:cd73654d13ed | 5 | #define pi 3.14159265 |
jcabello7 | 2:298c9cc5af48 | 6 | #define longString 128 |
jcabello7 | 2:298c9cc5af48 | 7 | |
jcabello7 | 0:acddf8d6d0cb | 8 | |
jcabello7 | 0:acddf8d6d0cb | 9 | Hc05::Hc05(PinName tx, PinName rx) : RawSerial(tx, rx){ |
jcabello7 | 0:acddf8d6d0cb | 10 | baud(230400); |
jcabello7 | 0:acddf8d6d0cb | 11 | mode = vel = ang = 0; |
jcabello7 | 0:acddf8d6d0cb | 12 | iniString(); |
jcabello7 | 0:acddf8d6d0cb | 13 | timer.start(); |
jcabello7 | 2:298c9cc5af48 | 14 | x = y = m1 = m2 = m3 = 0; |
jcabello7 | 2:298c9cc5af48 | 15 | tractaString(); |
jcabello7 | 0:acddf8d6d0cb | 16 | }; |
jcabello7 | 0:acddf8d6d0cb | 17 | |
jcabello7 | 0:acddf8d6d0cb | 18 | Hc05::~Hc05(){ |
jcabello7 | 0:acddf8d6d0cb | 19 | }; |
jcabello7 | 0:acddf8d6d0cb | 20 | |
jcabello7 | 0:acddf8d6d0cb | 21 | void Hc05::iniString(){ |
jcabello7 | 0:acddf8d6d0cb | 22 | strLlegit[0] = 'S'; |
jcabello7 | 0:acddf8d6d0cb | 23 | strLlegit[1] = 't'; |
jcabello7 | 0:acddf8d6d0cb | 24 | strLlegit[2] = 'r'; |
jcabello7 | 0:acddf8d6d0cb | 25 | strLlegit[3] = 'i'; |
jcabello7 | 0:acddf8d6d0cb | 26 | strLlegit[4] = 'n'; |
jcabello7 | 0:acddf8d6d0cb | 27 | strLlegit[5] = 'g'; |
jcabello7 | 0:acddf8d6d0cb | 28 | strLlegit[6] = '\0'; |
jcabello7 | 2:298c9cc5af48 | 29 | strM1[0] = strM2[0] = strM3[0] = 'V'; |
jcabello7 | 2:298c9cc5af48 | 30 | strM1[1] = strM2[1] = strM3[1] = '0'; |
jcabello7 | 2:298c9cc5af48 | 31 | strM1[2] = strM2[2] = strM3[2] = '0'; |
jcabello7 | 2:298c9cc5af48 | 32 | strM1[3] = strM2[3] = strM3[3] = '0'; |
jcabello7 | 2:298c9cc5af48 | 33 | strM1[4] = strM2[4] = strM3[4] = 'F'; |
jcabello7 | 2:298c9cc5af48 | 34 | strM1[5] = strM2[5] = strM3[5] = 'M'; |
jcabello7 | 2:298c9cc5af48 | 35 | strM1[6] = '1'; |
jcabello7 | 2:298c9cc5af48 | 36 | strM2[6] = '2'; |
jcabello7 | 2:298c9cc5af48 | 37 | strM3[6] = '3'; |
jcabello7 | 2:298c9cc5af48 | 38 | strM1[7] = strM2[7] = strM3[7] = strPos[0] = '\0'; |
jcabello7 | 0:acddf8d6d0cb | 39 | }; |
jcabello7 | 0:acddf8d6d0cb | 40 | |
jcabello7 | 0:acddf8d6d0cb | 41 | void Hc05::enviaString(char* str){ |
jcabello7 | 0:acddf8d6d0cb | 42 | int i = 0; |
jcabello7 | 0:acddf8d6d0cb | 43 | while((i < longString) && (str[i] != '\0')) { |
jcabello7 | 0:acddf8d6d0cb | 44 | putc(str[i]); |
jcabello7 | 0:acddf8d6d0cb | 45 | i++; |
jcabello7 | 0:acddf8d6d0cb | 46 | } |
jcabello7 | 0:acddf8d6d0cb | 47 | putc(13); |
jcabello7 | 0:acddf8d6d0cb | 48 | }; |
jcabello7 | 0:acddf8d6d0cb | 49 | |
jcabello7 | 0:acddf8d6d0cb | 50 | |
jcabello7 | 0:acddf8d6d0cb | 51 | bool Hc05::llegirString(){ |
jcabello7 | 0:acddf8d6d0cb | 52 | if(readable()){ |
jcabello7 | 0:acddf8d6d0cb | 53 | char c = getc(); |
jcabello7 | 0:acddf8d6d0cb | 54 | int i = 0; |
jcabello7 | 0:acddf8d6d0cb | 55 | while((i < longString) && (c != 13)) { |
jcabello7 | 0:acddf8d6d0cb | 56 | if(c!='@'){ |
jcabello7 | 0:acddf8d6d0cb | 57 | strLlegit[i] = c; |
jcabello7 | 0:acddf8d6d0cb | 58 | i++; |
jcabello7 | 0:acddf8d6d0cb | 59 | } |
jcabello7 | 0:acddf8d6d0cb | 60 | timer.reset(); |
jcabello7 | 0:acddf8d6d0cb | 61 | c = getc(); |
jcabello7 | 0:acddf8d6d0cb | 62 | } |
jcabello7 | 0:acddf8d6d0cb | 63 | strLlegit[i] = '\0'; |
jcabello7 | 0:acddf8d6d0cb | 64 | return true; |
jcabello7 | 0:acddf8d6d0cb | 65 | |
jcabello7 | 0:acddf8d6d0cb | 66 | } |
jcabello7 | 0:acddf8d6d0cb | 67 | return false; |
jcabello7 | 0:acddf8d6d0cb | 68 | }; |
jcabello7 | 0:acddf8d6d0cb | 69 | |
jcabello7 | 0:acddf8d6d0cb | 70 | void Hc05::tractaString(){ |
jcabello7 | 0:acddf8d6d0cb | 71 | if ((strLlegit[0] == 'a') && (strLlegit[1] == 'v') && (strLlegit[2] == 'a') && (strLlegit[3] == 'n')) |
jcabello7 | 0:acddf8d6d0cb | 72 | mode = 1; |
jcabello7 | 3:c594d33c7523 | 73 | else if ((strLlegit[0] == 'r') && (strLlegit[1] == 'o') && (strLlegit[2] == 't') && (strLlegit[3] == 'a')) |
jcabello7 | 0:acddf8d6d0cb | 74 | mode = 2; |
jcabello7 | 3:c594d33c7523 | 75 | else if ((strLlegit[0] == 'm') && (strLlegit[1] == 'o') && (strLlegit[2] == 'd') && (strLlegit[3] == 'e') && (strLlegit[5] == 'a') && (strLlegit[6] == 'u') && (strLlegit[7] == 't') && (strLlegit[8] == 'o')) |
jcabello7 | 3:c594d33c7523 | 76 | mode = 3; |
jcabello7 | 3:c594d33c7523 | 77 | else if ((strLlegit[0] == 'm') && (strLlegit[1] == 'o') && (strLlegit[2] == 'd') && (strLlegit[3] == 'e') && (strLlegit[5] == 'm') && (strLlegit[6] == 'a') && (strLlegit[7] == 'n') && (strLlegit[8] == 'u')) |
jcabello7 | 3:c594d33c7523 | 78 | mode = 4; |
jcabello7 | 3:c594d33c7523 | 79 | else if ((strLlegit[0] == 's') && (strLlegit[1] == 't') && (strLlegit[2] == 'o') && (strLlegit[3] == 'p')){ |
jcabello7 | 0:acddf8d6d0cb | 80 | mode = 0; |
jcabello7 | 0:acddf8d6d0cb | 81 | vel = 0; |
jcabello7 | 0:acddf8d6d0cb | 82 | ang = 0; |
jcabello7 | 0:acddf8d6d0cb | 83 | } |
jcabello7 | 3:c594d33c7523 | 84 | else ; |
jcabello7 | 0:acddf8d6d0cb | 85 | if ((mode == 1) || (mode==2)){ |
jcabello7 | 0:acddf8d6d0cb | 86 | char svel[5], sang[5]; |
jcabello7 | 0:acddf8d6d0cb | 87 | for(int i = 0; i<=4;i++){ |
jcabello7 | 0:acddf8d6d0cb | 88 | sang[i] = strLlegit[i+5]; |
jcabello7 | 0:acddf8d6d0cb | 89 | svel[i] = strLlegit[i+10]; |
jcabello7 | 0:acddf8d6d0cb | 90 | } |
jcabello7 | 0:acddf8d6d0cb | 91 | sang[4] = svel[4] = '\0'; |
jcabello7 | 0:acddf8d6d0cb | 92 | vel = atoi(svel); |
jcabello7 | 0:acddf8d6d0cb | 93 | ang = atoi(sang); |
jcabello7 | 0:acddf8d6d0cb | 94 | } |
jcabello7 | 3:c594d33c7523 | 95 | //Calcul dels motors |
jcabello7 | 3:c594d33c7523 | 96 | //calculaMotors(); |
jcabello7 | 2:298c9cc5af48 | 97 | |
jcabello7 | 0:acddf8d6d0cb | 98 | }; |
jcabello7 | 0:acddf8d6d0cb | 99 | |
jcabello7 | 0:acddf8d6d0cb | 100 | int Hc05::getMode(){return mode;}; |
jcabello7 | 0:acddf8d6d0cb | 101 | int Hc05::getVel(){return vel;}; |
jcabello7 | 0:acddf8d6d0cb | 102 | int Hc05::getAng(){return ang;}; |
jcabello7 | 0:acddf8d6d0cb | 103 | void Hc05::getStringLlegit(char* str){ |
jcabello7 | 0:acddf8d6d0cb | 104 | int i = 0; |
jcabello7 | 0:acddf8d6d0cb | 105 | while((i < longString) && (strLlegit[i] != '\0')) { |
jcabello7 | 0:acddf8d6d0cb | 106 | str[i] = strLlegit[i]; |
jcabello7 | 0:acddf8d6d0cb | 107 | i++; |
jcabello7 | 0:acddf8d6d0cb | 108 | } |
jcabello7 | 0:acddf8d6d0cb | 109 | str[i] = '\0'; |
jcabello7 | 0:acddf8d6d0cb | 110 | }; |
jcabello7 | 0:acddf8d6d0cb | 111 | |
jcabello7 | 0:acddf8d6d0cb | 112 | void Hc05::comprovaConnexio(){ |
jcabello7 | 0:acddf8d6d0cb | 113 | if(getTimer() > 300) |
jcabello7 | 0:acddf8d6d0cb | 114 | mode = -1; |
jcabello7 | 0:acddf8d6d0cb | 115 | }; |
jcabello7 | 0:acddf8d6d0cb | 116 | |
jcabello7 | 0:acddf8d6d0cb | 117 | int Hc05::getTimer(){ |
jcabello7 | 0:acddf8d6d0cb | 118 | return timer.read_ms(); |
jcabello7 | 0:acddf8d6d0cb | 119 | }; |
jcabello7 | 0:acddf8d6d0cb | 120 | |
jcabello7 | 1:cd73654d13ed | 121 | //Calculs motors |
jcabello7 | 1:cd73654d13ed | 122 | void Hc05::calculaMotors(){ |
jcabello7 | 4:de2112ca0735 | 123 | if(mode==1){ //Falta afegir una condició perquè no deixi avançar en determinada direcció si detecta els ultrasons if((mode==1) && ((ultraso1 > 10) && (ang<-45) && (ang>45)) && ((ultraso2 > 10) && (ang<90) && (ang>180)) && ((ultraso3 > 10) && (ang<-180) && (ang>-90)))) |
jcabello7 | 2:298c9cc5af48 | 124 | x = cos(ang*pi/180); |
jcabello7 | 2:298c9cc5af48 | 125 | y = sin(ang*pi/180); |
jcabello7 | 2:298c9cc5af48 | 126 | m1 = (-y)*vel; |
jcabello7 | 2:298c9cc5af48 | 127 | m3 = (x*0.8660254038+y*0.5)*vel; |
jcabello7 | 2:298c9cc5af48 | 128 | m2 = (x*(-0.8660254038)+y*0.5)*vel; |
jcabello7 | 2:298c9cc5af48 | 129 | } |
jcabello7 | 2:298c9cc5af48 | 130 | else if ((mode==2) && (ang!=0)){ |
jcabello7 | 2:298c9cc5af48 | 131 | x = y = 0; |
jcabello7 | 2:298c9cc5af48 | 132 | if(ang>0) |
jcabello7 | 2:298c9cc5af48 | 133 | m1 = m2 = m3 = vel; |
jcabello7 | 2:298c9cc5af48 | 134 | else |
jcabello7 | 2:298c9cc5af48 | 135 | m1 = m2 = m3 = -(vel); |
jcabello7 | 2:298c9cc5af48 | 136 | } |
jcabello7 | 2:298c9cc5af48 | 137 | else { |
jcabello7 | 2:298c9cc5af48 | 138 | m1 = m2 = m3 = x = y = 0; |
jcabello7 | 2:298c9cc5af48 | 139 | } |
jcabello7 | 3:c594d33c7523 | 140 | //Omple els strings per enviar a l'arduino |
jcabello7 | 3:c594d33c7523 | 141 | char cDireccio[3]; |
jcabello7 | 3:c594d33c7523 | 142 | if (m1<0) |
jcabello7 | 3:c594d33c7523 | 143 | cDireccio[0]='B'; |
jcabello7 | 3:c594d33c7523 | 144 | else cDireccio[0]='F'; |
jcabello7 | 3:c594d33c7523 | 145 | if (m2<0) |
jcabello7 | 3:c594d33c7523 | 146 | cDireccio[1]='B'; |
jcabello7 | 3:c594d33c7523 | 147 | else cDireccio[1]='F'; |
jcabello7 | 3:c594d33c7523 | 148 | if (m3<0) |
jcabello7 | 3:c594d33c7523 | 149 | cDireccio[2]='B'; |
jcabello7 | 3:c594d33c7523 | 150 | else cDireccio[2]='F'; |
jcabello7 | 3:c594d33c7523 | 151 | |
jcabello7 | 3:c594d33c7523 | 152 | snprintf(strM1, longString, "V%.3i%cM1", abs(m1), cDireccio[0]); |
jcabello7 | 3:c594d33c7523 | 153 | snprintf(strM2, longString, "V%.3i%cM2", abs(m2), cDireccio[1]); |
jcabello7 | 3:c594d33c7523 | 154 | snprintf(strM3, longString, "V%.3i%cM3", abs(m3), cDireccio[2]); |
jcabello7 | 3:c594d33c7523 | 155 | snprintf(strPos, longString, "X=%g Y=%g", x, y); |
jcabello7 | 1:cd73654d13ed | 156 | }; |
jcabello7 | 1:cd73654d13ed | 157 | |
jcabello7 | 2:298c9cc5af48 | 158 | void Hc05::getStringM1(char* str){ |
jcabello7 | 2:298c9cc5af48 | 159 | int i = 0; |
jcabello7 | 2:298c9cc5af48 | 160 | while((i < longString) && (strM1[i] != '\0')) { |
jcabello7 | 2:298c9cc5af48 | 161 | str[i] = strM1[i]; |
jcabello7 | 2:298c9cc5af48 | 162 | i++; |
jcabello7 | 2:298c9cc5af48 | 163 | } |
jcabello7 | 2:298c9cc5af48 | 164 | str[i] = '\0'; |
jcabello7 | 2:298c9cc5af48 | 165 | }; |
jcabello7 | 2:298c9cc5af48 | 166 | |
jcabello7 | 2:298c9cc5af48 | 167 | void Hc05::getStringM2(char* str){ |
jcabello7 | 2:298c9cc5af48 | 168 | int i = 0; |
jcabello7 | 2:298c9cc5af48 | 169 | while((i < longString) && (strM2[i] != '\0')) { |
jcabello7 | 2:298c9cc5af48 | 170 | str[i] = strM2[i]; |
jcabello7 | 2:298c9cc5af48 | 171 | i++; |
jcabello7 | 2:298c9cc5af48 | 172 | } |
jcabello7 | 2:298c9cc5af48 | 173 | str[i] = '\0'; |
jcabello7 | 2:298c9cc5af48 | 174 | }; |
jcabello7 | 2:298c9cc5af48 | 175 | |
jcabello7 | 2:298c9cc5af48 | 176 | void Hc05::getStringM3(char* str){ |
jcabello7 | 2:298c9cc5af48 | 177 | int i = 0; |
jcabello7 | 2:298c9cc5af48 | 178 | while((i < longString) && (strM3[i] != '\0')) { |
jcabello7 | 2:298c9cc5af48 | 179 | str[i] = strM3[i]; |
jcabello7 | 2:298c9cc5af48 | 180 | i++; |
jcabello7 | 2:298c9cc5af48 | 181 | } |
jcabello7 | 2:298c9cc5af48 | 182 | str[i] = '\0'; |
jcabello7 | 2:298c9cc5af48 | 183 | }; |
jcabello7 | 2:298c9cc5af48 | 184 | |
jcabello7 | 2:298c9cc5af48 | 185 | float Hc05::getX(){ |
jcabello7 | 1:cd73654d13ed | 186 | return x; |
jcabello7 | 1:cd73654d13ed | 187 | }; |
jcabello7 | 1:cd73654d13ed | 188 | |
jcabello7 | 2:298c9cc5af48 | 189 | float Hc05::getY(){ |
jcabello7 | 1:cd73654d13ed | 190 | return y; |
jcabello7 | 1:cd73654d13ed | 191 | }; |
jcabello7 | 1:cd73654d13ed | 192 | |
jcabello7 | 2:298c9cc5af48 | 193 | float Hc05::getM1(){ |
jcabello7 | 1:cd73654d13ed | 194 | return m1; |
jcabello7 | 1:cd73654d13ed | 195 | }; |
jcabello7 | 1:cd73654d13ed | 196 | |
jcabello7 | 2:298c9cc5af48 | 197 | float Hc05::getM2(){ |
jcabello7 | 1:cd73654d13ed | 198 | return m2; |
jcabello7 | 1:cd73654d13ed | 199 | }; |
jcabello7 | 1:cd73654d13ed | 200 | |
jcabello7 | 2:298c9cc5af48 | 201 | float Hc05::getM3(){ |
jcabello7 | 1:cd73654d13ed | 202 | return m3; |
jcabello7 | 1:cd73654d13ed | 203 | }; |