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@3:c594d33c7523, 2015-12-23 (annotated)

Committer:
jcabello7
Date:
Wed Dec 23 15:04:55 2015 +0000
Revision:
3:c594d33c7523
Parent:
2:298c9cc5af48
Child:
4:de2112ca0735
Millorada l'estructura de la llibreria (ara cedeix el control del c?lcul dels motors al programa principal)

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 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 3:c594d33c7523 123 if(mode==1){ //Falta afegir una condició perquè no deixi avançar en determinada direcció si detecta els ultrasons
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 };