Programming for Shadow Robot control for smart phone mapping application.
Dependencies: Motor LSM9DS1_Library_cal mbed Servo
Revision 5:48c8fb81288e, committed 2016-12-09
- Comitter:
- greiner218
- Date:
- Fri Dec 09 02:39:10 2016 +0000
- Parent:
- 4:bc55afdd43de
- Commit message:
- Tuned slam.
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Fri Dec 09 01:10:31 2016 +0000 +++ b/main.cpp Fri Dec 09 02:39:10 2016 +0000 @@ -238,7 +238,7 @@ case(1): //Ping Left Sonar // trigger sonar to send a ping - pc.printf("Pinging sonar 1...\n\r"); + //pc.printf("Pinging sonar 1...\n\r"); triggerL = 1; sonar.reset(); //wait_us(10.0); @@ -258,20 +258,20 @@ timeout.reset(); timeout.start(); while (echoL==1) { - if (timeout.read_ms() > TIMEOUT_VAL_MS*3) return 0; + if (timeout.read_ms() > TIMEOUT_VAL_MS*5) return 0; }; echo1FallWait = 0; //stop timer and read value sonar.stop(); //subtract software overhead timer delay and scale to cm distance = (sonar.read_us()-correction)/58.0; - pc.printf("Got distance %f cm back.\n\r", distance); + //pc.printf("Got distance %f cm back.\n\r", distance); //wait so that any echo(s) return before sending another ping wait(0.015); break; case(2): //Ping Right Sonar // trigger sonar to send a ping - pc.printf("Pinging sonar 2...\n\r"); + //pc.printf("Pinging sonar 2...\n\r"); triggerR = 1; sonar.reset(); //wait_us(10.0); @@ -291,14 +291,14 @@ timeout.reset(); timeout.start(); while (echoR==1) { - if (timeout.read_ms() > TIMEOUT_VAL_MS*3) return 0; + if (timeout.read_ms() > TIMEOUT_VAL_MS*5) return 0; }; echo2FallWait = 0; //stop timer and read value sonar.stop(); //subtract software overhead timer delay and scale to cm distance = (sonar.read_us()-correction)/58.0; - pc.printf("Got distance %f cm back.\n\r", distance); + //pc.printf("Got distance %f cm back.\n\r", distance); //wait so that any echo(s) return before sending another ping wait(0.015); break; @@ -331,18 +331,20 @@ sendCmd('A',AL); sendCmd('D',dL); dist_list[slam_ind] = dL; - angle_list[slam_ind] = AL; + angle_list[slam_ind] = AL-heading; if (slam_ind < 63) slam_ind++; + pc.printf("(%.1f,%.1f)\n\r",AL-heading,dL); } if (dR > 20 && dR < 200) { sendCmd('A',AR); sendCmd('D',dR); dist_list[slam_ind] = dL; - angle_list[slam_ind] = AR; + angle_list[slam_ind] = AR-heading; if (slam_ind < 63) slam_ind++; + pc.printf("(%.1f,%.1f)\n\r",AR-heading,dR); } - angle = angle + 0.1; + angle = angle + 0.05; } angle = 1; } @@ -361,18 +363,20 @@ sendCmd('A',AL); sendCmd('D',dL); dist_list[slam_ind] = dL; - angle_list[slam_ind] = AL; + angle_list[slam_ind] = AL-heading; if (slam_ind < 63) slam_ind++; + pc.printf("(%.1f,%.1f)\n\r",AL-heading,dL); } if (dR > 20 && dR < 200) { sendCmd('A',AR); sendCmd('D',dR); dist_list[slam_ind] = dL; - angle_list[slam_ind] = AR; + angle_list[slam_ind] = AR-heading; if (slam_ind < 63) slam_ind++; + pc.printf("(%.1f,%.1f)\n\r",AR-heading,dR); } - angle = angle - 0.1; + angle = angle - 0.05; } angle = 0; } @@ -398,16 +402,16 @@ dist = (tickDistR + tickDistL)/2.0f*DIST_FACTOR; dist = (dirL + dirR)*dist/2.0; //Average the ticks and convert to linear distance. slam_dist += dist; - pc.printf("Magnetic Heading: %f degress ",heading); - pc.printf("L ticks: %d R ticks: %d ",tickDistL, tickDistR); + // pc.printf("Magnetic Heading: %f degress ",heading); + //pc.printf("L ticks: %d R ticks: %d ",tickDistL, tickDistR); tickDistR = 0;//Reset the ticks. tickDistL = 0; deltaX = dist*(float)sin((double)heading*PI/180.0f); //Do trig. deltaY = dist*(float)cos((double)heading*PI/180.0f); posX = posX + deltaX; posY = posY + deltaY; - pc.printf("deltaX: %f deltaY: %f ",deltaX,deltaY); - pc.printf("posX: %f posY:%f \n\r",posX,posY); + //pc.printf("deltaX: %f deltaY: %f ",deltaX,deltaY); + //pc.printf("posX: %f posY:%f \n\r",posX,posY); sendCmd('X',posX); sendCmd('Y',posY); sendCmd('H',heading); @@ -494,9 +498,9 @@ //Iterate to determine if detection in "danger zone" for (int i = 0; i<64; i++) { - if ((angle_list[i] - heading) < 60 && (angle_list[i] - heading) > -60) + if ((angle_list[i]) < 80 && (angle_list[i]) > -80) { - if (dist_list[i] < 30) + if (dist_list[i] < 50 && dist_list[i] > 1) { return 1; @@ -505,6 +509,7 @@ } return 0; } + void slam() { @@ -517,7 +522,8 @@ { //move forward 20cm and scan moveFwd(); - while(slam_dist < 20)pc.printf("SlamDist: %f\n\r",slam_dist); + //while(slam_dist < 20)pc.printf("SlamDist: %f\n\r",slam_dist); + wait(.8); moveStp(); slam_dist = 0; detect = slam_sweep(); @@ -525,7 +531,7 @@ //Else, turn left slightly. detect = 0; turnL(); - wait(1); + wait(.8); moveStp(); //and scan detect = slam_sweep(); @@ -563,7 +569,7 @@ switch (cmd) { case 'B': - pc.printf("A button was pressed!\n\r"); + //pc.printf("A button was pressed!\n\r"); cmd = bt.getc(); switch(cmd) {