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
- Committer:
- jcabello7
- Date:
- 2015-12-21
- Revision:
- 2:298c9cc5af48
- Parent:
- 1:cd73654d13ed
- Child:
- 3:c594d33c7523
File content as of revision 2:298c9cc5af48:
#include "Hc05.h" #include "mbed.h" #include "math.h" #define pi 3.14159265 #define longString 128 Hc05::Hc05(PinName tx, PinName rx) : RawSerial(tx, rx){ baud(230400); mode = vel = ang = 0; iniString(); timer.start(); x = y = m1 = m2 = m3 = 0; tractaString(); }; Hc05::~Hc05(){ }; void Hc05::iniString(){ strLlegit[0] = 'S'; strLlegit[1] = 't'; strLlegit[2] = 'r'; strLlegit[3] = 'i'; strLlegit[4] = 'n'; strLlegit[5] = 'g'; strLlegit[6] = '\0'; strM1[0] = strM2[0] = strM3[0] = 'V'; strM1[1] = strM2[1] = strM3[1] = '0'; strM1[2] = strM2[2] = strM3[2] = '0'; strM1[3] = strM2[3] = strM3[3] = '0'; strM1[4] = strM2[4] = strM3[4] = 'F'; strM1[5] = strM2[5] = strM3[5] = 'M'; strM1[6] = '1'; strM2[6] = '2'; strM3[6] = '3'; strM1[7] = strM2[7] = strM3[7] = strPos[0] = '\0'; }; void Hc05::enviaString(char* str){ int i = 0; while((i < longString) && (str[i] != '\0')) { putc(str[i]); i++; } putc(13); }; bool Hc05::llegirString(){ if(readable()){ char c = getc(); int i = 0; while((i < longString) && (c != 13)) { if(c!='@'){ strLlegit[i] = c; i++; } timer.reset(); c = getc(); } strLlegit[i] = '\0'; return true; } return false; }; void Hc05::tractaString(){ if ((strLlegit[0] == 'a') && (strLlegit[1] == 'v') && (strLlegit[2] == 'a') && (strLlegit[3] == 'n')) mode = 1; if ((strLlegit[0] == 'r') && (strLlegit[1] == 'o') && (strLlegit[2] == 't') && (strLlegit[3] == 'a')) mode = 2; if ((strLlegit[0] == 's') && (strLlegit[1] == 't') && (strLlegit[2] == 'o') && (strLlegit[3] == 'p')){ mode = 0; vel = 0; ang = 0; } if ((mode == 1) || (mode==2)){ char svel[5], sang[5]; for(int i = 0; i<=4;i++){ sang[i] = strLlegit[i+5]; svel[i] = strLlegit[i+10]; } sang[4] = svel[4] = '\0'; vel = atoi(svel); ang = atoi(sang); } 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')) mode = 3; calculaMotors(); char cDireccio[3]; if (m1<0) cDireccio[0]='B'; else cDireccio[0]='F'; if (m2<0) cDireccio[1]='B'; else cDireccio[1]='F'; if (m3<0) cDireccio[2]='B'; else cDireccio[2]='F'; snprintf(strM1, longString, "V%.3i%cM1", abs(m1), cDireccio[0]); snprintf(strM2, longString, "V%.3i%cM2", abs(m2), cDireccio[1]); snprintf(strM3, longString, "V%.3i%cM3", abs(m3), cDireccio[2]); snprintf(strPos, longString, "X=%g Y=%g", x, y); }; int Hc05::getMode(){return mode;}; int Hc05::getVel(){return vel;}; int Hc05::getAng(){return ang;}; void Hc05::getStringLlegit(char* str){ int i = 0; while((i < longString) && (strLlegit[i] != '\0')) { str[i] = strLlegit[i]; i++; } str[i] = '\0'; }; void Hc05::comprovaConnexio(){ if(getTimer() > 300) mode = -1; }; int Hc05::getTimer(){ return timer.read_ms(); }; //Calculs motors void Hc05::calculaMotors(){ if(mode==1){ x = cos(ang*pi/180); y = sin(ang*pi/180); m1 = (-y)*vel; m3 = (x*0.8660254038+y*0.5)*vel; m2 = (x*(-0.8660254038)+y*0.5)*vel; } else if ((mode==2) && (ang!=0)){ x = y = 0; if(ang>0) m1 = m2 = m3 = vel; else m1 = m2 = m3 = -(vel); } else { m1 = m2 = m3 = x = y = 0; } }; void Hc05::getStringM1(char* str){ int i = 0; while((i < longString) && (strM1[i] != '\0')) { str[i] = strM1[i]; i++; } str[i] = '\0'; }; void Hc05::getStringM2(char* str){ int i = 0; while((i < longString) && (strM2[i] != '\0')) { str[i] = strM2[i]; i++; } str[i] = '\0'; }; void Hc05::getStringM3(char* str){ int i = 0; while((i < longString) && (strM3[i] != '\0')) { str[i] = strM3[i]; i++; } str[i] = '\0'; }; float Hc05::getX(){ return x; }; float Hc05::getY(){ return y; }; float Hc05::getM1(){ return m1; }; float Hc05::getM2(){ return m2; }; float Hc05::getM3(){ return m3; };