snipt

Ctrl+h for KB shortcuts

C

Display

#include <math.h>

const int xpin = A0;      // accelerometer x-axis analog pin
const int ypin = A1;      // accelerometer y-axis analog pin
const int Ratepin = A2;   // gyro rate pin

double AdcRx;
double AdcRy;
double Rx;
double Ry;
double Rate;
double gyroInitial = 0;
double RxInitial = 0;
double RyInitial = 0;
double ArcSineX;
double ArcSineY;
double GyroRate;
  double FinalAngle = 0;

#define M_PI 3.14159265358979323846

void setup()
{
  Serial.begin(115200);
  RxInitial = analogRead(xpin);
  RyInitial = analogRead(ypin);

  for(int i=0; i<100; i++){
    gyroInitial += analogRead(Ratepin);
  }
  gyroInitial /= 100;
}
void loop()
{
  AdcRx = analogRead(xpin) - RxInitial;
  Rx = (AdcRx * 5.0 / 1023);
  ArcSineX = (asin(Rx) * (180 / M_PI));
  
  AdcRy = analogRead(ypin) - RyInitial;
  Ry = (AdcRy * 5.0 / 1023);
  ArcSineY = (asin(Ry) *(180 / M_PI));
  
  Rate = analogRead(Ratepin) - gyroInitial;
  GyroRate = (Rate * 5.0 / 1023) / 0.025;
  
  FinalAngle = 0.98 * (FinalAngle * GyroRate) + 0.02 * ArcSineY;
  
  
  Serial.print("X: ");
  Serial.print(analogRead(xpin));
  Serial.print("\t\t");
  
  Serial.print("Y: ");
  Serial.print(analogRead(ypin));
  Serial.print("\t\t");
  
  Serial.print("Angle X: ");
  Serial.print(ArcSineX);
  Serial.print("\t\t");
  
  Serial.print("Angle Y: ");
  Serial.print(ArcSineY);
  Serial.print("\t\t"); 
  
  Serial.print("Gyro Rate: ");
  Serial.print(GyroRate);
  Serial.print("\t\t");
  
  Serial.print("Final Angle: ");
  Serial.println(FinalAngle);
  Serial.println();
  delay(100);
}
https://snipt.net/embed/ebf78b512222fe4dcd14e7d5060a15b0/
/raw/ebf78b512222fe4dcd14e7d5060a15b0/
ebf78b512222fe4dcd14e7d5060a15b0
c
C
73
2019-06-17T05:11:05
True
False
False
/api/public/snipt/28737/
display
<table class="highlighttable"><tr><td class="linenos"><div class="linenodiv"><pre><a href="#L-1"> 1</a> <a href="#L-2"> 2</a> <a href="#L-3"> 3</a> <a href="#L-4"> 4</a> <a href="#L-5"> 5</a> <a href="#L-6"> 6</a> <a href="#L-7"> 7</a> <a href="#L-8"> 8</a> <a href="#L-9"> 9</a> <a href="#L-10">10</a> <a href="#L-11">11</a> <a href="#L-12">12</a> <a href="#L-13">13</a> <a href="#L-14">14</a> <a href="#L-15">15</a> <a href="#L-16">16</a> <a href="#L-17">17</a> <a href="#L-18">18</a> <a href="#L-19">19</a> <a href="#L-20">20</a> <a href="#L-21">21</a> <a href="#L-22">22</a> <a href="#L-23">23</a> <a href="#L-24">24</a> <a href="#L-25">25</a> <a href="#L-26">26</a> <a href="#L-27">27</a> <a href="#L-28">28</a> <a href="#L-29">29</a> <a href="#L-30">30</a> <a href="#L-31">31</a> <a href="#L-32">32</a> <a href="#L-33">33</a> <a href="#L-34">34</a> <a href="#L-35">35</a> <a href="#L-36">36</a> <a href="#L-37">37</a> <a href="#L-38">38</a> <a href="#L-39">39</a> <a href="#L-40">40</a> <a href="#L-41">41</a> <a href="#L-42">42</a> <a href="#L-43">43</a> <a href="#L-44">44</a> <a href="#L-45">45</a> <a href="#L-46">46</a> <a href="#L-47">47</a> <a href="#L-48">48</a> <a href="#L-49">49</a> <a href="#L-50">50</a> <a href="#L-51">51</a> <a href="#L-52">52</a> <a href="#L-53">53</a> <a href="#L-54">54</a> <a href="#L-55">55</a> <a href="#L-56">56</a> <a href="#L-57">57</a> <a href="#L-58">58</a> <a href="#L-59">59</a> <a href="#L-60">60</a> <a href="#L-61">61</a> <a href="#L-62">62</a> <a href="#L-63">63</a> <a href="#L-64">64</a> <a href="#L-65">65</a> <a href="#L-66">66</a> <a href="#L-67">67</a> <a href="#L-68">68</a> <a href="#L-69">69</a> <a href="#L-70">70</a> <a href="#L-71">71</a> <a href="#L-72">72</a> <a href="#L-73">73</a></pre></div></td><td class="code"><div class="highlight"><pre><span></span><span id="L-1"><a name="L-1"></a><span class="cp">#include</span> <span class="cpf">&lt;math.h&gt;</span><span class="cp"></span> </span><span id="L-2"><a name="L-2"></a> </span><span id="L-3"><a name="L-3"></a><span class="k">const</span> <span class="kt">int</span> <span class="n">xpin</span> <span class="o">=</span> <span class="n">A0</span><span class="p">;</span> <span class="c1">// accelerometer x-axis analog pin</span> </span><span id="L-4"><a name="L-4"></a><span class="k">const</span> <span class="kt">int</span> <span class="n">ypin</span> <span class="o">=</span> <span class="n">A1</span><span class="p">;</span> <span class="c1">// accelerometer y-axis analog pin</span> </span><span id="L-5"><a name="L-5"></a><span class="k">const</span> <span class="kt">int</span> <span class="n">Ratepin</span> <span class="o">=</span> <span class="n">A2</span><span class="p">;</span> <span class="c1">// gyro rate pin</span> </span><span id="L-6"><a name="L-6"></a> </span><span id="L-7"><a name="L-7"></a><span class="kt">double</span> <span class="n">AdcRx</span><span class="p">;</span> </span><span id="L-8"><a name="L-8"></a><span class="kt">double</span> <span class="n">AdcRy</span><span class="p">;</span> </span><span id="L-9"><a name="L-9"></a><span class="kt">double</span> <span class="n">Rx</span><span class="p">;</span> </span><span id="L-10"><a name="L-10"></a><span class="kt">double</span> <span class="n">Ry</span><span class="p">;</span> </span><span id="L-11"><a name="L-11"></a><span class="kt">double</span> <span class="n">Rate</span><span class="p">;</span> </span><span id="L-12"><a name="L-12"></a><span class="kt">double</span> <span class="n">gyroInitial</span> <span class="o">=</span> <span class="mi">0</span><span class="p">;</span> </span><span id="L-13"><a name="L-13"></a><span class="kt">double</span> <span class="n">RxInitial</span> <span class="o">=</span> <span class="mi">0</span><span class="p">;</span> </span><span id="L-14"><a name="L-14"></a><span class="kt">double</span> <span class="n">RyInitial</span> <span class="o">=</span> <span class="mi">0</span><span class="p">;</span> </span><span id="L-15"><a name="L-15"></a><span class="kt">double</span> <span class="n">ArcSineX</span><span class="p">;</span> </span><span id="L-16"><a name="L-16"></a><span class="kt">double</span> <span class="n">ArcSineY</span><span class="p">;</span> </span><span id="L-17"><a name="L-17"></a><span class="kt">double</span> <span class="n">GyroRate</span><span class="p">;</span> </span><span id="L-18"><a name="L-18"></a> <span class="kt">double</span> <span class="n">FinalAngle</span> <span class="o">=</span> <span class="mi">0</span><span class="p">;</span> </span><span id="L-19"><a name="L-19"></a> </span><span id="L-20"><a name="L-20"></a><span class="cp">#define M_PI 3.14159265358979323846</span> </span><span id="L-21"><a name="L-21"></a> </span><span id="L-22"><a name="L-22"></a><span class="kt">void</span> <span class="nf">setup</span><span class="p">()</span> </span><span id="L-23"><a name="L-23"></a><span class="p">{</span> </span><span id="L-24"><a name="L-24"></a> <span class="n">Serial</span><span class="p">.</span><span class="n">begin</span><span class="p">(</span><span class="mi">115200</span><span class="p">);</span> </span><span id="L-25"><a name="L-25"></a> <span class="n">RxInitial</span> <span class="o">=</span> <span class="n">analogRead</span><span class="p">(</span><span class="n">xpin</span><span class="p">);</span> </span><span id="L-26"><a name="L-26"></a> <span class="n">RyInitial</span> <span class="o">=</span> <span class="n">analogRead</span><span class="p">(</span><span class="n">ypin</span><span class="p">);</span> </span><span id="L-27"><a name="L-27"></a> </span><span id="L-28"><a name="L-28"></a> <span class="k">for</span><span class="p">(</span><span class="kt">int</span> <span class="n">i</span><span class="o">=</span><span class="mi">0</span><span class="p">;</span> <span class="n">i</span><span class="o">&lt;</span><span class="mi">100</span><span class="p">;</span> <span class="n">i</span><span class="o">++</span><span class="p">){</span> </span><span id="L-29"><a name="L-29"></a> <span class="n">gyroInitial</span> <span class="o">+=</span> <span class="n">analogRead</span><span class="p">(</span><span class="n">Ratepin</span><span class="p">);</span> </span><span id="L-30"><a name="L-30"></a> <span class="p">}</span> </span><span id="L-31"><a name="L-31"></a> <span class="n">gyroInitial</span> <span class="o">/=</span> <span class="mi">100</span><span class="p">;</span> </span><span id="L-32"><a name="L-32"></a><span class="p">}</span> </span><span id="L-33"><a name="L-33"></a><span class="kt">void</span> <span class="nf">loop</span><span class="p">()</span> </span><span id="L-34"><a name="L-34"></a><span class="p">{</span> </span><span id="L-35"><a name="L-35"></a> <span class="n">AdcRx</span> <span class="o">=</span> <span class="n">analogRead</span><span class="p">(</span><span class="n">xpin</span><span class="p">)</span> <span class="o">-</span> <span class="n">RxInitial</span><span class="p">;</span> </span><span id="L-36"><a name="L-36"></a> <span class="n">Rx</span> <span class="o">=</span> <span class="p">(</span><span class="n">AdcRx</span> <span class="o">*</span> <span class="mf">5.0</span> <span class="o">/</span> <span class="mi">1023</span><span class="p">);</span> </span><span id="L-37"><a name="L-37"></a> <span class="n">ArcSineX</span> <span class="o">=</span> <span class="p">(</span><span class="n">asin</span><span class="p">(</span><span class="n">Rx</span><span class="p">)</span> <span class="o">*</span> <span class="p">(</span><span class="mi">180</span> <span class="o">/</span> <span class="n">M_PI</span><span class="p">));</span> </span><span id="L-38"><a name="L-38"></a> </span><span id="L-39"><a name="L-39"></a> <span class="n">AdcRy</span> <span class="o">=</span> <span class="n">analogRead</span><span class="p">(</span><span class="n">ypin</span><span class="p">)</span> <span class="o">-</span> <span class="n">RyInitial</span><span class="p">;</span> </span><span id="L-40"><a name="L-40"></a> <span class="n">Ry</span> <span class="o">=</span> <span class="p">(</span><span class="n">AdcRy</span> <span class="o">*</span> <span class="mf">5.0</span> <span class="o">/</span> <span class="mi">1023</span><span class="p">);</span> </span><span id="L-41"><a name="L-41"></a> <span class="n">ArcSineY</span> <span class="o">=</span> <span class="p">(</span><span class="n">asin</span><span class="p">(</span><span class="n">Ry</span><span class="p">)</span> <span class="o">*</span><span class="p">(</span><span class="mi">180</span> <span class="o">/</span> <span class="n">M_PI</span><span class="p">));</span> </span><span id="L-42"><a name="L-42"></a> </span><span id="L-43"><a name="L-43"></a> <span class="n">Rate</span> <span class="o">=</span> <span class="n">analogRead</span><span class="p">(</span><span class="n">Ratepin</span><span class="p">)</span> <span class="o">-</span> <span class="n">gyroInitial</span><span class="p">;</span> </span><span id="L-44"><a name="L-44"></a> <span class="n">GyroRate</span> <span class="o">=</span> <span class="p">(</span><span class="n">Rate</span> <span class="o">*</span> <span class="mf">5.0</span> <span class="o">/</span> <span class="mi">1023</span><span class="p">)</span> <span class="o">/</span> <span class="mf">0.025</span><span class="p">;</span> </span><span id="L-45"><a name="L-45"></a> </span><span id="L-46"><a name="L-46"></a> <span class="n">FinalAngle</span> <span class="o">=</span> <span class="mf">0.98</span> <span class="o">*</span> <span class="p">(</span><span class="n">FinalAngle</span> <span class="o">*</span> <span class="n">GyroRate</span><span class="p">)</span> <span class="o">+</span> <span class="mf">0.02</span> <span class="o">*</span> <span class="n">ArcSineY</span><span class="p">;</span> </span><span id="L-47"><a name="L-47"></a> </span><span id="L-48"><a name="L-48"></a> </span><span id="L-49"><a name="L-49"></a> <span class="n">Serial</span><span class="p">.</span><span class="n">print</span><span class="p">(</span><span class="s">&quot;X: &quot;</span><span class="p">);</span> </span><span id="L-50"><a name="L-50"></a> <span class="n">Serial</span><span class="p">.</span><span class="n">print</span><span class="p">(</span><span class="n">analogRead</span><span class="p">(</span><span class="n">xpin</span><span class="p">));</span> </span><span id="L-51"><a name="L-51"></a> <span class="n">Serial</span><span class="p">.</span><span class="n">print</span><span class="p">(</span><span class="s">&quot;</span><span class="se">\t\t</span><span class="s">&quot;</span><span class="p">);</span> </span><span id="L-52"><a name="L-52"></a> </span><span id="L-53"><a name="L-53"></a> <span class="n">Serial</span><span class="p">.</span><span class="n">print</span><span class="p">(</span><span class="s">&quot;Y: &quot;</span><span class="p">);</span> </span><span id="L-54"><a name="L-54"></a> <span class="n">Serial</span><span class="p">.</span><span class="n">print</span><span class="p">(</span><span class="n">analogRead</span><span class="p">(</span><span class="n">ypin</span><span class="p">));</span> </span><span id="L-55"><a name="L-55"></a> <span class="n">Serial</span><span class="p">.</span><span class="n">print</span><span class="p">(</span><span class="s">&quot;</span><span class="se">\t\t</span><span class="s">&quot;</span><span class="p">);</span> </span><span id="L-56"><a name="L-56"></a> </span><span id="L-57"><a name="L-57"></a> <span class="n">Serial</span><span class="p">.</span><span class="n">print</span><span class="p">(</span><span class="s">&quot;Angle X: &quot;</span><span class="p">);</span> </span><span id="L-58"><a name="L-58"></a> <span class="n">Serial</span><span class="p">.</span><span class="n">print</span><span class="p">(</span><span class="n">ArcSineX</span><span class="p">);</span> </span><span id="L-59"><a name="L-59"></a> <span class="n">Serial</span><span class="p">.</span><span class="n">print</span><span class="p">(</span><span class="s">&quot;</span><span class="se">\t\t</span><span class="s">&quot;</span><span class="p">);</span> </span><span id="L-60"><a name="L-60"></a> </span><span id="L-61"><a name="L-61"></a> <span class="n">Serial</span><span class="p">.</span><span class="n">print</span><span class="p">(</span><span class="s">&quot;Angle Y: &quot;</span><span class="p">);</span> </span><span id="L-62"><a name="L-62"></a> <span class="n">Serial</span><span class="p">.</span><span class="n">print</span><span class="p">(</span><span class="n">ArcSineY</span><span class="p">);</span> </span><span id="L-63"><a name="L-63"></a> <span class="n">Serial</span><span class="p">.</span><span class="n">print</span><span class="p">(</span><span class="s">&quot;</span><span class="se">\t\t</span><span class="s">&quot;</span><span class="p">);</span> </span><span id="L-64"><a name="L-64"></a> </span><span id="L-65"><a name="L-65"></a> <span class="n">Serial</span><span class="p">.</span><span class="n">print</span><span class="p">(</span><span class="s">&quot;Gyro Rate: &quot;</span><span class="p">);</span> </span><span id="L-66"><a name="L-66"></a> <span class="n">Serial</span><span class="p">.</span><span class="n">print</span><span class="p">(</span><span class="n">GyroRate</span><span class="p">);</span> </span><span id="L-67"><a name="L-67"></a> <span class="n">Serial</span><span class="p">.</span><span class="n">print</span><span class="p">(</span><span class="s">&quot;</span><span class="se">\t\t</span><span class="s">&quot;</span><span class="p">);</span> </span><span id="L-68"><a name="L-68"></a> </span><span id="L-69"><a name="L-69"></a> <span class="n">Serial</span><span class="p">.</span><span class="n">print</span><span class="p">(</span><span class="s">&quot;Final Angle: &quot;</span><span class="p">);</span> </span><span id="L-70"><a name="L-70"></a> <span class="n">Serial</span><span class="p">.</span><span class="n">println</span><span class="p">(</span><span class="n">FinalAngle</span><span class="p">);</span> </span><span id="L-71"><a name="L-71"></a> <span class="n">Serial</span><span class="p">.</span><span class="n">println</span><span class="p">();</span> </span><span id="L-72"><a name="L-72"></a> <span class="n">delay</span><span class="p">(</span><span class="mi">100</span><span class="p">);</span> </span><span id="L-73"><a name="L-73"></a><span class="p">}</span> </span></pre></div> </td></tr></table>
display