nullpoint.py (3991B)
1 #!/usr/bin/env python3 2 3 import matplotlib 4 5 matplotlib.use('gtk4agg') 6 7 import matplotlib.pyplot as plot 8 9 LMOTOR_RAW_VALS = ''' 10 0 14624 11 20 13423 12 40 12514 13 60 11658 14 80 10856 15 100 10229 16 120 9622 17 140 8863 18 160 8208 19 180 7523 20 200 6811 21 220 6131 22 240 5419 23 260 4736 24 280 4034 25 300 3355 26 320 2645 27 340 1956 28 360 1299 29 380 659 30 400 0 31 420 0 32 440 567 33 460 1229 34 480 1770 35 500 2316 36 520 2895 37 540 3442 38 560 4003 39 580 4544 40 600 5087 41 620 5654 42 640 6214 43 660 6741 44 680 7263 45 700 7796 46 720 8348 47 740 8927 48 760 9473 49 780 9999 50 800 10507 51 820 11076 52 840 11565 53 860 11951 54 880 12529 55 900 12916 56 920 13352 57 940 13913 58 960 14360 59 980 14893 60 1000 15104 61 ''' 62 63 RMOTOR_RAW_VALS = ''' 64 0 17342 65 20 15527 66 40 14194 67 60 13538 68 80 12795 69 100 11942 70 120 11107 71 140 10307 72 160 9496 73 180 8701 74 200 7884 75 220 7074 76 240 6208 77 260 5391 78 280 4550 79 300 3701 80 320 2848 81 340 2017 82 360 1193 83 380 355 84 400 0 85 420 514 86 440 1345 87 460 2136 88 480 2857 89 500 3534 90 520 4200 91 540 4856 92 560 5506 93 580 6158 94 600 6807 95 620 7473 96 640 8111 97 660 8759 98 680 9414 99 700 9990 100 720 10520 101 740 11187 102 760 11810 103 780 12355 104 800 12981 105 820 13560 106 840 14175 107 860 14841 108 880 15171 109 900 15868 110 920 16443 111 940 16865 112 960 17287 113 980 17815 114 1000 18014 115 ''' 116 117 def parse_motor_counts(raw_counts): 118 counts = dict() 119 for line in raw_counts.strip().split('\n'): 120 [pwm, count] = line.split() 121 counts[int(pwm)] = float(count) 122 123 return counts 124 125 LMOTOR_COUNTS = parse_motor_counts(LMOTOR_RAW_VALS) 126 RMOTOR_COUNTS = parse_motor_counts(RMOTOR_RAW_VALS) 127 128 MAX = 1000 129 STEP = 20 130 FULLRANGE = list(range(0, MAX + STEP, STEP)) 131 132 def find_nullpoint(counts): 133 ''' 134 (x1, y1) where y1 <= 0 135 (x2, y2) where 0 <= y2 136 137 d / abs(y1) = ((x2 - x1) - d) / abs(y2) 138 abs(y2) * d = (abs(y1) * (x2 - x1)) - (abs(y1) * d) 139 (abs(y1) * d) + (abs(y2) * d) = abs(y1) * (x2 - x1) 140 d = (abs(y2) * (x2 - x1)) / (abs(y1) + abs(y2)) 141 142 x1 + d = nullpoint 143 ''' 144 145 mp = 0 146 for i in FULLRANGE: 147 if counts[i] == 0.0: 148 mp = i 149 break 150 else: 151 raise ValueError(f'Failed to find midpoint, expected count of 0.0 in counts') 152 153 x1 = i - 3 * STEP 154 y1 = counts[x1] 155 156 x2 = i + 3 * STEP 157 y2 = counts[x2] 158 159 d = (abs(y1) * (x2 - x1)) / (abs(y1) + abs(y2)) 160 161 return int(x1 + d) 162 163 LMOTOR_VALS = [LMOTOR_COUNTS[x] for x in FULLRANGE] 164 RMOTOR_VALS = [RMOTOR_COUNTS[x] for x in FULLRANGE] 165 166 print(f'LMOTOR:\n\tcounts: {LMOTOR_VALS}\n\tnullpoint: {find_nullpoint(LMOTOR_COUNTS)}') 167 print(f'RMOTOR:\n\tcounts: {RMOTOR_VALS}\n\tnullpoint: {find_nullpoint(RMOTOR_COUNTS)}') 168 169 def all_before(l, key): 170 def __impl(l, key): 171 for v in l: 172 if v == key: break 173 yield v 174 175 return list(__impl(l, key)) 176 177 def all_after(l, key): 178 def __impl(l, key): 179 out = False 180 for v in l: 181 if v == key: out = True 182 if out: yield v 183 184 return list(__impl(l, key)) 185 186 def negate(l): 187 return [-v for v in l] 188 189 def reverse(l): 190 return list(reversed(l)) 191 192 # figure for calculating lmotor and rmotor null points 193 plot.subplot(311) 194 plot.grid(visible=True) 195 plot.plot(FULLRANGE, negate(all_before(LMOTOR_VALS, 0)) + all_after(LMOTOR_VALS, 0), color='r') 196 plot.plot(FULLRANGE, all_before(RMOTOR_VALS, 0) + negate(all_after(RMOTOR_VALS, 0)), color='b') 197 plot.axhline(y=0, color='black', linestyle='-', linewidth=0.5) 198 199 plot.subplot(312) 200 plot.grid(visible=True) 201 202 lmotor_rev, lmotor_fwd = reverse(all_before(LMOTOR_VALS, 0) + [0]), all_after(LMOTOR_VALS, 0) 203 rmotor_fwd, rmotor_rev = all_after(RMOTOR_VALS, 0), reverse(all_before(RMOTOR_VALS, 0) + [0]) 204 205 plot.plot(FULLRANGE[:len(lmotor_fwd)], lmotor_fwd, color='r') 206 plot.plot(FULLRANGE[:len(lmotor_rev)], lmotor_rev, color='r', linestyle='--') 207 plot.plot(FULLRANGE[:len(rmotor_fwd)], rmotor_fwd, color='b') 208 plot.plot(FULLRANGE[:len(rmotor_rev)], rmotor_rev, color='b', linestyle='--') 209 210 plot.subplot(313) 211 plot.grid(visible=True) 212 213 plot.plot(FULLRANGE, negate(all_before(LMOTOR_VALS, 0)) + all_after(LMOTOR_VALS, 0), color='r') 214 plot.plot(FULLRANGE, negate(all_before(RMOTOR_VALS, 0)) + all_after(RMOTOR_VALS, 0), color='b') 215 216 plot.show() 217