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@1:cd73654d13ed, 2015-12-21 (annotated)

Committer:
jcabello7
Date:
Mon Dec 21 17:21:34 2015 +0000
Revision:
1:cd73654d13ed
Parent:
0:acddf8d6d0cb
Child:
2:298c9cc5af48
Integra els c?lculs dels motors

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 0:acddf8d6d0cb 6
jcabello7 0:acddf8d6d0cb 7 Hc05::Hc05(PinName tx, PinName rx) : RawSerial(tx, rx){
jcabello7 0:acddf8d6d0cb 8 baud(230400);
jcabello7 0:acddf8d6d0cb 9 mode = vel = ang = 0;
jcabello7 0:acddf8d6d0cb 10 iniString();
jcabello7 0:acddf8d6d0cb 11 timer.start();
jcabello7 1:cd73654d13ed 12 x = y = m1 = m2 = m3 = 0.0;
jcabello7 0:acddf8d6d0cb 13 };
jcabello7 0:acddf8d6d0cb 14
jcabello7 0:acddf8d6d0cb 15 Hc05::~Hc05(){
jcabello7 0:acddf8d6d0cb 16 };
jcabello7 0:acddf8d6d0cb 17
jcabello7 0:acddf8d6d0cb 18 void Hc05::iniString(){
jcabello7 0:acddf8d6d0cb 19 strLlegit[0] = 'S';
jcabello7 0:acddf8d6d0cb 20 strLlegit[1] = 't';
jcabello7 0:acddf8d6d0cb 21 strLlegit[2] = 'r';
jcabello7 0:acddf8d6d0cb 22 strLlegit[3] = 'i';
jcabello7 0:acddf8d6d0cb 23 strLlegit[4] = 'n';
jcabello7 0:acddf8d6d0cb 24 strLlegit[5] = 'g';
jcabello7 0:acddf8d6d0cb 25 strLlegit[6] = '\0';
jcabello7 0:acddf8d6d0cb 26 };
jcabello7 0:acddf8d6d0cb 27
jcabello7 0:acddf8d6d0cb 28 void Hc05::enviaString(char* str){
jcabello7 0:acddf8d6d0cb 29 int i = 0;
jcabello7 0:acddf8d6d0cb 30 while((i < longString) && (str[i] != '\0')) {
jcabello7 0:acddf8d6d0cb 31 putc(str[i]);
jcabello7 0:acddf8d6d0cb 32 i++;
jcabello7 0:acddf8d6d0cb 33 }
jcabello7 0:acddf8d6d0cb 34 putc(13);
jcabello7 0:acddf8d6d0cb 35 };
jcabello7 0:acddf8d6d0cb 36
jcabello7 0:acddf8d6d0cb 37
jcabello7 0:acddf8d6d0cb 38 bool Hc05::llegirString(){
jcabello7 0:acddf8d6d0cb 39 if(readable()){
jcabello7 0:acddf8d6d0cb 40 char c = getc();
jcabello7 0:acddf8d6d0cb 41 int i = 0;
jcabello7 0:acddf8d6d0cb 42 while((i < longString) && (c != 13)) {
jcabello7 0:acddf8d6d0cb 43 if(c!='@'){
jcabello7 0:acddf8d6d0cb 44 strLlegit[i] = c;
jcabello7 0:acddf8d6d0cb 45 i++;
jcabello7 0:acddf8d6d0cb 46 }
jcabello7 0:acddf8d6d0cb 47 timer.reset();
jcabello7 0:acddf8d6d0cb 48 c = getc();
jcabello7 0:acddf8d6d0cb 49 }
jcabello7 0:acddf8d6d0cb 50 strLlegit[i] = '\0';
jcabello7 0:acddf8d6d0cb 51 return true;
jcabello7 0:acddf8d6d0cb 52
jcabello7 0:acddf8d6d0cb 53 }
jcabello7 0:acddf8d6d0cb 54 return false;
jcabello7 0:acddf8d6d0cb 55 };
jcabello7 0:acddf8d6d0cb 56
jcabello7 0:acddf8d6d0cb 57 void Hc05::tractaString(){
jcabello7 0:acddf8d6d0cb 58 if ((strLlegit[0] == 'a') && (strLlegit[1] == 'v') && (strLlegit[2] == 'a') && (strLlegit[3] == 'n'))
jcabello7 0:acddf8d6d0cb 59 mode = 1;
jcabello7 0:acddf8d6d0cb 60 if ((strLlegit[0] == 'r') && (strLlegit[1] == 'o') && (strLlegit[2] == 't') && (strLlegit[3] == 'a'))
jcabello7 0:acddf8d6d0cb 61 mode = 2;
jcabello7 0:acddf8d6d0cb 62 if ((strLlegit[0] == 's') && (strLlegit[1] == 't') && (strLlegit[2] == 'o') && (strLlegit[3] == 'p')){
jcabello7 0:acddf8d6d0cb 63 mode = 0;
jcabello7 0:acddf8d6d0cb 64 vel = 0;
jcabello7 0:acddf8d6d0cb 65 ang = 0;
jcabello7 0:acddf8d6d0cb 66 }
jcabello7 0:acddf8d6d0cb 67 if ((mode == 1) || (mode==2)){
jcabello7 0:acddf8d6d0cb 68 char svel[5], sang[5];
jcabello7 0:acddf8d6d0cb 69 for(int i = 0; i<=4;i++){
jcabello7 0:acddf8d6d0cb 70 sang[i] = strLlegit[i+5];
jcabello7 0:acddf8d6d0cb 71 svel[i] = strLlegit[i+10];
jcabello7 0:acddf8d6d0cb 72 }
jcabello7 0:acddf8d6d0cb 73 sang[4] = svel[4] = '\0';
jcabello7 0:acddf8d6d0cb 74 vel = atoi(svel);
jcabello7 0:acddf8d6d0cb 75 ang = atoi(sang);
jcabello7 0:acddf8d6d0cb 76 }
jcabello7 0:acddf8d6d0cb 77 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 0:acddf8d6d0cb 78 mode = 3;
jcabello7 0:acddf8d6d0cb 79 };
jcabello7 0:acddf8d6d0cb 80
jcabello7 0:acddf8d6d0cb 81 int Hc05::getMode(){return mode;};
jcabello7 0:acddf8d6d0cb 82 int Hc05::getVel(){return vel;};
jcabello7 0:acddf8d6d0cb 83 int Hc05::getAng(){return ang;};
jcabello7 0:acddf8d6d0cb 84 void Hc05::getStringLlegit(char* str){
jcabello7 0:acddf8d6d0cb 85 int i = 0;
jcabello7 0:acddf8d6d0cb 86 while((i < longString) && (strLlegit[i] != '\0')) {
jcabello7 0:acddf8d6d0cb 87 str[i] = strLlegit[i];
jcabello7 0:acddf8d6d0cb 88 i++;
jcabello7 0:acddf8d6d0cb 89 }
jcabello7 0:acddf8d6d0cb 90 str[i] = '\0';
jcabello7 0:acddf8d6d0cb 91 };
jcabello7 0:acddf8d6d0cb 92
jcabello7 0:acddf8d6d0cb 93 void Hc05::comprovaConnexio(){
jcabello7 0:acddf8d6d0cb 94 if(getTimer() > 300)
jcabello7 0:acddf8d6d0cb 95 mode = -1;
jcabello7 0:acddf8d6d0cb 96 };
jcabello7 0:acddf8d6d0cb 97
jcabello7 0:acddf8d6d0cb 98 int Hc05::getTimer(){
jcabello7 0:acddf8d6d0cb 99 return timer.read_ms();
jcabello7 0:acddf8d6d0cb 100 };
jcabello7 0:acddf8d6d0cb 101
jcabello7 1:cd73654d13ed 102 //Calculs motors
jcabello7 1:cd73654d13ed 103 void Hc05::calculaMotors(){
jcabello7 1:cd73654d13ed 104 x = cos(ang*pi/180);
jcabello7 1:cd73654d13ed 105 y = sin(ang*pi/180);
jcabello7 1:cd73654d13ed 106 m1 = -y;
jcabello7 1:cd73654d13ed 107 m3 = x*0.8660254038+y*0.5;
jcabello7 1:cd73654d13ed 108 m2 = x*(-0.8660254038)+y*0.5;
jcabello7 1:cd73654d13ed 109 };
jcabello7 1:cd73654d13ed 110
jcabello7 1:cd73654d13ed 111 float getX(){
jcabello7 1:cd73654d13ed 112 return x;
jcabello7 1:cd73654d13ed 113 };
jcabello7 1:cd73654d13ed 114
jcabello7 1:cd73654d13ed 115 float getY(){
jcabello7 1:cd73654d13ed 116 return y;
jcabello7 1:cd73654d13ed 117 };
jcabello7 1:cd73654d13ed 118
jcabello7 1:cd73654d13ed 119 float getM1(){
jcabello7 1:cd73654d13ed 120 return m1;
jcabello7 1:cd73654d13ed 121 };
jcabello7 1:cd73654d13ed 122
jcabello7 1:cd73654d13ed 123 float getM2(){
jcabello7 1:cd73654d13ed 124 return m2;
jcabello7 1:cd73654d13ed 125 };
jcabello7 1:cd73654d13ed 126
jcabello7 1:cd73654d13ed 127 float getM3(){
jcabello7 1:cd73654d13ed 128 return m3;
jcabello7 1:cd73654d13ed 129 };