Sooner Competitive Robotics / Mbed 2 deprecated IEEE_14_Freescale

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

Fork of IEEE_14_Freescale by IEEE 2014 Mbed

droidBT.cpp@49:7d172c133dbf, 2014-04-03 (annotated)

Committer:
sswatek
Date:
Thu Apr 03 13:17:04 2014 +0000
Revision:
49:7d172c133dbf
Parent:
24:46eeab7cebc6 
fixed bluetooth comms, servo setpoints, and developing a pickup procedure

Who changed what in which revision?

UserRevisionLine numberNew contents of line
soonerbot 16:6d900f687c18 1 #include "droidBT.h"
sswatek 24:46eeab7cebc6 2 #include "nodbgprint.h"
soonerbot 16:6d900f687c18 3
soonerbot 16:6d900f687c18 4 droidBT::droidBT(PinName tx, PinName rx) : droid(tx,rx){
soonerbot 16:6d900f687c18 5 BTStart=0;
soonerbot 16:6d900f687c18 6 BTEnd=0;
soonerbot 16:6d900f687c18 7 DCStart=0;
soonerbot 16:6d900f687c18 8 DCEnd=0;
soonerbot 16:6d900f687c18 9 droid.baud(115200);
soonerbot 16:6d900f687c18 10 droid.attach(this,&droidBT::BTEnqueue);
soonerbot 16:6d900f687c18 11 }
soonerbot 16:6d900f687c18 12
soonerbot 16:6d900f687c18 13 void droidBT::BTEnqueue(){
soonerbot 16:6d900f687c18 14 //while(droid.readable()){
soonerbot 16:6d900f687c18 15 //led1=1;
soonerbot 16:6d900f687c18 16 BTBuffer[BTEnd]=droid.getc();
soonerbot 16:6d900f687c18 17 //int a = LPC_UART1->RBR;
soonerbot 16:6d900f687c18 18 //led2=1;
sswatek 19:3d77dc964d4b 19 //commits
rmutalik 20:5b8fde077301 20 // fffffffffuuuuuuuu
soonerbot 16:6d900f687c18 21 //int b = LPC_UART2->RBR;
soonerbot 16:6d900f687c18 22 //led3=1;
sswatek 19:3d77dc964d4b 23 //with
soonerbot 16:6d900f687c18 24 //int c = LPC_UART3->RBR;
sswatek 19:3d77dc964d4b 25 //messing
rmutalik 20:5b8fde077301 26 // hello world
rmutalik 20:5b8fde077301 27 // hi
soonerbot 16:6d900f687c18 28 //led4=1;
soonerbot 16:6d900f687c18 29 //DBGPRINT("Got %x\n", BTBuffer[BTEnd]);
soonerbot 16:6d900f687c18 30 BTEnd++;
soonerbot 16:6d900f687c18 31 BTEnd%=BTBUFSIZE;
soonerbot 16:6d900f687c18 32 //led1=0;
soonerbot 16:6d900f687c18 33 //}
soonerbot 16:6d900f687c18 34 }
soonerbot 16:6d900f687c18 35
soonerbot 16:6d900f687c18 36 int droidBT::clearData(){
soonerbot 16:6d900f687c18 37 DCStart=DCEnd=0;
soonerbot 16:6d900f687c18 38 return 1;
soonerbot 16:6d900f687c18 39 }
soonerbot 16:6d900f687c18 40
soonerbot 16:6d900f687c18 41 int droidBT::getData(int cmd, int* dest){
soonerbot 16:6d900f687c18 42 procBuf(1);
soonerbot 16:6d900f687c18 43 int i;
soonerbot 16:6d900f687c18 44 for(i=DCStart;i!=DCEnd;i=(i+1)%DCHUNKSIZE){
soonerbot 16:6d900f687c18 45 if((DataChunk[i][0]&0xF0)==0x60 && DataChunk[i][1]==cmd){
soonerbot 16:6d900f687c18 46 DataChunk[i][0]=0;
soonerbot 16:6d900f687c18 47 if(i==DCStart){
soonerbot 16:6d900f687c18 48 DCStart++;
soonerbot 16:6d900f687c18 49 }
sswatek 24:46eeab7cebc6 50 for(int j=0;j<17;j++){
soonerbot 16:6d900f687c18 51 dest[j]=DataChunk[i][j];
sswatek 24:46eeab7cebc6 52 DataChunk[i][j]=0;
sswatek 24:46eeab7cebc6 53 }
soonerbot 16:6d900f687c18 54 return 1;
soonerbot 16:6d900f687c18 55 }
soonerbot 16:6d900f687c18 56 }
soonerbot 16:6d900f687c18 57 return 0;
soonerbot 16:6d900f687c18 58 }
soonerbot 16:6d900f687c18 59
soonerbot 16:6d900f687c18 60 int droidBT::procBuf(int cmd){
soonerbot 16:6d900f687c18 61 int i=BTStart;
soonerbot 16:6d900f687c18 62 int startChunk=i;
sswatek 24:46eeab7cebc6 63 //int seen=0;
sswatek 23:53cafcd67828 64 int cheapHash=0;
soonerbot 16:6d900f687c18 65 while (i!=BTEnd) {
soonerbot 16:6d900f687c18 66 if(i==startChunk){ //first bit in the chunk
soonerbot 16:6d900f687c18 67 if((BTBuffer[i]&0xF0)==0x60){//data returned
sswatek 23:53cafcd67828 68 cheapHash=BTBuffer[i];
soonerbot 16:6d900f687c18 69 DBGPRINT("gettin data\r\n",1);
soonerbot 16:6d900f687c18 70 i++;
sswatek 23:53cafcd67828 71 } else if((BTBuffer[i]&0xF0)==0xF0){//ack
sswatek 23:53cafcd67828 72 cheapHash=BTBuffer[i];
sswatek 23:53cafcd67828 73 DBGPRINT("gettin ack %x\r\n",BTBuffer[i]);
sswatek 23:53cafcd67828 74 BTBuffer[i]=0xF1;
sswatek 23:53cafcd67828 75 i++;
soonerbot 16:6d900f687c18 76 } else {//dunno what this data is, toss
sswatek 49:7d172c133dbf 77 //BTStart++;
sswatek 23:53cafcd67828 78 DBGPRINT("throwing away %x\r\n",BTBuffer[i]);
soonerbot 16:6d900f687c18 79 i++;
sswatek 49:7d172c133dbf 80 startChunk=i;
sswatek 49:7d172c133dbf 81 BTStart = i;
soonerbot 16:6d900f687c18 82 }
soonerbot 16:6d900f687c18 83 } else {
soonerbot 16:6d900f687c18 84 DBGPRINT("not first\r\n",1);
soonerbot 16:6d900f687c18 85 if(bufSize()>=(BTBuffer[startChunk]&0xF)+1){
soonerbot 16:6d900f687c18 86 DBGPRINT("got enough space %x, %d\r\n",BTBuffer[startChunk]&0xF, DCEnd);
soonerbot 16:6d900f687c18 87 DataChunk[DCEnd][0]=BTBuffer[startChunk];
soonerbot 16:6d900f687c18 88 int j;
soonerbot 16:6d900f687c18 89 for(j=0;j<(BTBuffer[startChunk]&0xF);j++){
soonerbot 16:6d900f687c18 90 DBGPRINT("copying %d, %d\r\n",BTBuffer[i+j], j);
soonerbot 16:6d900f687c18 91 DataChunk[DCEnd][j+1]=BTBuffer[i+j];
sswatek 23:53cafcd67828 92 if(j<((BTBuffer[startChunk]&0xF)-1)){
sswatek 23:53cafcd67828 93 cheapHash^=BTBuffer[i+j];
sswatek 23:53cafcd67828 94 }
soonerbot 16:6d900f687c18 95 }
sswatek 23:53cafcd67828 96 //check and store if the hash matched
sswatek 23:53cafcd67828 97 if(cheapHash == BTBuffer[i+(BTBuffer[startChunk]&0xF)-1]){
sswatek 23:53cafcd67828 98 DBGPRINT("Hash Matches\r\n",1);
sswatek 23:53cafcd67828 99 DataChunk[DCEnd][(BTBuffer[startChunk]&0xF)]=1;
sswatek 23:53cafcd67828 100 if(DCEnd>=DCHUNKSIZE)
sswatek 23:53cafcd67828 101 DCEnd=0;
sswatek 23:53cafcd67828 102 DCEnd++;
sswatek 23:53cafcd67828 103 }else{
sswatek 23:53cafcd67828 104 DBGPRINT("Hash Does Not Match\r\n",1);
sswatek 23:53cafcd67828 105 DataChunk[DCEnd][(BTBuffer[startChunk]&0xF)]=0;
sswatek 23:53cafcd67828 106 }
sswatek 24:46eeab7cebc6 107 DBGPRINT("Got Data of size %d [%x %d %d]\n", BTBuffer[startChunk]&0xF, DataChunk[DCEnd-1][1], DataChunk[DCEnd-1][2], DataChunk[DCEnd-1][3]);
sswatek 23:53cafcd67828 108
soonerbot 16:6d900f687c18 109 BTStart=i+j;
soonerbot 16:6d900f687c18 110 startChunk=i=BTStart;
soonerbot 16:6d900f687c18 111 } else {
soonerbot 16:6d900f687c18 112 break;
soonerbot 16:6d900f687c18 113 }
soonerbot 16:6d900f687c18 114 }
soonerbot 16:6d900f687c18 115
soonerbot 16:6d900f687c18 116 }
sswatek 24:46eeab7cebc6 117 return 0;
soonerbot 16:6d900f687c18 118 }
soonerbot 16:6d900f687c18 119 int droidBT::bufSize(){
soonerbot 16:6d900f687c18 120 return (BTEnd+BTBUFSIZE-BTStart)%BTBUFSIZE;
soonerbot 16:6d900f687c18 121 }
soonerbot 16:6d900f687c18 122
soonerbot 16:6d900f687c18 123 //returns both bits in an int form to look for
soonerbot 16:6d900f687c18 124 int droidBT::setState(char State) {
soonerbot 16:6d900f687c18 125 int sendbuf[10];
soonerbot 16:6d900f687c18 126 sendbuf[0]=0xF3;
soonerbot 16:6d900f687c18 127 sendbuf[1]=State;
soonerbot 16:6d900f687c18 128 sendbuf[2]=0x11;
soonerbot 16:6d900f687c18 129 sendbuf[3]=0x44;
soonerbot 16:6d900f687c18 130 int i, cheapHash=0;
soonerbot 16:6d900f687c18 131 droid.putc(sendbuf[0]);
soonerbot 16:6d900f687c18 132 for(i=1;i<4;i++){
soonerbot 16:6d900f687c18 133 droid.putc(sendbuf[i]);
soonerbot 16:6d900f687c18 134 cheapHash^=sendbuf[i];
soonerbot 16:6d900f687c18 135 }
sswatek 24:46eeab7cebc6 136 return 0xF300|(cheapHash&0xFF);
soonerbot 16:6d900f687c18 137 }
soonerbot 16:6d900f687c18 138
soonerbot 16:6d900f687c18 139 int droidBT::sendCmd(int cmd, int* info, int len) {
sswatek 17:e247d58d9f42 140 //int sendbuf[10];
sswatek 17:e247d58d9f42 141 //sendbuf[0]=(0xF0|(len+1));
sswatek 17:e247d58d9f42 142 //sendbuf[1]=cmd;
sswatek 17:e247d58d9f42 143 //sendbuf[2]=0x11;
sswatek 17:e247d58d9f42 144 //sendbuf[3]=0x44;
soonerbot 16:6d900f687c18 145 int i, cheapHash=cmd;
soonerbot 16:6d900f687c18 146 droid.putc((0xF0|(len+1)));
soonerbot 16:6d900f687c18 147 droid.putc(cmd);
soonerbot 16:6d900f687c18 148 for(i=0;i<len;i++){
soonerbot 16:6d900f687c18 149 droid.putc(info[i]);
soonerbot 16:6d900f687c18 150 cheapHash^=info[i];
soonerbot 16:6d900f687c18 151 }
sswatek 23:53cafcd67828 152 return 0xF000|((len+1)<<8)|(cheapHash&0xFF);
soonerbot 16:6d900f687c18 153 }
soonerbot 16:6d900f687c18 154
soonerbot 16:6d900f687c18 155 int droidBT::getAck(int hashetc){
soonerbot 16:6d900f687c18 156 int firstbit=(hashetc&0xFF00)>>8;
soonerbot 16:6d900f687c18 157 int secondbit=hashetc&0xFF;
soonerbot 16:6d900f687c18 158 int i;
soonerbot 16:6d900f687c18 159 if(BTStart==BTEnd) return 0;
soonerbot 16:6d900f687c18 160 for(i=BTStart;i!=BTEnd-1;i=(i+1)%BTBUFSIZE){
soonerbot 16:6d900f687c18 161 if(BTBuffer[i]==firstbit && BTBuffer[i+1]==secondbit){
soonerbot 16:6d900f687c18 162 BTBuffer[i]=BTBuffer[i+1]=0;
soonerbot 16:6d900f687c18 163 if(i==BTStart)
soonerbot 16:6d900f687c18 164 BTStart+=2;
soonerbot 16:6d900f687c18 165 return 1;
soonerbot 16:6d900f687c18 166 }
soonerbot 16:6d900f687c18 167 }
soonerbot 16:6d900f687c18 168 return 0;
soonerbot 16:6d900f687c18 169 }