Jose Gutierrez Cabello / Hc05

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@5:92151b9067e4, 2016-01-19 (annotated)

Committer:
jcabello7
Date:
Tue Jan 19 14:43:51 2016 +0000
Revision:
5:92151b9067e4
Parent:
4:de2112ca0735 
- No deixa avan?ar al robot si hi ha un obstacle en la direcci? d'avan?ament.; - Canviat el port s?rie per un que si que funciona.

Who changed what in which revision?

UserRevisionLine numberNew 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 5:92151b9067e4 5 #define pi 3.14159265 //Valor de pi
jcabello7 5:92151b9067e4 6 #define longString 128 //Longitud màxima dels strings
jcabello7 5:92151b9067e4 7 #define minUltrasons 200 //Valor mínim dels ultrasons
jcabello7 2:298c9cc5af48 8
jcabello7 0:acddf8d6d0cb 9
jcabello7 0:acddf8d6d0cb 10 Hc05::Hc05(PinName tx, PinName rx) : RawSerial(tx, rx){
jcabello7 0:acddf8d6d0cb 11 baud(230400);
jcabello7 0:acddf8d6d0cb 12 mode = vel = ang = 0;
jcabello7 0:acddf8d6d0cb 13 iniString();
jcabello7 0:acddf8d6d0cb 14 timer.start();
jcabello7 2:298c9cc5af48 15 x = y = m1 = m2 = m3 = 0;
jcabello7 2:298c9cc5af48 16 tractaString();
jcabello7 0:acddf8d6d0cb 17 };
jcabello7 0:acddf8d6d0cb 18
jcabello7 0:acddf8d6d0cb 19 Hc05::~Hc05(){
jcabello7 0:acddf8d6d0cb 20 };
jcabello7 0:acddf8d6d0cb 21
jcabello7 0:acddf8d6d0cb 22 void Hc05::iniString(){
jcabello7 0:acddf8d6d0cb 23 strLlegit[0] = 'S';
jcabello7 0:acddf8d6d0cb 24 strLlegit[1] = 't';
jcabello7 0:acddf8d6d0cb 25 strLlegit[2] = 'r';
jcabello7 0:acddf8d6d0cb 26 strLlegit[3] = 'i';
jcabello7 0:acddf8d6d0cb 27 strLlegit[4] = 'n';
jcabello7 0:acddf8d6d0cb 28 strLlegit[5] = 'g';
jcabello7 0:acddf8d6d0cb 29 strLlegit[6] = '\0';
jcabello7 2:298c9cc5af48 30 strM1[0] = strM2[0] = strM3[0] = 'V';
jcabello7 2:298c9cc5af48 31 strM1[1] = strM2[1] = strM3[1] = '0';
jcabello7 2:298c9cc5af48 32 strM1[2] = strM2[2] = strM3[2] = '0';
jcabello7 2:298c9cc5af48 33 strM1[3] = strM2[3] = strM3[3] = '0';
jcabello7 2:298c9cc5af48 34 strM1[4] = strM2[4] = strM3[4] = 'F';
jcabello7 2:298c9cc5af48 35 strM1[5] = strM2[5] = strM3[5] = 'M';
jcabello7 2:298c9cc5af48 36 strM1[6] = '1';
jcabello7 2:298c9cc5af48 37 strM2[6] = '2';
jcabello7 2:298c9cc5af48 38 strM3[6] = '3';
jcabello7 2:298c9cc5af48 39 strM1[7] = strM2[7] = strM3[7] = strPos[0] = '\0';
jcabello7 0:acddf8d6d0cb 40 };
jcabello7 0:acddf8d6d0cb 41
jcabello7 0:acddf8d6d0cb 42 void Hc05::enviaString(char* str){
jcabello7 0:acddf8d6d0cb 43 int i = 0;
jcabello7 0:acddf8d6d0cb 44 while((i < longString) && (str[i] != '\0')) {
jcabello7 0:acddf8d6d0cb 45 putc(str[i]);
jcabello7 0:acddf8d6d0cb 46 i++;
jcabello7 0:acddf8d6d0cb 47 }
jcabello7 0:acddf8d6d0cb 48 putc(13);
jcabello7 0:acddf8d6d0cb 49 };
jcabello7 0:acddf8d6d0cb 50
jcabello7 0:acddf8d6d0cb 51
jcabello7 0:acddf8d6d0cb 52 bool Hc05::llegirString(){
jcabello7 0:acddf8d6d0cb 53 if(readable()){
jcabello7 0:acddf8d6d0cb 54 char c = getc();
jcabello7 0:acddf8d6d0cb 55 int i = 0;
jcabello7 0:acddf8d6d0cb 56 while((i < longString) && (c != 13)) {
jcabello7 0:acddf8d6d0cb 57 if(c!='@'){
jcabello7 0:acddf8d6d0cb 58 strLlegit[i] = c;
jcabello7 0:acddf8d6d0cb 59 i++;
jcabello7 0:acddf8d6d0cb 60 }
jcabello7 0:acddf8d6d0cb 61 timer.reset();
jcabello7 0:acddf8d6d0cb 62 c = getc();
jcabello7 0:acddf8d6d0cb 63 }
jcabello7 0:acddf8d6d0cb 64 strLlegit[i] = '\0';
jcabello7 0:acddf8d6d0cb 65 return true;
jcabello7 0:acddf8d6d0cb 66
jcabello7 0:acddf8d6d0cb 67 }
jcabello7 0:acddf8d6d0cb 68 return false;
jcabello7 0:acddf8d6d0cb 69 };
jcabello7 0:acddf8d6d0cb 70
jcabello7 0:acddf8d6d0cb 71 void Hc05::tractaString(){
jcabello7 0:acddf8d6d0cb 72 if ((strLlegit[0] == 'a') && (strLlegit[1] == 'v') && (strLlegit[2] == 'a') && (strLlegit[3] == 'n'))
jcabello7 0:acddf8d6d0cb 73 mode = 1;
jcabello7 3:c594d33c7523 74 else if ((strLlegit[0] == 'r') && (strLlegit[1] == 'o') && (strLlegit[2] == 't') && (strLlegit[3] == 'a'))
jcabello7 0:acddf8d6d0cb 75 mode = 2;
jcabello7 3:c594d33c7523 76 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 77 mode = 3;
jcabello7 3:c594d33c7523 78 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 79 mode = 4;
jcabello7 3:c594d33c7523 80 else if ((strLlegit[0] == 's') && (strLlegit[1] == 't') && (strLlegit[2] == 'o') && (strLlegit[3] == 'p')){
jcabello7 0:acddf8d6d0cb 81 mode = 0;
jcabello7 0:acddf8d6d0cb 82 vel = 0;
jcabello7 0:acddf8d6d0cb 83 ang = 0;
jcabello7 0:acddf8d6d0cb 84 }
jcabello7 3:c594d33c7523 85 else ;
jcabello7 0:acddf8d6d0cb 86 if ((mode == 1) || (mode==2)){
jcabello7 0:acddf8d6d0cb 87 char svel[5], sang[5];
jcabello7 0:acddf8d6d0cb 88 for(int i = 0; i<=4;i++){
jcabello7 0:acddf8d6d0cb 89 sang[i] = strLlegit[i+5];
jcabello7 0:acddf8d6d0cb 90 svel[i] = strLlegit[i+10];
jcabello7 0:acddf8d6d0cb 91 }
jcabello7 0:acddf8d6d0cb 92 sang[4] = svel[4] = '\0';
jcabello7 0:acddf8d6d0cb 93 vel = atoi(svel);
jcabello7 0:acddf8d6d0cb 94 ang = atoi(sang);
jcabello7 0:acddf8d6d0cb 95 }
jcabello7 3:c594d33c7523 96 //Calcul dels motors
jcabello7 3:c594d33c7523 97 //calculaMotors();
jcabello7 2:298c9cc5af48 98
jcabello7 0:acddf8d6d0cb 99 };
jcabello7 0:acddf8d6d0cb 100
jcabello7 0:acddf8d6d0cb 101 int Hc05::getMode(){return mode;};
jcabello7 0:acddf8d6d0cb 102 int Hc05::getVel(){return vel;};
jcabello7 0:acddf8d6d0cb 103 int Hc05::getAng(){return ang;};
jcabello7 0:acddf8d6d0cb 104 void Hc05::getStringLlegit(char* str){
jcabello7 0:acddf8d6d0cb 105 int i = 0;
jcabello7 0:acddf8d6d0cb 106 while((i < longString) && (strLlegit[i] != '\0')) {
jcabello7 0:acddf8d6d0cb 107 str[i] = strLlegit[i];
jcabello7 0:acddf8d6d0cb 108 i++;
jcabello7 0:acddf8d6d0cb 109 }
jcabello7 0:acddf8d6d0cb 110 str[i] = '\0';
jcabello7 0:acddf8d6d0cb 111 };
jcabello7 0:acddf8d6d0cb 112
jcabello7 0:acddf8d6d0cb 113 void Hc05::comprovaConnexio(){
jcabello7 0:acddf8d6d0cb 114 if(getTimer() > 300)
jcabello7 0:acddf8d6d0cb 115 mode = -1;
jcabello7 0:acddf8d6d0cb 116 };
jcabello7 0:acddf8d6d0cb 117
jcabello7 0:acddf8d6d0cb 118 int Hc05::getTimer(){
jcabello7 0:acddf8d6d0cb 119 return timer.read_ms();
jcabello7 0:acddf8d6d0cb 120 };
jcabello7 0:acddf8d6d0cb 121
jcabello7 1:cd73654d13ed 122 //Calculs motors
jcabello7 5:92151b9067e4 123 void Hc05::calculaMotors(float S1, float S2, float S3){
jcabello7 5:92151b9067e4 124 if(mode==1) { // Comprova que no hi hagin obstacles en la direcció en que volem avançar
jcabello7 5:92151b9067e4 125 if ( (!((ang>=-45) && (ang<=45) && (S1<=minUltrasons))) && (!((ang>=90) && (ang<=180) && (S2<=minUltrasons))) && (!((ang>=-90) && (ang<=-180) && (S3<=minUltrasons))) ) {
jcabello7 5:92151b9067e4 126 x = cos(ang*pi/180);
jcabello7 5:92151b9067e4 127 y = sin(ang*pi/180);
jcabello7 5:92151b9067e4 128 m1 = (-y)*vel;
jcabello7 5:92151b9067e4 129 m3 = (x*0.8660254038+y*0.5)*vel;
jcabello7 5:92151b9067e4 130 m2 = (x*(-0.8660254038)+y*0.5)*vel;
jcabello7 5:92151b9067e4 131 }
jcabello7 5:92151b9067e4 132 else {m1 = m2 = m3 = x = y = 0;}
jcabello7 2:298c9cc5af48 133 }
jcabello7 2:298c9cc5af48 134 else if ((mode==2) && (ang!=0)){
jcabello7 2:298c9cc5af48 135 x = y = 0;
jcabello7 2:298c9cc5af48 136 if(ang>0)
jcabello7 2:298c9cc5af48 137 m1 = m2 = m3 = vel;
jcabello7 2:298c9cc5af48 138 else
jcabello7 2:298c9cc5af48 139 m1 = m2 = m3 = -(vel);
jcabello7 2:298c9cc5af48 140 }
jcabello7 2:298c9cc5af48 141 else {
jcabello7 2:298c9cc5af48 142 m1 = m2 = m3 = x = y = 0;
jcabello7 2:298c9cc5af48 143 }
jcabello7 3:c594d33c7523 144 //Omple els strings per enviar a l'arduino
jcabello7 3:c594d33c7523 145 char cDireccio[3];
jcabello7 3:c594d33c7523 146 if (m1<0)
jcabello7 3:c594d33c7523 147 cDireccio[0]='B';
jcabello7 3:c594d33c7523 148 else cDireccio[0]='F';
jcabello7 3:c594d33c7523 149 if (m2<0)
jcabello7 3:c594d33c7523 150 cDireccio[1]='B';
jcabello7 3:c594d33c7523 151 else cDireccio[1]='F';
jcabello7 3:c594d33c7523 152 if (m3<0)
jcabello7 3:c594d33c7523 153 cDireccio[2]='B';
jcabello7 3:c594d33c7523 154 else cDireccio[2]='F';
jcabello7 3:c594d33c7523 155
jcabello7 3:c594d33c7523 156 snprintf(strM1, longString, "V%.3i%cM1", abs(m1), cDireccio[0]);
jcabello7 3:c594d33c7523 157 snprintf(strM2, longString, "V%.3i%cM2", abs(m2), cDireccio[1]);
jcabello7 3:c594d33c7523 158 snprintf(strM3, longString, "V%.3i%cM3", abs(m3), cDireccio[2]);
jcabello7 3:c594d33c7523 159 snprintf(strPos, longString, "X=%g Y=%g", x, y);
jcabello7 1:cd73654d13ed 160 };
jcabello7 1:cd73654d13ed 161
jcabello7 2:298c9cc5af48 162 void Hc05::getStringM1(char* str){
jcabello7 2:298c9cc5af48 163 int i = 0;
jcabello7 2:298c9cc5af48 164 while((i < longString) && (strM1[i] != '\0')) {
jcabello7 2:298c9cc5af48 165 str[i] = strM1[i];
jcabello7 2:298c9cc5af48 166 i++;
jcabello7 2:298c9cc5af48 167 }
jcabello7 2:298c9cc5af48 168 str[i] = '\0';
jcabello7 2:298c9cc5af48 169 };
jcabello7 2:298c9cc5af48 170
jcabello7 2:298c9cc5af48 171 void Hc05::getStringM2(char* str){
jcabello7 2:298c9cc5af48 172 int i = 0;
jcabello7 2:298c9cc5af48 173 while((i < longString) && (strM2[i] != '\0')) {
jcabello7 2:298c9cc5af48 174 str[i] = strM2[i];
jcabello7 2:298c9cc5af48 175 i++;
jcabello7 2:298c9cc5af48 176 }
jcabello7 2:298c9cc5af48 177 str[i] = '\0';
jcabello7 2:298c9cc5af48 178 };
jcabello7 2:298c9cc5af48 179
jcabello7 2:298c9cc5af48 180 void Hc05::getStringM3(char* str){
jcabello7 2:298c9cc5af48 181 int i = 0;
jcabello7 2:298c9cc5af48 182 while((i < longString) && (strM3[i] != '\0')) {
jcabello7 2:298c9cc5af48 183 str[i] = strM3[i];
jcabello7 2:298c9cc5af48 184 i++;
jcabello7 2:298c9cc5af48 185 }
jcabello7 2:298c9cc5af48 186 str[i] = '\0';
jcabello7 2:298c9cc5af48 187 };
jcabello7 2:298c9cc5af48 188
jcabello7 2:298c9cc5af48 189 float Hc05::getX(){
jcabello7 1:cd73654d13ed 190 return x;
jcabello7 1:cd73654d13ed 191 };
jcabello7 1:cd73654d13ed 192
jcabello7 2:298c9cc5af48 193 float Hc05::getY(){
jcabello7 1:cd73654d13ed 194 return y;
jcabello7 1:cd73654d13ed 195 };
jcabello7 1:cd73654d13ed 196
jcabello7 2:298c9cc5af48 197 float Hc05::getM1(){
jcabello7 1:cd73654d13ed 198 return m1;
jcabello7 1:cd73654d13ed 199 };
jcabello7 1:cd73654d13ed 200
jcabello7 2:298c9cc5af48 201 float Hc05::getM2(){
jcabello7 1:cd73654d13ed 202 return m2;
jcabello7 1:cd73654d13ed 203 };
jcabello7 1:cd73654d13ed 204
jcabello7 2:298c9cc5af48 205 float Hc05::getM3(){
jcabello7 1:cd73654d13ed 206 return m3;
jcabello7 1:cd73654d13ed 207 };