Posted 21 October 2023,
The WALL_OFFSET_DIST_AHEAD anomaly is triggered when WallE3 is about to run head-on into an upcoming wall. The idea behind handling this anomaly is to allow WallE3 to navigate around internal corners. Up until a few days ago, WALL_OFFSET_DIST_AHEAD anomaly just called ‘BackupAndTurn90Deg(bool bIsCCW)’. This function backed the robot up to achieve the desired wall offset, turned 90º in the direction away from the last tracked wall to parallel the upcoming wall and then exited, whereupon loop() was re-entered from the top and a new tracking assessment was made.
However, this treatment failed spectacularly in the guest bedroom, as the ‘new’ wall was too short to track properly. This problem led to the development of the ‘RunToDaylight’ algorithm to allow WallE3 to find the best direction in which to travel. I now have RunToDaylightV2() working very nicely, but now I have the opposite problem; now RunToDaylightV2() is more likely than not to simply turn WallE3 180º and go back the way he came – perfectly legitimate from RunToDaylight’s point of view, but boring and too simplistic.
So, how to mix the two approaches (BackupAndTurn90Deg & RunToDaylight) so that RunToDaylight is used in tight corners, but BackupAndTurn90Deg is used for ‘normal’ wall configurations where there is ample (or at least reasonable) room for travel on the perpendicular wall.
I’m going to try combining the two options. The idea is that BackupAndTurn90Deg would be the default response to a WALL_OFFSET_DIST_AHEAD anomaly, but if the available front distance after the 90º turn is less than something like 2 x WALL_OFFSET_DIST_AHEAD (currently set at 30cm), then the RunToDaylight will be called to find a better direction in which to move.
This turned out to be pretty easy. The changes necessary to the WALL_OFFSET_DIST_AHEAD anomaly case in HandleAnomalousConditions is shown below:
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 |
case ANOMALY_WALL_OFFSET_DIST_AHEAD: {//09/30/23 added braces for scoping, collapsibility //09/26/23 ported from WallE3_Complete_V4.ino //gl_LastAnomalyCode = ANOMALY_WALL_OFFSET_DIST_AHEAD; //added 03/14/23 gl_pSerPort->printf("ANOMALY_WALL_OFFSET_DIST_AHEAD case detected with tracking case %s\n", WallTrackStrArray[trackcase]); StopBothMotors(); delay(1000); //delay just for visual indication that we got here //10/20/23 Replace with RunToDaylightV2() //10/22/23 Now trying to combine BackupAndTurn90Deg with RunToDaylightV2() BackupAndTurn90Deg(trackcase == TRACKING_RIGHT); UpdateAllDistances(); //gl_CurTrackingCase = TRACKING_NEITHER; //added 09/30/23 to force fresh deternination of tracking side gl_pSerPort->printf("In ANOMALY_WALL_OFFSET_DIST_AHEAD case just after call to BackupAndTurn90Deg: front dist = %d\n", gl_FrontCm ); if (gl_FrontCm < (2 * WALL_OFFSET_TGTDIST_CM)) { gl_CurTrackingCase = RunToDaylightV2(); } //gl_CurTrackingCase = RunToDaylightV2(); gl_LastAnomalyCode = ANOMALY_NONE; //rev 09/30/23 to clear anomaly flag gl_pSerPort->printf("In ANOMALY_WALL_OFFSET_DIST_AHEAD case exit with last anomaly code = %s\n", AnomalyStrArray[gl_LastAnomalyCode]); } |
The following short video and telemetry shows the action when WallE3 encounters a corner with not enough room to track the next wall in the internal corner.
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 |
TrackLeftWallOffset: Start tracking offset of 30cm at 0.5 Just after CaptureWallOffset(TRACKING_LEFT, 18.2) Sec LCen RCen Deg LF LR LStr Front Rear FVar RVar LSpd RSpd ACODE TRKDIR 0.6 18.3 47.6 0.4 17.7 18.6 -0.09 139 31 75938 12971 127 0 NONE LEFT 0.7 18.1 47.5 5.7 17.7 18.8 -0.11 141 31 74819 12884 127 0 NONE LEFT 0.8 17.5 50.3 15.3 17.9 18.1 -0.02 142 31 74915 12853 127 0 NONE LEFT 0.9 18.8 288.1 26.4 19.8 18.6 0.06 136 42 76156 12830 127 11 NONE LEFT 1.0 23.3 309.8 38.1 23.7 20.1 0.36 78 68 79912 12718 31 118 NONE LEFT 1.1 27.3 176.6 45.2 28.0 25.7 0.23 61 67 85222 12506 28 121 NONE LEFT 1.2 31.9 168.4 43.9 33.9 29.9 0.40 58 42 91230 12602 0 127 NONE LEFT 1.3 33.6 206.6 38.5 36.6 31.2 0.54 58 68 97538 12657 0 127 NONE LEFT 1.4 34.0 299.9 31.5 35.9 31.8 0.41 64 146 103665 12413 0 127 NONE LEFT 1.5 34.0 38.6 22.1 35.1 32.4 0.27 92 82 109006 12336 0 127 NONE LEFT 1.6 33.7 34.3 10.3 33.7 33.1 0.06 115 61 113009 12468 31 119 NONE LEFT 1.7 33.9 30.8 -2.3 33.1 33.6 -0.05 110 70 116525 12626 68 81 NONE LEFT 1.8 35.0 28.9 -10.7 33.8 34.9 -0.12 110 160 119155 12477 89 60 NONE LEFT 1.9 34.8 29.0 -12.2 33.2 35.3 -0.21 106 140 120642 12398 127 14 NONE LEFT 2.0 33.9 30.0 -9.5 32.5 34.8 -0.23 100 146 120733 12338 127 0 NONE LEFT 2.1 32.3 30.3 -4.1 31.1 32.6 -0.15 95 156 119031 12284 119 30 NONE LEFT 2.2 31.5 31.9 2.1 29.4 31.0 -0.16 90 179 115183 12215 127 18 NONE LEFT 2.3 29.1 31.2 9.9 28.4 29.5 -0.11 86 90 108782 12380 127 22 NONE LEFT 2.4 29.7 32.1 17.9 29.7 28.7 -0.01 84 94 99392 12577 86 63 NONE LEFT 2.5 30.9 32.9 23.1 31.3 29.9 0.14 84 82 86570 12794 33 116 NONE LEFT 2.6 32.6 32.6 22.4 33.9 31.7 0.22 79 76 69936 13088 0 127 NONE LEFT 2.7 33.4 30.1 18.6 33.7 32.6 0.11 73 93 49052 13321 20 127 NONE LEFT 2.8 34.7 27.5 12.1 35.3 33.6 0.17 67 114 23427 13462 2 127 NONE LEFT 2.9 34.8 25.2 3.7 34.6 34.1 0.05 61 118 780 13585 28 121 NONE LEFT 3.0 35.2 24.3 -5.4 34.4 35.2 -0.08 57 140 751 13665 66 83 NONE LEFT 3.1 36.4 25.0 -11.4 35.2 36.3 -0.10 53 192 710 13658 75 74 NONE LEFT 3.2 36.5 26.5 -12.3 34.6 36.7 -0.21 49 194 646 13644 119 30 NONE LEFT 3.3 35.0 26.6 -10.8 33.0 34.6 -0.16 44 200 567 13621 105 45 NONE LEFT 3.4 33.2 27.7 -7.3 32.1 32.8 -0.07 41 205 479 13586 79 70 NONE LEFT 3.5 32.3 28.1 -4.3 31.0 32.4 -0.14 36 206 532 13536 116 33 NONE LEFT 3.6 30.8 29.2 -1.3 29.9 31.1 -0.12 32 195 598 13463 124 25 NONE LEFT Stopping Motors! Anomaly Code is 4 --> WALL_OFFSET_DIST_AHEAD 3.7: Top of loop() - calling UpdateAllEnvironmentParameters() Battery Voltage = 8.00 Just after first UpdateAllEnvironmentParameters() & 200mSec delay at top of loop() 4.1: gl_Left/RightCenterCm = 30.6/29.4, Left/RightSteerVal = 0.02/-0.14 Just before call to HandleAnomalousConditions() with anomaly code = WALL_OFFSET_DIST_AHEAD In HandleAnomalousConditions(LEFT) with last anomaly code = WALL_OFFSET_DIST_AHEAD ANOMALY_WALL_OFFSET_DIST_AHEAD case detected with tracking case LEFT In BackupAndTurn90Deg(CW)with front distance = 17 MTFD: at start, tgt = 30cm, curr_dist = 18, front/rear var = 83325/13332 MTFD: Stopped with front dist = 28, anomaly code = WALL_OFFSET_DIST_AHEAD BackupAndTurn90Deg finished In ANOMALY_WALL_OFFSET_DIST_AHEAD case just after call to BackupAndTurn90Deg: front dist = 31 In RunToDaylightV2() RunToDaylightV2(): After REV sort, arrays are FrontD Hdg LeftD LSteer RightD RSteer 243 163.1 36.9 -0.3 29.3 0.3 209 172.5 34.4 -0.2 26.2 0.1 185 -177.3 34.1 -0.1 25.8 -0.1 166 -167.0 34.6 -0.0 26.8 -0.3 85 -157.5 35.7 0.2 28.6 -0.3 76 153.4 42.0 -0.4 31.3 0.4 54 143.4 48.9 -0.2 35.2 0.7 54 -146.8 38.6 0.4 32.1 -0.5 48 43.3 38.3 0.7 45.3 -0.7 42 133.3 44.3 0.4 44.3 1.0 40 53.7 45.1 0.2 56.5 -1.0 40 33.1 32.5 0.5 38.6 -0.5 40 -136.1 44.9 0.5 37.8 -0.8 40 -44.7 43.1 -0.7 49.2 -0.2 38 -34.3 35.2 -0.6 43.2 0.7 37 123.9 39.6 0.3 63.9 1.0 35 22.7 29.4 0.2 35.6 -0.3 35 64.0 42.1 -0.4 74.0 -1.0 34 114.4 36.2 0.2 98.8 1.0 33 93.4 35.3 -0.0 155.5 0.8 33 -54.2 59.0 -1.0 41.0 -0.6 33 74.4 36.8 -0.1 116.2 -1.0 33 83.9 36.3 -0.1 215.0 0.0 33 -24.7 31.5 -0.3 38.0 0.5 33 -15.1 28.5 -0.2 35.1 0.2 33 13.4 27.2 0.1 33.3 -0.2 32 104.8 34.2 0.1 149.2 0.9 32 -5.5 26.9 -0.1 33.7 0.1 32 3.9 26.4 -0.0 32.8 -0.0 31 -125.7 52.6 0.5 43.6 0.3 29 -64.8 92.0 -1.0 37.4 -0.4 28 -115.2 71.0 0.5 38.1 0.5 27 -75.1 140.9 0.2 33.8 -0.2 26 -85.3 194.8 -0.3 32.3 -0.1 26 -104.9 107.2 0.9 34.2 0.3 25 -95.6 211.6 0.2 32.9 0.1 FrontD Hdg LeftD LSteer RightD RSteer at index = 1, FrontD = 209, bWallTrackAvailable = 1 Trackable Wall found at index = 0 FrontD Hdg LeftD LSteer RightD RSteer 243 163.1 36.9 -0.3 29.3 0.3 Turning to heading = 163.1 Just after turning to 163.1 FrontD Hdg LeftD LSteer RightD RSteer 243 163.1 36.9 -0.3 29.3 0.3 Something Went Wrong! bWallTrackAvailable = true but couldn't decide which wall to track! FrontD Hdg LeftD LSteer RightD RSteer 243 163.1 36.9 -0.3 29.3 0.3 RunToDaylightV2() just before return with trkdir = NEITHER In ANOMALY_WALL_OFFSET_DIST_AHEAD case exit with last anomaly code = NONE 22.3: Top of loop() - calling UpdateAllEnvironmentParameters() Battery Voltage = 8.00 Just after first UpdateAllEnvironmentParameters() & 200mSec delay at top of loop() 22.8: gl_Left/RightCenterCm = 36.8/24.5, Left/RightSteerVal = -0.20/0.11 Just before call to HandleAnomalousConditions() with anomaly code = NONE In HandleAnomalousConditions(NEITHER) with last anomaly code = NONE In HandleAnomalousConditions(NEITHER) ANOMALY_NONE CASE 22.8: gl_Left/RightCenterCm = 38.9/24.8, Left/RightSteerVal = -0.22/0.11 TrackRightWallOffset(350.0, 0.0, 20.0, 30) called TrackRightWallOffset: Start tracking offset of 30cm at 22.8 Just after CaptureWallOffset(TRACKING_RIGHT, 24.8) Sec LCen RCen Deg RF RR RStr Front Rear FVar RVar LSpd RSpd ACODE TRKDIR 22.9 38.8 25.0 164.4 26.1 24.3 0.16 215 32 3496 16460 88 61 NONE RIGHT 23.0 38.5 24.9 164.6 25.7 24.3 0.18 211 33 3932 16578 89 61 NONE RIGHT 23.0 38.2 24.9 165.2 25.4 24.5 0.09 210 32 4333 16680 65 84 NONE RIGHT 23.1 38.2 24.8 165.5 25.8 24.3 0.15 212 33 4719 16765 77 72 NONE RIGHT 23.1 36.6 24.5 165.3 26.0 24.6 0.14 209 33 5063 16832 75 74 NONE RIGHT 23.2 37.4 24.8 165.0 26.3 24.7 0.16 208 34 5376 16877 81 68 NONE RIGHT 23.2 36.8 25.6 164.9 27.0 25.6 0.14 204 37 5644 16896 82 68 NONE RIGHT 23.3 35.5 26.0 164.7 27.5 25.4 0.21 204 39 5888 16887 105 44 NONE RIGHT 23.3 36.3 26.7 164.8 27.7 25.3 0.24 204 41 6109 16856 116 33 NONE RIGHT 23.4 35.3 26.9 165.4 28.4 25.8 0.26 200 42 6287 16799 117 33 NONE RIGHT 23.4 34.8 27.6 166.4 28.8 27.5 0.13 195 44 6422 16716 93 56 NONE RIGHT 23.5 34.1 28.3 168.1 29.5 27.4 0.21 195 46 6535 16605 118 31 NONE RIGHT 23.5 33.8 28.8 169.5 29.4 27.6 0.18 188 49 6609 16468 117 32 NONE RIGHT 23.5 32.5 28.5 170.9 29.8 28.3 0.15 189 51 6671 16302 107 42 NONE RIGHT 23.6 31.8 28.8 173.0 29.8 28.8 0.10 186 53 6711 16108 96 53 NONE RIGHT 23.6 31.2 29.3 174.8 29.8 29.4 0.03 187 55 6747 15883 79 70 NONE RIGHT 23.7 30.5 29.4 176.4 29.8 29.8 0.00 191 57 6800 15629 68 81 NONE RIGHT 23.8 30.0 29.7 177.0 29.9 29.2 0.07 191 60 6875 15345 91 58 NONE RIGHT 23.8 29.7 29.8 177.1 29.8 29.7 0.01 195 62 6825 15029 72 77 NONE RIGHT 23.9 29.9 30.0 177.1 29.9 29.9 0.00 193 64 6693 14682 74 75 NONE RIGHT 23.9 29.5 30.2 176.8 30.0 29.9 0.01 187 68 6659 14301 78 71 NONE RIGHT 24.0 29.9 29.9 176.5 30.4 30.1 0.03 186 71 6686 13889 78 71 NONE RIGHT 24.0 30.1 30.1 176.6 30.4 30.5 -0.01 182 73 6741 13445 71 78 NONE RIGHT 24.1 30.0 30.5 176.8 30.6 30.3 0.03 180 75 6774 12967 84 65 NONE RIGHT 24.1 29.6 30.8 176.8 30.7 30.3 0.04 178 79 6741 12455 88 61 NONE RIGHT 24.2 29.4 30.5 176.7 30.9 30.7 0.02 173 81 6646 11910 82 67 NONE RIGHT 24.2 29.1 30.4 176.7 30.6 30.5 0.01 172 85 6512 11329 78 71 NONE RIGHT 24.3 29.8 30.7 176.5 31.5 30.8 0.07 171 88 6351 10713 98 51 NONE RIGHT 24.3 29.6 31.2 176.3 31.5 30.7 0.08 169 91 6163 10063 109 40 NONE RIGHT 24.3 29.8 31.1 176.6 32.6 30.8 0.17 167 93 5957 9377 127 16 NONE RIGHT 24.4 29.1 32.3 177.0 33.2 31.4 0.12 162 96 5733 8656 124 25 NONE RIGHT 24.5 28.9 32.5 178.9 32.9 31.7 0.12 165 99 5504 7898 124 26 NONE RIGHT 24.5 29.3 33.1 -179.1 33.1 32.1 0.10 114 101 5278 7105 124 26 NONE RIGHT 24.6 30.0 32.5 -176.8 32.8 32.5 0.03 166 109 5064 6276 100 49 NONE RIGHT 24.6 30.1 33.1 -174.8 33.2 33.0 0.02 186 109 4871 5410 102 47 NONE RIGHT 24.6 31.7 32.7 -172.6 33.0 33.4 -0.04 73 114 4731 4507 76 73 NONE RIGHT 24.7 32.9 32.7 -170.7 32.7 33.8 -0.11 64 114 4608 3565 45 104 NONE RIGHT 24.7 33.8 33.1 -169.5 32.7 34.2 -0.15 60 114 4496 2582 36 113 NONE RIGHT 24.8 34.4 32.8 -170.0 32.0 34.3 -0.23 57 114 4391 1559 0 127 NONE RIGHT 24.8 34.8 32.4 -170.8 31.5 33.9 -0.24 54 118 4300 1567 0 127 NONE RIGHT 24.9 34.7 31.5 -172.4 31.0 33.5 -0.25 52 122 4223 1579 0 127 NONE RIGHT 24.9 35.3 31.2 -174.6 30.4 32.4 -0.20 57 127 4142 1596 0 127 NONE RIGHT 25.0 35.6 30.5 -177.6 29.9 32.3 -0.24 172 131 3918 1617 0 127 NONE RIGHT 25.1 36.2 29.2 178.1 29.1 30.0 -0.11 74 133 3805 1640 19 127 NONE RIGHT 25.1 36.0 28.9 174.1 29.0 29.8 -0.07 116 136 3561 1665 35 114 NONE RIGHT 25.1 36.1 28.8 170.9 29.3 29.1 -0.01 118 137 3292 1689 56 93 NONE RIGHT 25.2 36.6 29.3 166.9 29.7 28.5 0.12 107 139 3033 1711 100 49 NONE RIGHT 25.2 37.5 29.8 163.8 31.2 29.1 0.21 106 142 2772 1735 126 23 NONE RIGHT 25.3 37.8 30.6 162.7 32.2 29.6 0.26 103 146 2649 1762 127 6 NONE RIGHT 25.3 37.2 31.3 163.5 35.3 30.4 0.49 102 147 2656 1797 127 0 NONE RIGHT 25.4 36.3 32.0 164.3 39.4 30.5 0.89 100 150 2649 1837 127 0 NONE RIGHT RightSteer = 1.0, result = TRUE 25.4 36.4 33.5 166.2 43.9 31.0 1.00 99 155 2644 1886 127 0 EXCESS_STEER_VAL RIGHT Stopping Motors! Error Code is 8 --> EXCESS_STEER_VAL |
And the following video and telemetry shows the action when WallE3 does have room enough to follow the internal corner wall. Note that in these two conditions, the code was not changed – the only thing that changed between the two runs is the ‘third wall’ was moved away from the first one to give WallE3 sufficient room to follow the internal corner wall.
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 |
TrackLeftWallOffset: Start tracking offset of 30cm at 0.5 Just after CaptureWallOffset(TRACKING_LEFT, 23.7) Sec LCen RCen Deg LF LR LStr Front Rear FVar RVar LSpd RSpd ACODE TRKDIR 0.6 23.5 136.6 0.2 23.2 22.2 0.10 137 24 75967 13074 93 56 NONE LEFT 0.7 23.2 136.8 3.7 21.0 21.9 -0.09 134 25 75054 13028 127 0 NONE LEFT 0.8 23.0 138.1 11.0 23.4 21.7 0.17 134 29 75356 13018 76 73 NONE LEFT 0.9 23.6 305.1 18.8 25.4 22.7 -0.04 137 55 76696 12963 127 14 NONE LEFT 1.0 25.2 301.6 22.8 25.2 23.7 0.32 137 133 78929 12568 27 122 NONE LEFT 1.1 26.6 303.7 22.9 27.4 25.3 0.12 137 146 81980 12179 69 80 NONE LEFT 1.2 28.7 299.7 22.3 29.2 27.4 0.18 132 152 85727 11816 34 115 NONE LEFT 1.3 30.6 299.8 21.7 31.0 29.5 0.15 125 158 90040 11478 30 119 NONE LEFT 1.4 32.2 297.0 18.8 32.4 31.6 0.08 118 154 94698 11178 33 116 NONE LEFT 1.5 32.9 149.5 14.8 33.0 32.1 0.09 110 82 99527 11065 28 121 NONE LEFT 1.6 33.3 130.8 9.5 32.9 32.8 0.01 103 66 104233 11181 49 100 NONE LEFT 1.7 33.5 126.9 4.9 33.0 33.1 -0.01 96 66 108488 11345 59 90 NONE LEFT 1.8 33.9 122.7 3.5 32.1 33.8 -0.17 92 71 111963 11524 125 24 NONE LEFT 1.9 33.6 122.1 4.7 32.4 33.6 -0.11 88 76 114348 11704 101 48 NONE LEFT 2.0 33.3 122.3 7.8 33.2 33.3 -0.01 83 85 115268 11872 58 91 NONE LEFT 2.1 34.2 122.4 8.3 33.5 33.9 -0.04 78 91 114377 12025 60 89 NONE LEFT 2.2 34.4 121.3 7.2 34.0 33.5 0.05 72 97 111318 12175 28 121 NONE LEFT 2.3 34.6 120.0 4.5 34.0 35.2 -0.12 69 103 105664 12320 94 56 NONE LEFT 2.4 35.4 118.8 2.7 34.2 35.0 -0.08 63 108 97033 12460 66 83 NONE LEFT 2.5 35.0 118.6 1.9 34.2 35.3 -0.11 57 112 84970 12591 84 65 NONE LEFT 2.6 35.5 118.5 1.5 34.9 34.9 -0.06 52 118 69027 12712 61 88 NONE LEFT 2.7 34.8 117.8 1.2 34.4 34.3 0.01 48 123 48709 12823 44 105 NONE LEFT 2.8 35.2 118.5 0.3 34.1 35.0 -0.09 42 132 23529 12909 70 79 NONE LEFT 2.9 35.0 118.5 -0.5 34.1 34.4 -0.03 37 136 1162 12986 51 98 NONE LEFT 3.0 34.7 118.5 -1.3 34.5 34.6 -0.01 33 141 1233 13048 51 98 NONE LEFT Stopping Motors! Anomaly Code is 4 --> WALL_OFFSET_DIST_AHEAD 3.0: Top of loop() - calling UpdateAllEnvironmentParameters() Battery Voltage = 7.99 Just after first UpdateAllEnvironmentParameters() & 200mSec delay at top of loop() 3.5: gl_Left/RightCenterCm = 34.8/120.2, Left/RightSteerVal = 0.00/0.09 Just before call to HandleAnomalousConditions() with anomaly code = WALL_OFFSET_DIST_AHEAD In HandleAnomalousConditions(LEFT) with last anomaly code = WALL_OFFSET_DIST_AHEAD ANOMALY_WALL_OFFSET_DIST_AHEAD case detected with tracking case LEFT In BackupAndTurn90Deg(CW)with front distance = 14 MTFD: at start, tgt = 30cm, curr_dist = 15, front/rear var = 83325/13332 MTFD: Stopped with front dist = 30, anomaly code = WALL_OFFSET_DIST_AHEAD BackupAndTurn90Deg finished In ANOMALY_WALL_OFFSET_DIST_AHEAD case just after call to BackupAndTurn90Deg: front dist = 113 In ANOMALY_WALL_OFFSET_DIST_AHEAD case exit with last anomaly code = NONE 7.8: Top of loop() - calling UpdateAllEnvironmentParameters() Battery Voltage = 7.99 Just after first UpdateAllEnvironmentParameters() & 200mSec delay at top of loop() 8.3: gl_Left/RightCenterCm = 35.3/154.0, Left/RightSteerVal = -0.13/1.00 Just before call to HandleAnomalousConditions() with anomaly code = NONE In HandleAnomalousConditions(LEFT) with last anomaly code = NONE In HandleAnomalousConditions(LEFT) ANOMALY_NONE CASE 8.3: gl_Left/RightCenterCm = 35.5/154.3, Left/RightSteerVal = -0.07/1.00 gl_LeftCenterCm <= gl_RightCenterCm --> Calling TrackLeftWallOffset() TrackLeftWallOffset(350.0, 0.0, 20.0, 30) called TrackLeftWallOffset: Start tracking offset of 30cm at 8.3 Just after CaptureWallOffset(TRACKING_LEFT, 35.5) Sec LCen RCen Deg LF LR LStr Front Rear FVar RVar LSpd RSpd ACODE TRKDIR 8.5 35.3 153.6 86.7 33.7 35.1 -0.14 115 33 145053 8005 94 55 NONE LEFT 8.6 35.3 151.1 86.7 33.8 34.8 -0.10 112 34 143600 8487 80 69 NONE LEFT 8.7 35.2 163.1 86.4 33.6 34.7 -0.11 110 36 140307 8985 86 63 NONE LEFT 8.8 35.1 181.4 86.9 33.3 34.5 -0.12 106 40 134826 9477 87 62 NONE LEFT 8.9 34.9 191.9 87.1 33.8 34.8 -0.10 101 44 126798 9957 88 61 NONE LEFT 9.0 34.6 203.6 87.5 33.6 34.6 -0.10 95 49 115846 10418 88 62 NONE LEFT 9.1 34.5 207.8 88.1 33.2 34.5 -0.08 91 52 101521 10869 74 75 NONE LEFT 9.2 34.5 210.3 88.5 33.3 34.5 -0.12 85 57 83400 11299 96 53 NONE LEFT 9.3 34.6 213.6 89.3 33.3 34.3 -0.10 80 65 61019 11689 87 62 NONE LEFT 9.4 34.7 214.5 89.5 34.2 34.3 -0.01 77 70 33902 12052 51 98 NONE LEFT 9.5 35.0 215.6 89.2 34.2 34.3 -0.01 73 74 1562 12385 44 105 NONE LEFT 9.6 34.9 217.9 88.2 34.0 34.5 -0.05 70 79 1493 12686 64 85 NONE LEFT 9.7 35.0 224.1 87.9 33.7 35.0 -0.13 63 84 1411 12953 93 56 NONE LEFT 9.8 35.0 224.7 88.8 34.0 34.7 -0.09 57 89 1326 13185 71 78 NONE LEFT 9.9 34.9 227.7 89.1 33.3 34.6 -0.13 52 94 1240 13378 97 52 NONE LEFT 10.0 34.5 228.0 89.8 32.8 33.9 -0.11 46 102 1163 13518 94 55 NONE LEFT 10.1 33.9 230.3 91.3 32.7 33.4 -0.07 42 108 1095 13615 79 70 NONE LEFT 10.2 34.4 231.5 92.6 33.7 33.6 0.01 37 114 1044 13661 44 105 NONE LEFT 10.3 35.3 233.2 92.0 34.0 34.5 -0.05 32 120 1008 13657 56 93 NONE LEFT 10.4 35.8 235.1 90.5 35.1 34.7 0.04 29 125 984 13603 27 122 WALL_OFFSET_DIST_AHEAD LEFT Stopping Motors! Anomaly Code is 4 --> WALL_OFFSET_DIST_AHEAD 10.4: Top of loop() - calling UpdateAllEnvironmentParameters() Battery Voltage = 7.97 Just after first UpdateAllEnvironmentParameters() & 200mSec delay at top of loop() 10.8: gl_Left/RightCenterCm = 36.0/251.3, Left/RightSteerVal = -0.10/-0.25 Just before call to HandleAnomalousConditions() with anomaly code = WALL_OFFSET_DIST_AHEAD In HandleAnomalousConditions(LEFT) with last anomaly code = WALL_OFFSET_DIST_AHEAD ANOMALY_WALL_OFFSET_DIST_AHEAD case detected with tracking case LEFT In BackupAndTurn90Deg(CW)with front distance = 12 MTFD: at start, tgt = 30cm, curr_dist = 12, front/rear var = 83325/13332 MTFD: Stopped with front dist = 28, anomaly code = WALL_OFFSET_DIST_AHEAD BackupAndTurn90Deg finished In ANOMALY_WALL_OFFSET_DIST_AHEAD case just after call to BackupAndTurn90Deg: front dist = 224 In ANOMALY_WALL_OFFSET_DIST_AHEAD case exit with last anomaly code = NONE 15.0: Top of loop() - calling UpdateAllEnvironmentParameters() Battery Voltage = 7.98 Just after first UpdateAllEnvironmentParameters() & 200mSec delay at top of loop() 15.4: gl_Left/RightCenterCm = 34.4/120.0, Left/RightSteerVal = -0.16/-0.02 Just before call to HandleAnomalousConditions() with anomaly code = NONE In HandleAnomalousConditions(LEFT) with last anomaly code = NONE In HandleAnomalousConditions(LEFT) ANOMALY_NONE CASE 15.5: gl_Left/RightCenterCm = 34.2/119.9, Left/RightSteerVal = -0.19/-0.03 gl_LeftCenterCm <= gl_RightCenterCm --> Calling TrackLeftWallOffset() TrackLeftWallOffset(350.0, 0.0, 20.0, 30) called TrackLeftWallOffset: Start tracking offset of 30cm at 15.5 Just after CaptureWallOffset(TRACKING_LEFT, 34.2) Sec LCen RCen Deg LF LR LStr Front Rear FVar RVar LSpd RSpd ACODE TRKDIR 15.6 34.7 120.3 177.4 33.3 34.2 -0.09 243 32 135960 8890 80 69 NONE LEFT 15.7 34.3 120.2 178.8 33.1 34.0 -0.13 252 32 135728 9296 98 51 NONE LEFT 15.8 34.2 120.6 179.6 32.6 33.8 -0.12 248 35 134659 9719 96 54 NONE LEFT 15.9 34.2 119.8 -178.7 32.8 33.6 -0.08 237 39 132556 10139 76 73 NONE LEFT 16.0 34.6 119.5 -177.6 33.7 34.2 -0.05 231 43 129095 10557 64 85 NONE LEFT 16.1 35.0 119.6 -178.4 34.0 34.4 -0.04 224 47 123965 10970 54 95 NONE LEFT 16.2 34.9 119.0 -179.4 33.6 34.4 -0.08 224 52 116801 11371 73 76 NONE LEFT 16.3 34.9 118.9 179.6 34.1 35.0 -0.09 230 58 107229 11747 78 71 NONE LEFT 16.4 35.0 118.7 179.4 34.0 34.6 -0.12 225 63 94962 12108 88 61 NONE LEFT 16.5 35.4 118.6 180.0 33.8 34.9 -0.11 216 68 79632 12451 84 65 NONE LEFT 16.6 35.1 118.3 -179.8 34.1 34.5 -0.04 205 75 60821 12766 54 95 NONE LEFT 16.7 35.1 118.8 179.9 34.1 34.7 -0.06 203 80 38090 13048 61 88 NONE LEFT 16.8 35.1 119.1 179.0 34.2 34.8 -0.06 201 84 11019 13304 61 88 NONE LEFT 16.9 35.0 118.9 178.3 33.8 34.6 -0.08 198 89 10509 13531 68 81 NONE LEFT 17.0 35.2 119.8 177.7 33.8 34.9 -0.11 188 94 9849 13728 83 66 NONE LEFT 17.1 35.4 120.4 177.6 33.8 35.0 -0.12 186 99 9079 13890 89 61 NONE LEFT 17.2 35.1 121.6 178.6 34.2 34.9 -0.10 187 105 8232 14015 81 69 NONE LEFT 17.3 35.3 122.1 178.9 35.6 34.9 0.07 189 114 7310 14095 17 127 NONE LEFT 17.4 36.2 123.1 177.2 36.0 35.5 0.05 183 119 6325 14130 15 127 NONE LEFT 17.5 36.9 123.4 172.7 35.7 36.3 -0.06 155 124 5312 14124 53 96 NONE LEFT 17.6 37.3 124.6 168.8 35.5 37.3 -0.18 112 129 4421 14073 101 49 NONE LEFT 17.7 37.0 128.6 168.4 35.8 37.3 -0.15 129 134 3456 13977 87 62 NONE LEFT 17.8 36.1 134.5 169.9 34.7 36.2 -0.15 100 139 2642 13831 93 56 NONE LEFT 17.9 35.5 134.8 171.9 33.7 35.3 -0.10 98 144 1909 13635 84 65 NONE LEFT 18.0 34.2 135.9 174.1 33.4 34.7 -0.13 131 150 2024 13384 97 52 NONE LEFT 18.1 33.6 137.2 174.9 35.6 33.3 0.23 119 157 2103 13076 0 127 NONE LEFT Stopping Motors! Anomaly Code is 8 --> EXCESS_STEER_VAL |
So it seems this problem is pretty much solved – YAY!!
Stay Tuned,
Frank