Ryan Greiner
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 |
diff -r bc55afdd43de -r 48c8fb81288e main.cpp
--- 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)
{