Sooner Competitive Robotics
/
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.
Fork of
IEEE_14_Freescale
by 
IEEE 2014 Mbed
Diff: robot.cpp
- Revision:
- 27:688409727452
- Parent:
- 26:ade7c813538f
- Child:
- 29:1132155bc7da
--- a/robot.cpp Fri Mar 14 00:38:51 2014 +0000
+++ b/robot.cpp Fri Mar 14 02:13:10 2014 +0000
@@ -19,6 +19,8 @@
//storage for shape positions
circleX=circleY=rectX=rectY=rectRot=triX=triY=triRot=0;
+ cameraMode=-1;
+ flameLocation=0;
}
//driveforward, but set up so that
@@ -157,7 +159,7 @@
}
gyro.start();
return totalMove*dir;
- tim.stop();//may not need
+ //tim.stop();//may not need
}
//this is the main thing that both turns and goes forward
@@ -332,11 +334,11 @@
double currangle=double(gyro.getZ())*2*3.14159/4050000.0;
double angle=atan2(xInches-x,yInches-y)*180.0/3.14159;
double finangle=angle;
- /*if(int(currangle-angle)%360>180){ //needs to turn positive degrees
- finangle=currangle+double(int(angle-currangle)%360);
- } else {//negative degrees
- finangle=currangle-double(int(currangle-angle)%360);
- }
+ //if(int(currangle-angle)%360>180){ //needs to turn positive degrees
+ // finangle=currangle+double(int(angle-currangle)%360);
+ //} else {//negative degrees
+ // finangle=currangle-double(int(currangle-angle)%360);
+ //}
double acc=turn(0.4,finangle);
if(acc<-0.75 && acc>0.75){
acc=turn(0.3,finangle);
@@ -393,63 +395,111 @@
return (gyroticks-nowz)*dir;
}*/
-int robot::pollForShapes(){
+int robot::switchCameraMode(int mode){
+ if(mode==cameraMode){
+ return 1;
+ }
int response=0;
int gotAck=0;
- int responseData[16];
int testdata[]={'H','e','l','l','o'};
stepTimer.start();
- while(1){ //put the phone in mode 1 to start finding shapes
- response=BTLink.sendCmd(0x01,testdata,5);
+ while(1){
+ response=BTLink.sendCmd(mode,testdata,5);
stepTimer.reset();
while(1){
gotAck=BTLink.getAck(response);
if(gotAck || stepTimer.read_ms()>=500)
break;
}
- DBGPRINT("Mode1=%d, %d, %d [%d]\n\r",response,BTLink.bufSize(), gotAck, stepTimer.read_ms());
- if(gotAck)
- break;
- }
- while(1){ //scan the information we get back to find valid data
- stepTimer.reset();
- gotAck=0;
- while(1){
- BTLink.procBuf(0x01);
- gotAck=BTLink.getData(0x01, responseData);
- if(gotAck || stepTimer.read_ms()>=4000)
- break;
- }
- if(responseData[15]==1&&gotAck){
- for(int i=0;i<16;i++)
- DBGPRINT("%d,",responseData[i]);
- DBGPRINT("\n\r",1);
- circleX=circleY=rectX=rectY=rectRot=triX=triY=triRot=0;
- rectX=(responseData[4]<<8)|responseData[3];
- rectY=(responseData[6]<<8)|responseData[5];
- rectRot=responseData[2];
- circleX=(responseData[12]<<8)|responseData[11];
- circleY=(responseData[14]<<8)|responseData[13];
- triX=(responseData[8]<<8)|responseData[7];
- triY=(responseData[10]<<8)|responseData[9];
- triRot=responseData[15];
- DBGPRINT("Rect(%d, %d, %d) Tri(%d, %d, %d) Cir(%d, %d)\n\r",rectX,rectY,rectRot,triX,triY,triRot,circleX,circleY);
- break;
- } else {
- DBGPRINT("Did not pass hash check %d %d %d\n\r",responseData[15],gotAck,stepTimer.read_ms());
- }
- }
- while(1){ //put the phone back in default state
- response=BTLink.sendCmd(0x00,testdata,5);
- stepTimer.reset();
- while(1){
- gotAck=BTLink.getAck(response);
- if(gotAck || stepTimer.read_ms()>=500)
- break;
- }
- DBGPRINT("BackToNormal=%d, %d, %d [%d]\n\r",response,BTLink.bufSize(), gotAck, stepTimer.read_ms());
+ DBGPRINT("Mode%d=%d, %d, %d [%d]\n\r",mode,response,BTLink.bufSize(), gotAck, stepTimer.read_ms());
if(gotAck)
break;
}
stepTimer.stop();
+ cameraMode=mode;
+ return 1;
+}
+int robot::shapeCheck(){
+ if(cameraMode!=1){
+ switchCameraMode(1);
+ }
+ int gotAck=0;
+ int responseData[16];
+ stepTimer.start();
+ stepTimer.reset();
+ gotAck=0;
+ while(1){
+ BTLink.procBuf(0x01);
+ gotAck=BTLink.getData(0x01, responseData);
+ if(gotAck || stepTimer.read_ms()>=1000)
+ break;
+ }
+ stepTimer.stop();
+ if(responseData[15]==1&&gotAck){
+ for(int i=0;i<16;i++)
+ DBGPRINT("%d,",responseData[i]);
+ DBGPRINT("\n\r",1);
+ circleX=circleY=rectX=rectY=rectRot=triX=triY=triRot=0;
+ rectX=(responseData[4]<<8)|responseData[3];
+ rectY=(responseData[6]<<8)|responseData[5];
+ rectRot=responseData[2];
+ circleX=(responseData[12]<<8)|responseData[11];
+ circleY=(responseData[14]<<8)|responseData[13];
+ triX=((responseData[8]&0xF)<<8)|responseData[7];
+ triY=((responseData[10]&0xF)<<8)|responseData[9];
+ triRot=((responseData[8]&0xF0)>>4)|(responseData[10]&0xF0);
+ DBGPRINT("Rect(%d, %d, %d) Tri(%d, %d, %d) Cir(%d, %d)\n\r",rectX,rectY,rectRot,triX,triY,triRot,circleX,circleY);
+ return 1;
+ } else {
+ DBGPRINT("Did not pass hash check %d %d %d\n\r",responseData[15],gotAck,stepTimer.read_ms());
+ return 0;
+ }
+}
+int robot::pollForShapes(){
+ switchCameraMode(1);
+ BTLink.clearData();
+ int found=0;
+ for(int i=0;i<5;i++){
+ found=shapeCheck();
+ if(found)
+ break;
+ }
+ switchCameraMode(0);
+ return found;
+}
+int robot::rigCheck(){
+ if(cameraMode!=2){
+ switchCameraMode(2);
+ }
+ int gotAck=0;
+ int responseData[16];
+ stepTimer.start();
+ stepTimer.reset();
+ while(1){
+ BTLink.procBuf(0x02);
+ gotAck=BTLink.getData(0x02, responseData);
+ if(gotAck || stepTimer.read_ms()>=1000)
+ break;
+ }
+ stepTimer.stop();
+ if(responseData[3]==1 && gotAck){
+ flameLocation = responseData[2];
+ DBGPRINT("Found fire at %d in %d\n\r",flameLocation,stepTimer.read_ms());
+ return 1;
+ } else {
+ DBGPRINT("Did not pass hash check %d %d %d\n\r",responseData[3],gotAck,stepTimer.read_ms());
+ return 0;
+ }
+}
+int robot::pollForRigs(){
+ switchCameraMode(2);
+ BTLink.clearData();
+ int found=0;
+ for(int i=0;i<5;i++){
+ found=rigCheck();
+ if(found)
+ break;
+ }
+ switchCameraMode(0);
+ return found;
}
\ No newline at end of file