Update documentation
This commit is contained in:
Konstantin Lampalzer
parent
739b86c1f2
commit
43ec20e38a
3 changed files with 116 additions and 132 deletions
|
|
@ -166,85 +166,77 @@
|
|||
<span id="lib-linefollower"></span><h1>Linefollower Examples<a class="headerlink" href="#linefollower-examples" title="Permalink to this headline">¶</a></h1>
|
||||
<div class="section" id="simple-linefollower">
|
||||
<h2>Simple Linefollower<a class="headerlink" href="#simple-linefollower" title="Permalink to this headline">¶</a></h2>
|
||||
<div class="highlight-python notranslate"><div class="highlight"><pre><span></span><span class="kn">import</span> <span class="nn">time</span>
|
||||
<div class="highlight-python notranslate"><div class="highlight"><pre><span></span><span class="kn">from</span> <span class="nn">compLib.Motor</span> <span class="kn">import</span> <span class="n">Motor</span>
|
||||
<span class="kn">from</span> <span class="nn">compLib.Display</span> <span class="kn">import</span> <span class="n">Display</span>
|
||||
<span class="kn">from</span> <span class="nn">compLib.IRSensor</span> <span class="kn">import</span> <span class="n">IRSensor</span>
|
||||
<span class="kn">from</span> <span class="nn">compLib.Encoder</span> <span class="kn">import</span> <span class="n">Encoder</span>
|
||||
|
||||
<span class="kn">from</span> <span class="nn">compLib.IRWrapper</span> <span class="kn">import</span> <span class="n">IRWrapper</span>
|
||||
<span class="kn">from</span> <span class="nn">compLib.Motor</span> <span class="kn">import</span> <span class="n">Motor</span>
|
||||
<span class="kn">import</span> <span class="nn">time</span>
|
||||
|
||||
<span class="n">STRAIGHT_SPEED</span> <span class="o">=</span> <span class="mf">40.0</span>
|
||||
<span class="n">TURN_SPEED</span> <span class="o">=</span> <span class="mf">40.0</span>
|
||||
<span class="n">IRSensor</span><span class="o">.</span><span class="n">set</span><span class="p">(</span><span class="mi">1</span><span class="p">,</span> <span class="kc">True</span><span class="p">)</span>
|
||||
<span class="n">IRSensor</span><span class="o">.</span><span class="n">set</span><span class="p">(</span><span class="mi">2</span><span class="p">,</span> <span class="kc">True</span><span class="p">)</span>
|
||||
<span class="n">IRSensor</span><span class="o">.</span><span class="n">set</span><span class="p">(</span><span class="mi">3</span><span class="p">,</span> <span class="kc">True</span><span class="p">)</span>
|
||||
<span class="n">IRSensor</span><span class="o">.</span><span class="n">set</span><span class="p">(</span><span class="mi">4</span><span class="p">,</span> <span class="kc">True</span><span class="p">)</span>
|
||||
<span class="n">IRSensor</span><span class="o">.</span><span class="n">set</span><span class="p">(</span><span class="mi">5</span><span class="p">,</span> <span class="kc">True</span><span class="p">)</span>
|
||||
|
||||
<span class="n">COLOR_BREAK</span> <span class="o">=</span> <span class="mi">500</span>
|
||||
<span class="n">DRIVE_SPEED</span> <span class="o">=</span> <span class="mi">85</span>
|
||||
<span class="n">TURN_SPEED</span> <span class="o">=</span> <span class="mi">50</span>
|
||||
<span class="n">COLOR_BREAK</span> <span class="o">=</span> <span class="mi">750</span>
|
||||
<span class="n">KP</span> <span class="o">=</span> <span class="mf">50.0</span>
|
||||
<span class="n">KD</span> <span class="o">=</span> <span class="mf">0.0</span>
|
||||
|
||||
<span class="k">def</span> <span class="nf">drive</span><span class="p">(</span><span class="n">leftSpeed</span><span class="p">,</span> <span class="n">rightSpeed</span><span class="p">):</span>
|
||||
<span class="n">leftSpeed</span> <span class="o">*=</span> <span class="o">-</span><span class="mi">1</span>
|
||||
<span class="n">rightSpeed</span> <span class="o">*=</span> <span class="mf">0.906</span>
|
||||
|
||||
<span class="k">def</span> <span class="nf">drive</span><span class="p">(</span><span class="n">left</span><span class="p">,</span> <span class="n">right</span><span class="p">,</span> <span class="n">active_break</span><span class="o">=</span><span class="kc">False</span><span class="p">,</span> <span class="n">back_mult</span><span class="o">=</span><span class="mf">0.0</span><span class="p">):</span>
|
||||
<span class="n">left</span> <span class="o">=</span> <span class="nb">max</span><span class="p">(</span><span class="nb">min</span><span class="p">(</span><span class="n">left</span><span class="p">,</span> <span class="mi">100</span><span class="p">),</span> <span class="o">-</span><span class="mi">100</span><span class="p">)</span>
|
||||
<span class="n">right</span> <span class="o">=</span> <span class="nb">max</span><span class="p">(</span><span class="nb">min</span><span class="p">(</span><span class="n">right</span><span class="p">,</span> <span class="mi">100</span><span class="p">),</span> <span class="o">-</span><span class="mi">100</span><span class="p">)</span>
|
||||
<span class="n">Motor</span><span class="o">.</span><span class="n">power</span><span class="p">(</span><span class="mi">1</span><span class="p">,</span> <span class="nb">min</span><span class="p">(</span><span class="nb">max</span><span class="p">(</span><span class="o">-</span><span class="mi">100</span><span class="p">,</span> <span class="n">leftSpeed</span><span class="p">),</span> <span class="mi">100</span><span class="p">))</span>
|
||||
<span class="n">Motor</span><span class="o">.</span><span class="n">power</span><span class="p">(</span><span class="mi">2</span><span class="p">,</span> <span class="nb">min</span><span class="p">(</span><span class="nb">max</span><span class="p">(</span><span class="o">-</span><span class="mi">100</span><span class="p">,</span> <span class="n">rightSpeed</span><span class="p">),</span> <span class="mi">100</span><span class="p">))</span>
|
||||
|
||||
<span class="nb">print</span><span class="p">(</span><span class="s2">"left </span><span class="si">{}</span><span class="s2"> right </span><span class="si">{}</span><span class="s2">"</span><span class="o">.</span><span class="n">format</span><span class="p">(</span><span class="n">left</span><span class="p">,</span> <span class="n">right</span><span class="p">))</span>
|
||||
<span class="k">def</span> <span class="nf">follow</span><span class="p">(</span><span class="n">sleepTime</span> <span class="o">=</span> <span class="mf">0.1</span><span class="p">):</span>
|
||||
<span class="n">lastError</span> <span class="o">=</span> <span class="mi">0</span>
|
||||
<span class="n">sensorsBlack</span> <span class="o">=</span> <span class="mi">0</span>
|
||||
<span class="k">while</span> <span class="n">sensorsBlack</span> <span class="o"><</span> <span class="mi">3</span><span class="p">:</span>
|
||||
<span class="n">sensorsBlack</span> <span class="o">=</span> <span class="mi">0</span>
|
||||
<span class="k">for</span> <span class="n">i</span> <span class="ow">in</span> <span class="nb">range</span><span class="p">(</span><span class="mi">1</span><span class="p">,</span> <span class="mi">6</span><span class="p">):</span>
|
||||
<span class="k">if</span> <span class="n">IRSensor</span><span class="o">.</span><span class="n">read</span><span class="p">(</span><span class="n">i</span><span class="p">)</span> <span class="o">></span> <span class="n">COLOR_BREAK</span><span class="p">:</span>
|
||||
<span class="n">sensorsBlack</span> <span class="o">+=</span> <span class="mi">1</span>
|
||||
|
||||
<span class="n">Motor</span><span class="o">.</span><span class="n">power</span><span class="p">(</span><span class="mi">0</span><span class="p">,</span> <span class="n">left</span><span class="p">)</span>
|
||||
<span class="n">Motor</span><span class="o">.</span><span class="n">power</span><span class="p">(</span><span class="mi">1</span><span class="p">,</span> <span class="n">left</span> <span class="o">*</span> <span class="n">back_mult</span><span class="p">)</span>
|
||||
<span class="n">Motor</span><span class="o">.</span><span class="n">power</span><span class="p">(</span><span class="mi">2</span><span class="p">,</span> <span class="n">right</span> <span class="o">*</span> <span class="n">back_mult</span><span class="p">)</span>
|
||||
<span class="n">Motor</span><span class="o">.</span><span class="n">power</span><span class="p">(</span><span class="mi">3</span><span class="p">,</span> <span class="n">right</span><span class="p">)</span>
|
||||
<span class="n">error</span> <span class="o">=</span> <span class="n">lastError</span>
|
||||
<span class="k">if</span> <span class="n">IRSensor</span><span class="o">.</span><span class="n">read</span><span class="p">(</span><span class="mi">3</span><span class="p">)</span> <span class="o">></span> <span class="n">COLOR_BREAK</span><span class="p">:</span>
|
||||
<span class="n">error</span> <span class="o">=</span> <span class="mi">0</span>
|
||||
<span class="k">elif</span> <span class="n">IRSensor</span><span class="o">.</span><span class="n">read</span><span class="p">(</span><span class="mi">1</span><span class="p">)</span> <span class="o">></span> <span class="n">COLOR_BREAK</span><span class="p">:</span>
|
||||
<span class="n">error</span> <span class="o">=</span> <span class="o">-</span><span class="mf">1.5</span>
|
||||
<span class="k">elif</span> <span class="n">IRSensor</span><span class="o">.</span><span class="n">read</span><span class="p">(</span><span class="mi">5</span><span class="p">)</span> <span class="o">></span> <span class="n">COLOR_BREAK</span><span class="p">:</span>
|
||||
<span class="n">error</span> <span class="o">=</span> <span class="mf">1.5</span>
|
||||
<span class="k">elif</span> <span class="n">IRSensor</span><span class="o">.</span><span class="n">read</span><span class="p">(</span><span class="mi">2</span><span class="p">)</span> <span class="o">></span> <span class="n">COLOR_BREAK</span><span class="p">:</span>
|
||||
<span class="n">error</span> <span class="o">=</span> <span class="o">-</span><span class="mi">1</span>
|
||||
<span class="k">elif</span> <span class="n">IRSensor</span><span class="o">.</span><span class="n">read</span><span class="p">(</span><span class="mi">4</span><span class="p">)</span> <span class="o">></span> <span class="n">COLOR_BREAK</span><span class="p">:</span>
|
||||
<span class="n">error</span> <span class="o">=</span> <span class="mi">1</span>
|
||||
<span class="k">elif</span> <span class="n">error</span> <span class="o">==</span> <span class="mf">1.5</span><span class="p">:</span>
|
||||
<span class="n">error</span> <span class="o">=</span> <span class="mi">3</span>
|
||||
<span class="k">elif</span> <span class="n">error</span> <span class="o">==</span> <span class="o">-</span><span class="mf">1.5</span><span class="p">:</span>
|
||||
<span class="n">error</span> <span class="o">=</span> <span class="o">-</span><span class="mi">3</span>
|
||||
|
||||
<span class="k">if</span> <span class="n">active_break</span><span class="p">:</span>
|
||||
<span class="n">time</span><span class="o">.</span><span class="n">sleep</span><span class="p">(</span><span class="mf">0.001</span><span class="p">)</span>
|
||||
<span class="n">lastError</span> <span class="o">=</span> <span class="n">error</span>
|
||||
|
||||
<span class="n">Motor</span><span class="o">.</span><span class="n">power</span><span class="p">(</span><span class="mi">0</span><span class="p">,</span> <span class="mi">10</span><span class="p">)</span>
|
||||
<span class="n">Motor</span><span class="o">.</span><span class="n">power</span><span class="p">(</span><span class="mi">1</span><span class="p">,</span> <span class="mi">10</span><span class="p">)</span>
|
||||
<span class="n">Motor</span><span class="o">.</span><span class="n">power</span><span class="p">(</span><span class="mi">2</span><span class="p">,</span> <span class="mi">10</span><span class="p">)</span>
|
||||
<span class="n">Motor</span><span class="o">.</span><span class="n">power</span><span class="p">(</span><span class="mi">3</span><span class="p">,</span> <span class="mi">10</span><span class="p">)</span>
|
||||
<span class="n">adjustment</span> <span class="o">=</span> <span class="n">KP</span> <span class="o">*</span> <span class="n">error</span> <span class="o">+</span> <span class="n">KD</span> <span class="o">*</span> <span class="p">(</span><span class="n">error</span> <span class="o">-</span> <span class="n">lastError</span><span class="p">)</span>
|
||||
<span class="n">leftSpeed</span> <span class="o">=</span> <span class="n">DRIVE_SPEED</span> <span class="o">+</span> <span class="n">adjustment</span>
|
||||
<span class="n">rightSpeed</span> <span class="o">=</span> <span class="n">DRIVE_SPEED</span> <span class="o">-</span> <span class="n">adjustment</span>
|
||||
|
||||
<span class="n">time</span><span class="o">.</span><span class="n">sleep</span><span class="p">(</span><span class="mf">0.001</span><span class="p">)</span>
|
||||
|
||||
|
||||
<span class="k">def</span> <span class="nf">follow_till_line</span><span class="p">():</span>
|
||||
<span class="k">while</span> <span class="kc">True</span><span class="p">:</span>
|
||||
<span class="nb">print</span><span class="p">(</span><span class="s2">"left: </span><span class="si">{:.2f}</span><span class="s2"> middle: </span><span class="si">{:.2f}</span><span class="s2"> right: </span><span class="si">{:.2f}</span><span class="s2">"</span><span class="o">.</span><span class="n">format</span><span class="p">(</span><span class="n">IRWrapper</span><span class="o">.</span><span class="n">bottom_left_calibrated</span><span class="p">(),</span>
|
||||
<span class="n">IRWrapper</span><span class="o">.</span><span class="n">bottom_middle_calibrated</span><span class="p">(),</span>
|
||||
<span class="n">IRWrapper</span><span class="o">.</span><span class="n">bottom_right_calibrated</span><span class="p">()))</span>
|
||||
|
||||
<span class="n">left</span> <span class="o">=</span> <span class="n">IRWrapper</span><span class="o">.</span><span class="n">bottom_left_calibrated</span><span class="p">()</span>
|
||||
<span class="n">right</span> <span class="o">=</span> <span class="n">IRWrapper</span><span class="o">.</span><span class="n">bottom_right_calibrated</span><span class="p">()</span>
|
||||
|
||||
<span class="k">if</span> <span class="n">left</span> <span class="o">></span> <span class="n">COLOR_BREAK</span><span class="p">:</span>
|
||||
<span class="n">drive</span><span class="p">(</span><span class="o">-</span><span class="n">TURN_SPEED</span><span class="p">,</span> <span class="n">TURN_SPEED</span><span class="p">,</span> <span class="n">back_mult</span><span class="o">=</span><span class="mf">1.25</span><span class="p">)</span>
|
||||
<span class="k">while</span> <span class="n">IRWrapper</span><span class="o">.</span><span class="n">bottom_middle_calibrated</span><span class="p">()</span> <span class="o"><</span> <span class="n">COLOR_BREAK</span> <span class="ow">and</span> \
|
||||
<span class="n">IRWrapper</span><span class="o">.</span><span class="n">bottom_right_calibrated</span><span class="p">()</span> <span class="o"><</span> <span class="n">COLOR_BREAK</span><span class="p">:</span>
|
||||
<span class="k">pass</span>
|
||||
<span class="n">drive</span><span class="p">(</span><span class="mi">0</span><span class="p">,</span> <span class="mi">0</span><span class="p">)</span>
|
||||
<span class="k">elif</span> <span class="n">right</span> <span class="o">></span> <span class="n">COLOR_BREAK</span><span class="p">:</span>
|
||||
<span class="n">drive</span><span class="p">(</span><span class="n">TURN_SPEED</span><span class="p">,</span> <span class="o">-</span><span class="n">TURN_SPEED</span><span class="p">,</span> <span class="n">back_mult</span><span class="o">=</span><span class="mf">1.25</span><span class="p">)</span>
|
||||
<span class="k">while</span> <span class="n">IRWrapper</span><span class="o">.</span><span class="n">bottom_middle_calibrated</span><span class="p">()</span> <span class="o"><</span> <span class="n">COLOR_BREAK</span> <span class="ow">and</span> \
|
||||
<span class="n">IRWrapper</span><span class="o">.</span><span class="n">bottom_left_calibrated</span><span class="p">()</span> <span class="o"><</span> <span class="n">COLOR_BREAK</span><span class="p">:</span>
|
||||
<span class="k">pass</span>
|
||||
<span class="n">drive</span><span class="p">(</span><span class="mi">0</span><span class="p">,</span> <span class="mi">0</span><span class="p">)</span>
|
||||
<span class="k">else</span><span class="p">:</span>
|
||||
<span class="n">drive</span><span class="p">(</span><span class="n">STRAIGHT_SPEED</span><span class="p">,</span> <span class="n">STRAIGHT_SPEED</span><span class="p">,</span> <span class="kc">True</span><span class="p">)</span>
|
||||
|
||||
<span class="k">if</span> <span class="n">IRWrapper</span><span class="o">.</span><span class="n">bottom_left_calibrated</span><span class="p">()</span> <span class="o">></span> <span class="mi">750</span> <span class="ow">and</span> <span class="n">IRWrapper</span><span class="o">.</span><span class="n">bottom_right_calibrated</span><span class="p">()</span> <span class="o">></span> <span class="mi">750</span><span class="p">:</span>
|
||||
<span class="k">break</span>
|
||||
|
||||
<span class="nb">print</span><span class="p">(</span><span class="s2">"left: </span><span class="si">{}</span><span class="s2"> middle: </span><span class="si">{}</span><span class="s2"> right: </span><span class="si">{}</span><span class="s2">"</span><span class="o">.</span><span class="n">format</span><span class="p">(</span><span class="n">IRWrapper</span><span class="o">.</span><span class="n">bottom_left_calibrated</span><span class="p">(),</span>
|
||||
<span class="n">IRWrapper</span><span class="o">.</span><span class="n">bottom_middle_calibrated</span><span class="p">(),</span>
|
||||
<span class="n">IRWrapper</span><span class="o">.</span><span class="n">bottom_right_calibrated</span><span class="p">()))</span>
|
||||
<span class="nb">print</span><span class="p">(</span><span class="sa">f</span><span class="s2">"</span><span class="si">{</span><span class="n">leftSpeed</span><span class="si">}</span><span class="s2"> </span><span class="si">{</span><span class="n">rightSpeed</span><span class="si">}</span><span class="s2"> </span><span class="si">{</span><span class="n">adjustment</span><span class="si">}</span><span class="s2"> </span><span class="si">{</span><span class="n">error</span><span class="si">}</span><span class="s2">"</span><span class="p">)</span>
|
||||
<span class="n">drive</span><span class="p">(</span><span class="n">leftSpeed</span><span class="p">,</span> <span class="n">rightSpeed</span><span class="p">)</span>
|
||||
|
||||
<span class="n">drive</span><span class="p">(</span><span class="mi">0</span><span class="p">,</span> <span class="mi">0</span><span class="p">)</span>
|
||||
<span class="n">time</span><span class="o">.</span><span class="n">sleep</span><span class="p">(</span><span class="n">sleepTime</span><span class="p">)</span>
|
||||
|
||||
<span class="n">time</span><span class="o">.</span><span class="n">sleep</span><span class="p">(</span><span class="mf">0.2</span><span class="p">)</span>
|
||||
<span class="k">def</span> <span class="nf">main</span><span class="p">():</span>
|
||||
<span class="n">follow</span><span class="p">()</span>
|
||||
<span class="n">follow</span><span class="p">()</span>
|
||||
<span class="n">follow</span><span class="p">()</span>
|
||||
<span class="n">follow</span><span class="p">()</span>
|
||||
<span class="n">follow</span><span class="p">(</span><span class="mf">0.2</span><span class="p">)</span>
|
||||
|
||||
|
||||
<span class="k">if</span> <span class="vm">__name__</span> <span class="o">==</span> <span class="s2">"__main__"</span><span class="p">:</span>
|
||||
<span class="c1"># Put bot with middle sensor onto black line</span>
|
||||
<span class="n">IRWrapper</span><span class="o">.</span><span class="n">calibrate</span><span class="p">()</span>
|
||||
|
||||
<span class="c1"># Initialize Motors, so the bot doesn't speed off instantly</span>
|
||||
<span class="n">drive</span><span class="p">(</span><span class="mi">1</span><span class="p">,</span> <span class="mi">1</span><span class="p">)</span>
|
||||
<span class="n">time</span><span class="o">.</span><span class="n">sleep</span><span class="p">(</span><span class="mi">1</span><span class="p">)</span>
|
||||
|
||||
<span class="c1"># Follow line, till the left and right sensor are on black</span>
|
||||
<span class="n">follow_till_line</span><span class="p">()</span>
|
||||
<span class="n">main</span><span class="p">()</span>
|
||||
</pre></div>
|
||||
</div>
|
||||
</div>
|
||||
|
|
|
|||
Reference in a new issue