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@19:3d77dc964d4b, 2014-03-12 (annotated)

Committer:
sswatek
Date:
Wed Mar 12 22:15:50 2014 +0000
Revision:
19:3d77dc964d4b
Parent:
17:e247d58d9f42
Child:
22:ac1f69a841ce
test publish

Who changed what in which revision?

UserRevisionLine numberNew contents of line
soonerbot 16:6d900f687c18 1 #include "droidBT.h"
soonerbot 16:6d900f687c18 2 #include "dbgprint.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
soonerbot 16:6d900f687c18 20 //int b = LPC_UART2->RBR;
soonerbot 16:6d900f687c18 21 //led3=1;
sswatek 19:3d77dc964d4b 22 //with
soonerbot 16:6d900f687c18 23 //int c = LPC_UART3->RBR;
sswatek 19:3d77dc964d4b 24 //messing
soonerbot 16:6d900f687c18 25 //led4=1;
soonerbot 16:6d900f687c18 26 //DBGPRINT("Got %x\n", BTBuffer[BTEnd]);
soonerbot 16:6d900f687c18 27 BTEnd++;
soonerbot 16:6d900f687c18 28 BTEnd%=BTBUFSIZE;
soonerbot 16:6d900f687c18 29 //led1=0;
soonerbot 16:6d900f687c18 30 //}
soonerbot 16:6d900f687c18 31 }
soonerbot 16:6d900f687c18 32
soonerbot 16:6d900f687c18 33 int droidBT::clearData(){
soonerbot 16:6d900f687c18 34 DCStart=DCEnd=0;
soonerbot 16:6d900f687c18 35 return 1;
soonerbot 16:6d900f687c18 36 }
soonerbot 16:6d900f687c18 37
soonerbot 16:6d900f687c18 38 int droidBT::getData(int cmd, int* dest){
soonerbot 16:6d900f687c18 39 procBuf(1);
soonerbot 16:6d900f687c18 40 int i;
soonerbot 16:6d900f687c18 41 for(i=DCStart;i!=DCEnd;i=(i+1)%DCHUNKSIZE){
soonerbot 16:6d900f687c18 42 if((DataChunk[i][0]&0xF0)==0x60 && DataChunk[i][1]==cmd){
soonerbot 16:6d900f687c18 43 DataChunk[i][0]=0;
soonerbot 16:6d900f687c18 44 if(i==DCStart){
soonerbot 16:6d900f687c18 45 DCStart++;
soonerbot 16:6d900f687c18 46 }
soonerbot 16:6d900f687c18 47 for(int j=0;j<17;j++)
soonerbot 16:6d900f687c18 48 dest[j]=DataChunk[i][j];
soonerbot 16:6d900f687c18 49 return 1;
soonerbot 16:6d900f687c18 50 }
soonerbot 16:6d900f687c18 51 }
soonerbot 16:6d900f687c18 52 return 0;
soonerbot 16:6d900f687c18 53 }
soonerbot 16:6d900f687c18 54
soonerbot 16:6d900f687c18 55 int droidBT::procBuf(int cmd){
soonerbot 16:6d900f687c18 56 int i=BTStart;
soonerbot 16:6d900f687c18 57 int startChunk=i;
soonerbot 16:6d900f687c18 58 int seen=0;
soonerbot 16:6d900f687c18 59 while (i!=BTEnd) {
soonerbot 16:6d900f687c18 60 if(i==startChunk){ //first bit in the chunk
soonerbot 16:6d900f687c18 61 if((BTBuffer[i]&0xF0)==0x60){//data returned
soonerbot 16:6d900f687c18 62 i++;
soonerbot 16:6d900f687c18 63 DBGPRINT("gettin data\r\n",1);
soonerbot 16:6d900f687c18 64 } else if((BTBuffer[i]&0xF0)==0xC0){//ack
soonerbot 16:6d900f687c18 65 i++;
soonerbot 16:6d900f687c18 66 DBGPRINT("gettin ack\r\n",1);
soonerbot 16:6d900f687c18 67 } else {//dunno what this data is, toss
soonerbot 16:6d900f687c18 68 BTStart++;
soonerbot 16:6d900f687c18 69 i++;
soonerbot 16:6d900f687c18 70 startChunk=i;
soonerbot 16:6d900f687c18 71 DBGPRINT("throwing away\r\n",1);
soonerbot 16:6d900f687c18 72 }
soonerbot 16:6d900f687c18 73 } else {
soonerbot 16:6d900f687c18 74 DBGPRINT("not first\r\n",1);
soonerbot 16:6d900f687c18 75 if(bufSize()>=(BTBuffer[startChunk]&0xF)+1){
soonerbot 16:6d900f687c18 76 DBGPRINT("got enough space %x, %d\r\n",BTBuffer[startChunk]&0xF, DCEnd);
soonerbot 16:6d900f687c18 77 DataChunk[DCEnd][0]=BTBuffer[startChunk];
soonerbot 16:6d900f687c18 78 int j;
soonerbot 16:6d900f687c18 79 for(j=0;j<(BTBuffer[startChunk]&0xF);j++){
soonerbot 16:6d900f687c18 80 DBGPRINT("copying %d, %d\r\n",BTBuffer[i+j], j);
soonerbot 16:6d900f687c18 81 DataChunk[DCEnd][j+1]=BTBuffer[i+j];
soonerbot 16:6d900f687c18 82 }
soonerbot 16:6d900f687c18 83 if(DCEnd>=DCHUNKSIZE)
soonerbot 16:6d900f687c18 84 DCEnd=0;
soonerbot 16:6d900f687c18 85 DBGPRINT("Got Data of size %d [%x %d %d]\n", BTBuffer[startChunk]&0xF, DataChunk[DCEnd][1], DataChunk[DCEnd][2], DataChunk[DCEnd][3]);
soonerbot 16:6d900f687c18 86 DCEnd++;
soonerbot 16:6d900f687c18 87 BTStart=i+j;
soonerbot 16:6d900f687c18 88 startChunk=i=BTStart;
soonerbot 16:6d900f687c18 89 } else {
soonerbot 16:6d900f687c18 90 break;
soonerbot 16:6d900f687c18 91 }
soonerbot 16:6d900f687c18 92 }
soonerbot 16:6d900f687c18 93
soonerbot 16:6d900f687c18 94 }
soonerbot 16:6d900f687c18 95 return seen;
soonerbot 16:6d900f687c18 96 }
soonerbot 16:6d900f687c18 97 int droidBT::bufSize(){
soonerbot 16:6d900f687c18 98 return (BTEnd+BTBUFSIZE-BTStart)%BTBUFSIZE;
soonerbot 16:6d900f687c18 99 }
soonerbot 16:6d900f687c18 100
soonerbot 16:6d900f687c18 101 //returns both bits in an int form to look for
soonerbot 16:6d900f687c18 102 int droidBT::setState(char State) {
soonerbot 16:6d900f687c18 103 int sendbuf[10];
soonerbot 16:6d900f687c18 104 sendbuf[0]=0xF3;
soonerbot 16:6d900f687c18 105 sendbuf[1]=State;
soonerbot 16:6d900f687c18 106 sendbuf[2]=0x11;
soonerbot 16:6d900f687c18 107 sendbuf[3]=0x44;
soonerbot 16:6d900f687c18 108 int i, cheapHash=0;
soonerbot 16:6d900f687c18 109 droid.putc(sendbuf[0]);
soonerbot 16:6d900f687c18 110 for(i=1;i<4;i++){
soonerbot 16:6d900f687c18 111 droid.putc(sendbuf[i]);
soonerbot 16:6d900f687c18 112 cheapHash^=sendbuf[i];
soonerbot 16:6d900f687c18 113 }
soonerbot 16:6d900f687c18 114 return 0xF300|cheapHash;
soonerbot 16:6d900f687c18 115 }
soonerbot 16:6d900f687c18 116
soonerbot 16:6d900f687c18 117 int droidBT::sendCmd(int cmd, int* info, int len) {
sswatek 17:e247d58d9f42 118 //int sendbuf[10];
sswatek 17:e247d58d9f42 119 //sendbuf[0]=(0xF0|(len+1));
sswatek 17:e247d58d9f42 120 //sendbuf[1]=cmd;
sswatek 17:e247d58d9f42 121 //sendbuf[2]=0x11;
sswatek 17:e247d58d9f42 122 //sendbuf[3]=0x44;
soonerbot 16:6d900f687c18 123 int i, cheapHash=cmd;
soonerbot 16:6d900f687c18 124 droid.putc((0xF0|(len+1)));
soonerbot 16:6d900f687c18 125 droid.putc(cmd);
soonerbot 16:6d900f687c18 126 for(i=0;i<len;i++){
soonerbot 16:6d900f687c18 127 droid.putc(info[i]);
soonerbot 16:6d900f687c18 128 cheapHash^=info[i];
soonerbot 16:6d900f687c18 129 }
soonerbot 16:6d900f687c18 130 return 0xF000|((len+1)<<2)|cheapHash;
soonerbot 16:6d900f687c18 131 }
soonerbot 16:6d900f687c18 132
soonerbot 16:6d900f687c18 133 int droidBT::getAck(int hashetc){
soonerbot 16:6d900f687c18 134 int firstbit=(hashetc&0xFF00)>>8;
soonerbot 16:6d900f687c18 135 int secondbit=hashetc&0xFF;
soonerbot 16:6d900f687c18 136 int i;
soonerbot 16:6d900f687c18 137 if(BTStart==BTEnd) return 0;
soonerbot 16:6d900f687c18 138 for(i=BTStart;i!=BTEnd-1;i=(i+1)%BTBUFSIZE){
soonerbot 16:6d900f687c18 139 if(BTBuffer[i]==firstbit && BTBuffer[i+1]==secondbit){
soonerbot 16:6d900f687c18 140 BTBuffer[i]=BTBuffer[i+1]=0;
soonerbot 16:6d900f687c18 141 if(i==BTStart)
soonerbot 16:6d900f687c18 142 BTStart+=2;
soonerbot 16:6d900f687c18 143 return 1;
soonerbot 16:6d900f687c18 144 }
soonerbot 16:6d900f687c18 145 }
soonerbot 16:6d900f687c18 146 return 0;
soonerbot 16:6d900f687c18 147 }