snipt

Ctrl+h for KB shortcuts

C++

newrobo

#include <Servo.h>
#include <PPM.h>

//Set up a connection to the vex controller
//=========================================
PPM ppm(2);
//=========================================



//Set up the servos and motors to interface with the arduino
//==========================================================
Servo LEFT_MOTOR_ONE;
Servo LEFT_MOTOR_TWO;
//=====================
Servo RIGHT_MOTOR_ONE;
Servo RIGHT_MOTOR_TWO;
//=====================
Servo LIFT_MOTOR_ONE;
Servo LIFT_MOTOR_TWO;
//=====================
Servo SWEEPER_SERVO;
//==========================================================



//set up the global integer values used between funtions
//======================================================
int sweeper_position = 0;
int requested_sweeper_position = 0;
//=======================
int LEFT_INPUT;
int RIGHT_INPUT;
//======================================================



//set up the global booleans used between functions
//=================================================
boolean LIFT_INPUT;
boolean arm_toggled = false;
//=================================================


// Wait until controller is turned on.  Do not edit!!!
void waitForController () {
  while (ppm.getChannel(1) == 0) { } // do nothing until controller is switched on
}




// initialize all of the motors and servos with thir PWM ports on the arduino
void setup(){
  
  Serial.begin(9600);
  
  LEFT_MOTOR_ONE.attach(8);
  LEFT_MOTOR_TWO.attach(9);

  RIGHT_MOTOR_ONE.attach(6);
  RIGHT_MOTOR_TWO.attach(7);

  LIFT_MOTOR_ONE.attach(10);
  LIFT_MOTOR_TWO.attach(11);

  SWEEPER_SERVO.attach(12);
}


//the loop function does the following:
//  runs the autonymous code for 20s
//  runs the teleoperated code for 150s (2.5 minutes)
//  exits the program
void loop(){
  arm_in();
  waitForController();
  autonomous(20);
  teleop(150);
  finish();
}

void finish () {
  Serial.println("Done");
  delay(50);     // With no delay the program could exit before the serial has completed printing
  exit(0);       // Exit
}



// the poll function returns no values, but servs to store all of the inputs or their derrived values
// as global variables for use in other functions
void poll(){
  //the joysticks
  //used for driving
  LEFT_INPUT = ppm.getChannel(3);
  RIGHT_INPUT = ppm.getChannel(2);

  //the channel 6 buttons (right hand operated)
  //used for turning the lifting motors on and off
  if (ppm.getChannel(6) > 150) //down button
    LIFT_INPUT = false;
  if (ppm.getChannel(6) < 30) //up button
    LIFT_INPUT = true;

  //the channel 5 buttons (left hand operated)
  //used for opening and closing the arm
  if (ppm.getChannel(5) > 150) //down button
    requested_sweeper_position = 0;
  else if (ppm.getChannel(5) < 30) //up button
    requested_sweeper_position =120;
  else //no button
    arm_toggled = true;

}


//runs the autonomous section of the code for the set time
void autonomous(unsigned long time) {
  unsigned long startTime=millis();
  time=time * 1000;
  while (millis()-startTime<time) {
    operation_deqube();  
    operation_pick_me_up();
    
    while (millis()-startTime<time) {Serial.println("autonomous has time left");}
    Serial.println("Autonomous Over!");
    delay(200);
  }
}


//pulls the green cubes towards the conveyor belts
void operation_deqube(){
  arm_in();
  drive_backwards(1000);
  delay(1000);
  arm_out();
  drive_forwards(1000);
  move_arm(0,180,1000);
}


//drives back and forth with the treads running attempting to pick up cubes
void operation_pick_me_up(){
  //turn on the lift
  LIFT_MOTOR_TWO.write(0);
  LIFT_MOTOR_ONE.write(180);
  
  //turn a little on 
  drive_forwards(600);
  drive_backwards(600);
  
  delay(100);
  stop_driving();
  
  drive_forwards(1500);
  drive_backwards_turn(1000);
  
  drive_forwards(2000);
  drive_backwards_turn(1000);
  
  drive_forwards(2000);
  drive_backwards_turn(1000);
  
  drive_forwards(2000);
  drive_backwards_turn(1000);
  
  drive_forwards(2000);
  drive_backwards_turn(1000);
  
  
}

//drives back at full speed
//for use in auto
void drive_backwards(int time){
  Serial.println("driving backwards");
  LEFT_MOTOR_ONE.write(0);
  LEFT_MOTOR_TWO.write(0);
  RIGHT_MOTOR_ONE.write(180);
  RIGHT_MOTOR_TWO.write(180);
  delay(time);
  stop_driving();
}

//drives back at turning speed
//for use in auto
void drive_backwards_turn(int time){
  Serial.println("driving backwards and turning");
  LEFT_MOTOR_ONE.write(80);
  LEFT_MOTOR_TWO.write(80);
  RIGHT_MOTOR_ONE.write(180);
  RIGHT_MOTOR_TWO.write(180);
  delay(time);
  stop_driving();
}

//drives forward at full speed
//for use in auto
void drive_forwards(int time){
  LEFT_MOTOR_ONE.write(180);
  LEFT_MOTOR_TWO.write(180);
  RIGHT_MOTOR_ONE.write(0);
  RIGHT_MOTOR_TWO.write(0);
  delay(time);
  stop_driving(); 
}

//turns right at full speed
//for use in auto
void turn_right(int time){
  LEFT_MOTOR_ONE.write(180);
  LEFT_MOTOR_TWO.write(180);
  RIGHT_MOTOR_ONE.write(180);
  RIGHT_MOTOR_TWO.write(180);
  delay(time);
  stop_driving();
}

//turns left at full speed
//for use in auto
void turn_left(int time){
  LEFT_MOTOR_ONE.write(0);
  LEFT_MOTOR_TWO.write(0);
  RIGHT_MOTOR_ONE.write(0);
  RIGHT_MOTOR_TWO.write(0);
  delay(time);
  stop_driving();
}

//stops driving
//for use in auto and teleop
void stop_driving(){
  LEFT_MOTOR_ONE.write(90);
  LEFT_MOTOR_TWO.write(90);
  RIGHT_MOTOR_ONE.write(90);
  RIGHT_MOTOR_TWO.write(90);
}



// a tricky recursive function to move the servo more slowly
void move_arm(double toPosition, double fromPosition, double time){
  double dpms = (toPosition-fromPosition) / (time * 1.0);
  if (time == 0)
    return;
  else{
    delay(1);
    SWEEPER_SERVO.write(fromPosition+dpms);
    move_arm(toPosition, fromPosition+dpms, time-1);
  }
}


//move the arm in super fast
void arm_in(){
  SWEEPER_SERVO.write(0);
}

//move the arm out super fast
void arm_out(){
  SWEEPER_SERVO.write(180);
}







// the function to allow operator control for the input number of seconds
void teleop(unsigned long time) {
  unsigned long startTime=millis();
  time=time * 1000;
  while (millis()-startTime<time) {
    poll();

    LEFT_MOTOR_ONE.write(180-LEFT_INPUT);
    LEFT_MOTOR_TWO.write(180-LEFT_INPUT);

    RIGHT_MOTOR_ONE.write(RIGHT_INPUT);
    RIGHT_MOTOR_TWO.write(RIGHT_INPUT);

    LIFT_MOTOR_TWO.write(LIFT_INPUT?0:90);
    LIFT_MOTOR_ONE.write(LIFT_INPUT?180:90);

    if (arm_toggled){
      arm_toggled = false;
      SWEEPER_SERVO.write(requested_sweeper_position);
    }

  }
}
https://snipt.net/embed/bd755617a44875c8b3ea73270ce70cc5/
/raw/bd755617a44875c8b3ea73270ce70cc5/
bd755617a44875c8b3ea73270ce70cc5
cpp
C++
295
2019-08-25T09:15:03
True
False
False
Feb 28, 2013 at 05:34 PM
/api/public/snipt/56752/
newrobo
<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> <a href="#L-74"> 74</a> <a href="#L-75"> 75</a> <a href="#L-76"> 76</a> <a href="#L-77"> 77</a> <a href="#L-78"> 78</a> <a href="#L-79"> 79</a> <a href="#L-80"> 80</a> <a href="#L-81"> 81</a> <a href="#L-82"> 82</a> <a href="#L-83"> 83</a> <a href="#L-84"> 84</a> <a href="#L-85"> 85</a> <a href="#L-86"> 86</a> <a href="#L-87"> 87</a> <a href="#L-88"> 88</a> <a href="#L-89"> 89</a> <a href="#L-90"> 90</a> <a href="#L-91"> 91</a> <a href="#L-92"> 92</a> <a href="#L-93"> 93</a> <a href="#L-94"> 94</a> <a href="#L-95"> 95</a> <a href="#L-96"> 96</a> <a href="#L-97"> 97</a> <a href="#L-98"> 98</a> <a href="#L-99"> 99</a> <a href="#L-100">100</a> <a href="#L-101">101</a> <a href="#L-102">102</a> <a href="#L-103">103</a> <a href="#L-104">104</a> <a href="#L-105">105</a> <a href="#L-106">106</a> <a href="#L-107">107</a> <a href="#L-108">108</a> <a href="#L-109">109</a> <a href="#L-110">110</a> <a href="#L-111">111</a> <a href="#L-112">112</a> <a href="#L-113">113</a> <a href="#L-114">114</a> <a href="#L-115">115</a> <a href="#L-116">116</a> <a href="#L-117">117</a> <a href="#L-118">118</a> <a href="#L-119">119</a> <a href="#L-120">120</a> <a href="#L-121">121</a> <a href="#L-122">122</a> <a href="#L-123">123</a> <a href="#L-124">124</a> <a href="#L-125">125</a> <a href="#L-126">126</a> <a href="#L-127">127</a> <a href="#L-128">128</a> <a href="#L-129">129</a> <a href="#L-130">130</a> <a href="#L-131">131</a> <a href="#L-132">132</a> <a href="#L-133">133</a> <a href="#L-134">134</a> <a href="#L-135">135</a> <a href="#L-136">136</a> <a href="#L-137">137</a> <a href="#L-138">138</a> <a href="#L-139">139</a> <a href="#L-140">140</a> <a href="#L-141">141</a> <a href="#L-142">142</a> <a href="#L-143">143</a> <a href="#L-144">144</a> <a href="#L-145">145</a> <a href="#L-146">146</a> <a href="#L-147">147</a> <a href="#L-148">148</a> <a href="#L-149">149</a> <a href="#L-150">150</a> <a href="#L-151">151</a> <a href="#L-152">152</a> <a href="#L-153">153</a> <a href="#L-154">154</a> <a href="#L-155">155</a> <a href="#L-156">156</a> <a href="#L-157">157</a> <a href="#L-158">158</a> <a href="#L-159">159</a> <a href="#L-160">160</a> <a href="#L-161">161</a> <a href="#L-162">162</a> <a href="#L-163">163</a> <a href="#L-164">164</a> <a href="#L-165">165</a> <a href="#L-166">166</a> <a href="#L-167">167</a> <a href="#L-168">168</a> <a href="#L-169">169</a> <a href="#L-170">170</a> <a href="#L-171">171</a> <a href="#L-172">172</a> <a href="#L-173">173</a> <a href="#L-174">174</a> <a href="#L-175">175</a> <a href="#L-176">176</a> <a href="#L-177">177</a> <a href="#L-178">178</a> <a href="#L-179">179</a> <a href="#L-180">180</a> <a href="#L-181">181</a> <a href="#L-182">182</a> <a href="#L-183">183</a> <a href="#L-184">184</a> <a href="#L-185">185</a> <a href="#L-186">186</a> <a href="#L-187">187</a> <a href="#L-188">188</a> <a href="#L-189">189</a> <a href="#L-190">190</a> <a href="#L-191">191</a> <a href="#L-192">192</a> <a href="#L-193">193</a> <a href="#L-194">194</a> <a href="#L-195">195</a> <a href="#L-196">196</a> <a href="#L-197">197</a> <a href="#L-198">198</a> <a href="#L-199">199</a> <a href="#L-200">200</a> <a href="#L-201">201</a> <a href="#L-202">202</a> <a href="#L-203">203</a> <a href="#L-204">204</a> <a href="#L-205">205</a> <a href="#L-206">206</a> <a href="#L-207">207</a> <a href="#L-208">208</a> <a href="#L-209">209</a> <a href="#L-210">210</a> <a href="#L-211">211</a> <a href="#L-212">212</a> <a href="#L-213">213</a> <a href="#L-214">214</a> <a href="#L-215">215</a> <a href="#L-216">216</a> <a href="#L-217">217</a> <a href="#L-218">218</a> <a href="#L-219">219</a> <a href="#L-220">220</a> <a href="#L-221">221</a> <a href="#L-222">222</a> <a href="#L-223">223</a> <a href="#L-224">224</a> <a href="#L-225">225</a> <a href="#L-226">226</a> <a href="#L-227">227</a> <a href="#L-228">228</a> <a href="#L-229">229</a> <a href="#L-230">230</a> <a href="#L-231">231</a> <a href="#L-232">232</a> <a href="#L-233">233</a> <a href="#L-234">234</a> <a href="#L-235">235</a> <a href="#L-236">236</a> <a href="#L-237">237</a> <a href="#L-238">238</a> <a href="#L-239">239</a> <a href="#L-240">240</a> <a href="#L-241">241</a> <a href="#L-242">242</a> <a href="#L-243">243</a> <a href="#L-244">244</a> <a href="#L-245">245</a> <a href="#L-246">246</a> <a href="#L-247">247</a> <a href="#L-248">248</a> <a href="#L-249">249</a> <a href="#L-250">250</a> <a href="#L-251">251</a> <a href="#L-252">252</a> <a href="#L-253">253</a> <a href="#L-254">254</a> <a href="#L-255">255</a> <a href="#L-256">256</a> <a href="#L-257">257</a> <a href="#L-258">258</a> <a href="#L-259">259</a> <a href="#L-260">260</a> <a href="#L-261">261</a> <a href="#L-262">262</a> <a href="#L-263">263</a> <a href="#L-264">264</a> <a href="#L-265">265</a> <a href="#L-266">266</a> <a href="#L-267">267</a> <a href="#L-268">268</a> <a href="#L-269">269</a> <a href="#L-270">270</a> <a href="#L-271">271</a> <a href="#L-272">272</a> <a href="#L-273">273</a> <a href="#L-274">274</a> <a href="#L-275">275</a> <a href="#L-276">276</a> <a href="#L-277">277</a> <a href="#L-278">278</a> <a href="#L-279">279</a> <a href="#L-280">280</a> <a href="#L-281">281</a> <a href="#L-282">282</a> <a href="#L-283">283</a> <a href="#L-284">284</a> <a href="#L-285">285</a> <a href="#L-286">286</a> <a href="#L-287">287</a> <a href="#L-288">288</a> <a href="#L-289">289</a> <a href="#L-290">290</a> <a href="#L-291">291</a> <a href="#L-292">292</a> <a href="#L-293">293</a> <a href="#L-294">294</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;Servo.h&gt;</span><span class="cp"></span> </span><span id="L-2"><a name="L-2"></a><span class="cp">#include</span> <span class="cpf">&lt;PPM.h&gt;</span><span class="cp"></span> </span><span id="L-3"><a name="L-3"></a> </span><span id="L-4"><a name="L-4"></a><span class="c1">//Set up a connection to the vex controller</span> </span><span id="L-5"><a name="L-5"></a><span class="c1">//=========================================</span> </span><span id="L-6"><a name="L-6"></a><span class="n">PPM</span> <span class="nf">ppm</span><span class="p">(</span><span class="mi">2</span><span class="p">);</span> </span><span id="L-7"><a name="L-7"></a><span class="c1">//=========================================</span> </span><span id="L-8"><a name="L-8"></a> </span><span id="L-9"><a name="L-9"></a> </span><span id="L-10"><a name="L-10"></a> </span><span id="L-11"><a name="L-11"></a><span class="c1">//Set up the servos and motors to interface with the arduino</span> </span><span id="L-12"><a name="L-12"></a><span class="c1">//==========================================================</span> </span><span id="L-13"><a name="L-13"></a><span class="n">Servo</span> <span class="n">LEFT_MOTOR_ONE</span><span class="p">;</span> </span><span id="L-14"><a name="L-14"></a><span class="n">Servo</span> <span class="n">LEFT_MOTOR_TWO</span><span class="p">;</span> </span><span id="L-15"><a name="L-15"></a><span class="c1">//=====================</span> </span><span id="L-16"><a name="L-16"></a><span class="n">Servo</span> <span class="n">RIGHT_MOTOR_ONE</span><span class="p">;</span> </span><span id="L-17"><a name="L-17"></a><span class="n">Servo</span> <span class="n">RIGHT_MOTOR_TWO</span><span class="p">;</span> </span><span id="L-18"><a name="L-18"></a><span class="c1">//=====================</span> </span><span id="L-19"><a name="L-19"></a><span class="n">Servo</span> <span class="n">LIFT_MOTOR_ONE</span><span class="p">;</span> </span><span id="L-20"><a name="L-20"></a><span class="n">Servo</span> <span class="n">LIFT_MOTOR_TWO</span><span class="p">;</span> </span><span id="L-21"><a name="L-21"></a><span class="c1">//=====================</span> </span><span id="L-22"><a name="L-22"></a><span class="n">Servo</span> <span class="n">SWEEPER_SERVO</span><span class="p">;</span> </span><span id="L-23"><a name="L-23"></a><span class="c1">//==========================================================</span> </span><span id="L-24"><a name="L-24"></a> </span><span id="L-25"><a name="L-25"></a> </span><span id="L-26"><a name="L-26"></a> </span><span id="L-27"><a name="L-27"></a><span class="c1">//set up the global integer values used between funtions</span> </span><span id="L-28"><a name="L-28"></a><span class="c1">//======================================================</span> </span><span id="L-29"><a name="L-29"></a><span class="kt">int</span> <span class="n">sweeper_position</span> <span class="o">=</span> <span class="mi">0</span><span class="p">;</span> </span><span id="L-30"><a name="L-30"></a><span class="kt">int</span> <span class="n">requested_sweeper_position</span> <span class="o">=</span> <span class="mi">0</span><span class="p">;</span> </span><span id="L-31"><a name="L-31"></a><span class="c1">//=======================</span> </span><span id="L-32"><a name="L-32"></a><span class="kt">int</span> <span class="n">LEFT_INPUT</span><span class="p">;</span> </span><span id="L-33"><a name="L-33"></a><span class="kt">int</span> <span class="n">RIGHT_INPUT</span><span class="p">;</span> </span><span id="L-34"><a name="L-34"></a><span class="c1">//======================================================</span> </span><span id="L-35"><a name="L-35"></a> </span><span id="L-36"><a name="L-36"></a> </span><span id="L-37"><a name="L-37"></a> </span><span id="L-38"><a name="L-38"></a><span class="c1">//set up the global booleans used between functions</span> </span><span id="L-39"><a name="L-39"></a><span class="c1">//=================================================</span> </span><span id="L-40"><a name="L-40"></a><span class="n">boolean</span> <span class="n">LIFT_INPUT</span><span class="p">;</span> </span><span id="L-41"><a name="L-41"></a><span class="n">boolean</span> <span class="n">arm_toggled</span> <span class="o">=</span> <span class="nb">false</span><span class="p">;</span> </span><span id="L-42"><a name="L-42"></a><span class="c1">//=================================================</span> </span><span id="L-43"><a name="L-43"></a> </span><span id="L-44"><a name="L-44"></a> </span><span id="L-45"><a name="L-45"></a><span class="c1">// Wait until controller is turned on. Do not edit!!!</span> </span><span id="L-46"><a name="L-46"></a><span class="kt">void</span> <span class="nf">waitForController</span> <span class="p">()</span> <span class="p">{</span> </span><span id="L-47"><a name="L-47"></a> <span class="k">while</span> <span class="p">(</span><span class="n">ppm</span><span class="p">.</span><span class="n">getChannel</span><span class="p">(</span><span class="mi">1</span><span class="p">)</span> <span class="o">==</span> <span class="mi">0</span><span class="p">)</span> <span class="p">{</span> <span class="p">}</span> <span class="c1">// do nothing until controller is switched on</span> </span><span id="L-48"><a name="L-48"></a><span class="p">}</span> </span><span id="L-49"><a name="L-49"></a> </span><span id="L-50"><a name="L-50"></a> </span><span id="L-51"><a name="L-51"></a> </span><span id="L-52"><a name="L-52"></a> </span><span id="L-53"><a name="L-53"></a><span class="c1">// initialize all of the motors and servos with thir PWM ports on the arduino</span> </span><span id="L-54"><a name="L-54"></a><span class="kt">void</span> <span class="nf">setup</span><span class="p">(){</span> </span><span id="L-55"><a name="L-55"></a> </span><span id="L-56"><a name="L-56"></a> <span class="n">Serial</span><span class="p">.</span><span class="n">begin</span><span class="p">(</span><span class="mi">9600</span><span class="p">);</span> </span><span id="L-57"><a name="L-57"></a> </span><span id="L-58"><a name="L-58"></a> <span class="n">LEFT_MOTOR_ONE</span><span class="p">.</span><span class="n">attach</span><span class="p">(</span><span class="mi">8</span><span class="p">);</span> </span><span id="L-59"><a name="L-59"></a> <span class="n">LEFT_MOTOR_TWO</span><span class="p">.</span><span class="n">attach</span><span class="p">(</span><span class="mi">9</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">RIGHT_MOTOR_ONE</span><span class="p">.</span><span class="n">attach</span><span class="p">(</span><span class="mi">6</span><span class="p">);</span> </span><span id="L-62"><a name="L-62"></a> <span class="n">RIGHT_MOTOR_TWO</span><span class="p">.</span><span class="n">attach</span><span class="p">(</span><span class="mi">7</span><span class="p">);</span> </span><span id="L-63"><a name="L-63"></a> </span><span id="L-64"><a name="L-64"></a> <span class="n">LIFT_MOTOR_ONE</span><span class="p">.</span><span class="n">attach</span><span class="p">(</span><span class="mi">10</span><span class="p">);</span> </span><span id="L-65"><a name="L-65"></a> <span class="n">LIFT_MOTOR_TWO</span><span class="p">.</span><span class="n">attach</span><span class="p">(</span><span class="mi">11</span><span class="p">);</span> </span><span id="L-66"><a name="L-66"></a> </span><span id="L-67"><a name="L-67"></a> <span class="n">SWEEPER_SERVO</span><span class="p">.</span><span class="n">attach</span><span class="p">(</span><span class="mi">12</span><span class="p">);</span> </span><span id="L-68"><a name="L-68"></a><span class="p">}</span> </span><span id="L-69"><a name="L-69"></a> </span><span id="L-70"><a name="L-70"></a> </span><span id="L-71"><a name="L-71"></a><span class="c1">//the loop function does the following:</span> </span><span id="L-72"><a name="L-72"></a><span class="c1">// runs the autonymous code for 20s</span> </span><span id="L-73"><a name="L-73"></a><span class="c1">// runs the teleoperated code for 150s (2.5 minutes)</span> </span><span id="L-74"><a name="L-74"></a><span class="c1">// exits the program</span> </span><span id="L-75"><a name="L-75"></a><span class="kt">void</span> <span class="nf">loop</span><span class="p">(){</span> </span><span id="L-76"><a name="L-76"></a> <span class="n">arm_in</span><span class="p">();</span> </span><span id="L-77"><a name="L-77"></a> <span class="n">waitForController</span><span class="p">();</span> </span><span id="L-78"><a name="L-78"></a> <span class="n">autonomous</span><span class="p">(</span><span class="mi">20</span><span class="p">);</span> </span><span id="L-79"><a name="L-79"></a> <span class="n">teleop</span><span class="p">(</span><span class="mi">150</span><span class="p">);</span> </span><span id="L-80"><a name="L-80"></a> <span class="n">finish</span><span class="p">();</span> </span><span id="L-81"><a name="L-81"></a><span class="p">}</span> </span><span id="L-82"><a name="L-82"></a> </span><span id="L-83"><a name="L-83"></a><span class="kt">void</span> <span class="nf">finish</span> <span class="p">()</span> <span class="p">{</span> </span><span id="L-84"><a name="L-84"></a> <span class="n">Serial</span><span class="p">.</span><span class="n">println</span><span class="p">(</span><span class="s">&quot;Done&quot;</span><span class="p">);</span> </span><span id="L-85"><a name="L-85"></a> <span class="n">delay</span><span class="p">(</span><span class="mi">50</span><span class="p">);</span> <span class="c1">// With no delay the program could exit before the serial has completed printing</span> </span><span id="L-86"><a name="L-86"></a> <span class="n">exit</span><span class="p">(</span><span class="mi">0</span><span class="p">);</span> <span class="c1">// Exit</span> </span><span id="L-87"><a name="L-87"></a><span class="p">}</span> </span><span id="L-88"><a name="L-88"></a> </span><span id="L-89"><a name="L-89"></a> </span><span id="L-90"><a name="L-90"></a> </span><span id="L-91"><a name="L-91"></a><span class="c1">// the poll function returns no values, but servs to store all of the inputs or their derrived values</span> </span><span id="L-92"><a name="L-92"></a><span class="c1">// as global variables for use in other functions</span> </span><span id="L-93"><a name="L-93"></a><span class="kt">void</span> <span class="nf">poll</span><span class="p">(){</span> </span><span id="L-94"><a name="L-94"></a> <span class="c1">//the joysticks</span> </span><span id="L-95"><a name="L-95"></a> <span class="c1">//used for driving</span> </span><span id="L-96"><a name="L-96"></a> <span class="n">LEFT_INPUT</span> <span class="o">=</span> <span class="n">ppm</span><span class="p">.</span><span class="n">getChannel</span><span class="p">(</span><span class="mi">3</span><span class="p">);</span> </span><span id="L-97"><a name="L-97"></a> <span class="n">RIGHT_INPUT</span> <span class="o">=</span> <span class="n">ppm</span><span class="p">.</span><span class="n">getChannel</span><span class="p">(</span><span class="mi">2</span><span class="p">);</span> </span><span id="L-98"><a name="L-98"></a> </span><span id="L-99"><a name="L-99"></a> <span class="c1">//the channel 6 buttons (right hand operated)</span> </span><span id="L-100"><a name="L-100"></a> <span class="c1">//used for turning the lifting motors on and off</span> </span><span id="L-101"><a name="L-101"></a> <span class="k">if</span> <span class="p">(</span><span class="n">ppm</span><span class="p">.</span><span class="n">getChannel</span><span class="p">(</span><span class="mi">6</span><span class="p">)</span> <span class="o">&gt;</span> <span class="mi">150</span><span class="p">)</span> <span class="c1">//down button</span> </span><span id="L-102"><a name="L-102"></a> <span class="n">LIFT_INPUT</span> <span class="o">=</span> <span class="nb">false</span><span class="p">;</span> </span><span id="L-103"><a name="L-103"></a> <span class="k">if</span> <span class="p">(</span><span class="n">ppm</span><span class="p">.</span><span class="n">getChannel</span><span class="p">(</span><span class="mi">6</span><span class="p">)</span> <span class="o">&lt;</span> <span class="mi">30</span><span class="p">)</span> <span class="c1">//up button</span> </span><span id="L-104"><a name="L-104"></a> <span class="n">LIFT_INPUT</span> <span class="o">=</span> <span class="nb">true</span><span class="p">;</span> </span><span id="L-105"><a name="L-105"></a> </span><span id="L-106"><a name="L-106"></a> <span class="c1">//the channel 5 buttons (left hand operated)</span> </span><span id="L-107"><a name="L-107"></a> <span class="c1">//used for opening and closing the arm</span> </span><span id="L-108"><a name="L-108"></a> <span class="k">if</span> <span class="p">(</span><span class="n">ppm</span><span class="p">.</span><span class="n">getChannel</span><span class="p">(</span><span class="mi">5</span><span class="p">)</span> <span class="o">&gt;</span> <span class="mi">150</span><span class="p">)</span> <span class="c1">//down button</span> </span><span id="L-109"><a name="L-109"></a> <span class="n">requested_sweeper_position</span> <span class="o">=</span> <span class="mi">0</span><span class="p">;</span> </span><span id="L-110"><a name="L-110"></a> <span class="k">else</span> <span class="k">if</span> <span class="p">(</span><span class="n">ppm</span><span class="p">.</span><span class="n">getChannel</span><span class="p">(</span><span class="mi">5</span><span class="p">)</span> <span class="o">&lt;</span> <span class="mi">30</span><span class="p">)</span> <span class="c1">//up button</span> </span><span id="L-111"><a name="L-111"></a> <span class="n">requested_sweeper_position</span> <span class="o">=</span><span class="mi">120</span><span class="p">;</span> </span><span id="L-112"><a name="L-112"></a> <span class="k">else</span> <span class="c1">//no button</span> </span><span id="L-113"><a name="L-113"></a> <span class="n">arm_toggled</span> <span class="o">=</span> <span class="nb">true</span><span class="p">;</span> </span><span id="L-114"><a name="L-114"></a> </span><span id="L-115"><a name="L-115"></a><span class="p">}</span> </span><span id="L-116"><a name="L-116"></a> </span><span id="L-117"><a name="L-117"></a> </span><span id="L-118"><a name="L-118"></a><span class="c1">//runs the autonomous section of the code for the set time</span> </span><span id="L-119"><a name="L-119"></a><span class="kt">void</span> <span class="nf">autonomous</span><span class="p">(</span><span class="kt">unsigned</span> <span class="kt">long</span> <span class="n">time</span><span class="p">)</span> <span class="p">{</span> </span><span id="L-120"><a name="L-120"></a> <span class="kt">unsigned</span> <span class="kt">long</span> <span class="n">startTime</span><span class="o">=</span><span class="n">millis</span><span class="p">();</span> </span><span id="L-121"><a name="L-121"></a> <span class="n">time</span><span class="o">=</span><span class="n">time</span> <span class="o">*</span> <span class="mi">1000</span><span class="p">;</span> </span><span id="L-122"><a name="L-122"></a> <span class="k">while</span> <span class="p">(</span><span class="n">millis</span><span class="p">()</span><span class="o">-</span><span class="n">startTime</span><span class="o">&lt;</span><span class="n">time</span><span class="p">)</span> <span class="p">{</span> </span><span id="L-123"><a name="L-123"></a> <span class="n">operation_deqube</span><span class="p">();</span> </span><span id="L-124"><a name="L-124"></a> <span class="n">operation_pick_me_up</span><span class="p">();</span> </span><span id="L-125"><a name="L-125"></a> </span><span id="L-126"><a name="L-126"></a> <span class="k">while</span> <span class="p">(</span><span class="n">millis</span><span class="p">()</span><span class="o">-</span><span class="n">startTime</span><span class="o">&lt;</span><span class="n">time</span><span class="p">)</span> <span class="p">{</span><span class="n">Serial</span><span class="p">.</span><span class="n">println</span><span class="p">(</span><span class="s">&quot;autonomous has time left&quot;</span><span class="p">);}</span> </span><span id="L-127"><a name="L-127"></a> <span class="n">Serial</span><span class="p">.</span><span class="n">println</span><span class="p">(</span><span class="s">&quot;Autonomous Over!&quot;</span><span class="p">);</span> </span><span id="L-128"><a name="L-128"></a> <span class="n">delay</span><span class="p">(</span><span class="mi">200</span><span class="p">);</span> </span><span id="L-129"><a name="L-129"></a> <span class="p">}</span> </span><span id="L-130"><a name="L-130"></a><span class="p">}</span> </span><span id="L-131"><a name="L-131"></a> </span><span id="L-132"><a name="L-132"></a> </span><span id="L-133"><a name="L-133"></a><span class="c1">//pulls the green cubes towards the conveyor belts</span> </span><span id="L-134"><a name="L-134"></a><span class="kt">void</span> <span class="nf">operation_deqube</span><span class="p">(){</span> </span><span id="L-135"><a name="L-135"></a> <span class="n">arm_in</span><span class="p">();</span> </span><span id="L-136"><a name="L-136"></a> <span class="n">drive_backwards</span><span class="p">(</span><span class="mi">1000</span><span class="p">);</span> </span><span id="L-137"><a name="L-137"></a> <span class="n">delay</span><span class="p">(</span><span class="mi">1000</span><span class="p">);</span> </span><span id="L-138"><a name="L-138"></a> <span class="n">arm_out</span><span class="p">();</span> </span><span id="L-139"><a name="L-139"></a> <span class="n">drive_forwards</span><span class="p">(</span><span class="mi">1000</span><span class="p">);</span> </span><span id="L-140"><a name="L-140"></a> <span class="n">move_arm</span><span class="p">(</span><span class="mi">0</span><span class="p">,</span><span class="mi">180</span><span class="p">,</span><span class="mi">1000</span><span class="p">);</span> </span><span id="L-141"><a name="L-141"></a><span class="p">}</span> </span><span id="L-142"><a name="L-142"></a> </span><span id="L-143"><a name="L-143"></a> </span><span id="L-144"><a name="L-144"></a><span class="c1">//drives back and forth with the treads running attempting to pick up cubes</span> </span><span id="L-145"><a name="L-145"></a><span class="kt">void</span> <span class="nf">operation_pick_me_up</span><span class="p">(){</span> </span><span id="L-146"><a name="L-146"></a> <span class="c1">//turn on the lift</span> </span><span id="L-147"><a name="L-147"></a> <span class="n">LIFT_MOTOR_TWO</span><span class="p">.</span><span class="n">write</span><span class="p">(</span><span class="mi">0</span><span class="p">);</span> </span><span id="L-148"><a name="L-148"></a> <span class="n">LIFT_MOTOR_ONE</span><span class="p">.</span><span class="n">write</span><span class="p">(</span><span class="mi">180</span><span class="p">);</span> </span><span id="L-149"><a name="L-149"></a> </span><span id="L-150"><a name="L-150"></a> <span class="c1">//turn a little on </span> </span><span id="L-151"><a name="L-151"></a> <span class="n">drive_forwards</span><span class="p">(</span><span class="mi">600</span><span class="p">);</span> </span><span id="L-152"><a name="L-152"></a> <span class="n">drive_backwards</span><span class="p">(</span><span class="mi">600</span><span class="p">);</span> </span><span id="L-153"><a name="L-153"></a> </span><span id="L-154"><a name="L-154"></a> <span class="n">delay</span><span class="p">(</span><span class="mi">100</span><span class="p">);</span> </span><span id="L-155"><a name="L-155"></a> <span class="n">stop_driving</span><span class="p">();</span> </span><span id="L-156"><a name="L-156"></a> </span><span id="L-157"><a name="L-157"></a> <span class="n">drive_forwards</span><span class="p">(</span><span class="mi">1500</span><span class="p">);</span> </span><span id="L-158"><a name="L-158"></a> <span class="n">drive_backwards_turn</span><span class="p">(</span><span class="mi">1000</span><span class="p">);</span> </span><span id="L-159"><a name="L-159"></a> </span><span id="L-160"><a name="L-160"></a> <span class="n">drive_forwards</span><span class="p">(</span><span class="mi">2000</span><span class="p">);</span> </span><span id="L-161"><a name="L-161"></a> <span class="n">drive_backwards_turn</span><span class="p">(</span><span class="mi">1000</span><span class="p">);</span> </span><span id="L-162"><a name="L-162"></a> </span><span id="L-163"><a name="L-163"></a> <span class="n">drive_forwards</span><span class="p">(</span><span class="mi">2000</span><span class="p">);</span> </span><span id="L-164"><a name="L-164"></a> <span class="n">drive_backwards_turn</span><span class="p">(</span><span class="mi">1000</span><span class="p">);</span> </span><span id="L-165"><a name="L-165"></a> </span><span id="L-166"><a name="L-166"></a> <span class="n">drive_forwards</span><span class="p">(</span><span class="mi">2000</span><span class="p">);</span> </span><span id="L-167"><a name="L-167"></a> <span class="n">drive_backwards_turn</span><span class="p">(</span><span class="mi">1000</span><span class="p">);</span> </span><span id="L-168"><a name="L-168"></a> </span><span id="L-169"><a name="L-169"></a> <span class="n">drive_forwards</span><span class="p">(</span><span class="mi">2000</span><span class="p">);</span> </span><span id="L-170"><a name="L-170"></a> <span class="n">drive_backwards_turn</span><span class="p">(</span><span class="mi">1000</span><span class="p">);</span> </span><span id="L-171"><a name="L-171"></a> </span><span id="L-172"><a name="L-172"></a> </span><span id="L-173"><a name="L-173"></a><span class="p">}</span> </span><span id="L-174"><a name="L-174"></a> </span><span id="L-175"><a name="L-175"></a><span class="c1">//drives back at full speed</span> </span><span id="L-176"><a name="L-176"></a><span class="c1">//for use in auto</span> </span><span id="L-177"><a name="L-177"></a><span class="kt">void</span> <span class="nf">drive_backwards</span><span class="p">(</span><span class="kt">int</span> <span class="n">time</span><span class="p">){</span> </span><span id="L-178"><a name="L-178"></a> <span class="n">Serial</span><span class="p">.</span><span class="n">println</span><span class="p">(</span><span class="s">&quot;driving backwards&quot;</span><span class="p">);</span> </span><span id="L-179"><a name="L-179"></a> <span class="n">LEFT_MOTOR_ONE</span><span class="p">.</span><span class="n">write</span><span class="p">(</span><span class="mi">0</span><span class="p">);</span> </span><span id="L-180"><a name="L-180"></a> <span class="n">LEFT_MOTOR_TWO</span><span class="p">.</span><span class="n">write</span><span class="p">(</span><span class="mi">0</span><span class="p">);</span> </span><span id="L-181"><a name="L-181"></a> <span class="n">RIGHT_MOTOR_ONE</span><span class="p">.</span><span class="n">write</span><span class="p">(</span><span class="mi">180</span><span class="p">);</span> </span><span id="L-182"><a name="L-182"></a> <span class="n">RIGHT_MOTOR_TWO</span><span class="p">.</span><span class="n">write</span><span class="p">(</span><span class="mi">180</span><span class="p">);</span> </span><span id="L-183"><a name="L-183"></a> <span class="n">delay</span><span class="p">(</span><span class="n">time</span><span class="p">);</span> </span><span id="L-184"><a name="L-184"></a> <span class="n">stop_driving</span><span class="p">();</span> </span><span id="L-185"><a name="L-185"></a><span class="p">}</span> </span><span id="L-186"><a name="L-186"></a> </span><span id="L-187"><a name="L-187"></a><span class="c1">//drives back at turning speed</span> </span><span id="L-188"><a name="L-188"></a><span class="c1">//for use in auto</span> </span><span id="L-189"><a name="L-189"></a><span class="kt">void</span> <span class="nf">drive_backwards_turn</span><span class="p">(</span><span class="kt">int</span> <span class="n">time</span><span class="p">){</span> </span><span id="L-190"><a name="L-190"></a> <span class="n">Serial</span><span class="p">.</span><span class="n">println</span><span class="p">(</span><span class="s">&quot;driving backwards and turning&quot;</span><span class="p">);</span> </span><span id="L-191"><a name="L-191"></a> <span class="n">LEFT_MOTOR_ONE</span><span class="p">.</span><span class="n">write</span><span class="p">(</span><span class="mi">80</span><span class="p">);</span> </span><span id="L-192"><a name="L-192"></a> <span class="n">LEFT_MOTOR_TWO</span><span class="p">.</span><span class="n">write</span><span class="p">(</span><span class="mi">80</span><span class="p">);</span> </span><span id="L-193"><a name="L-193"></a> <span class="n">RIGHT_MOTOR_ONE</span><span class="p">.</span><span class="n">write</span><span class="p">(</span><span class="mi">180</span><span class="p">);</span> </span><span id="L-194"><a name="L-194"></a> <span class="n">RIGHT_MOTOR_TWO</span><span class="p">.</span><span class="n">write</span><span class="p">(</span><span class="mi">180</span><span class="p">);</span> </span><span id="L-195"><a name="L-195"></a> <span class="n">delay</span><span class="p">(</span><span class="n">time</span><span class="p">);</span> </span><span id="L-196"><a name="L-196"></a> <span class="n">stop_driving</span><span class="p">();</span> </span><span id="L-197"><a name="L-197"></a><span class="p">}</span> </span><span id="L-198"><a name="L-198"></a> </span><span id="L-199"><a name="L-199"></a><span class="c1">//drives forward at full speed</span> </span><span id="L-200"><a name="L-200"></a><span class="c1">//for use in auto</span> </span><span id="L-201"><a name="L-201"></a><span class="kt">void</span> <span class="nf">drive_forwards</span><span class="p">(</span><span class="kt">int</span> <span class="n">time</span><span class="p">){</span> </span><span id="L-202"><a name="L-202"></a> <span class="n">LEFT_MOTOR_ONE</span><span class="p">.</span><span class="n">write</span><span class="p">(</span><span class="mi">180</span><span class="p">);</span> </span><span id="L-203"><a name="L-203"></a> <span class="n">LEFT_MOTOR_TWO</span><span class="p">.</span><span class="n">write</span><span class="p">(</span><span class="mi">180</span><span class="p">);</span> </span><span id="L-204"><a name="L-204"></a> <span class="n">RIGHT_MOTOR_ONE</span><span class="p">.</span><span class="n">write</span><span class="p">(</span><span class="mi">0</span><span class="p">);</span> </span><span id="L-205"><a name="L-205"></a> <span class="n">RIGHT_MOTOR_TWO</span><span class="p">.</span><span class="n">write</span><span class="p">(</span><span class="mi">0</span><span class="p">);</span> </span><span id="L-206"><a name="L-206"></a> <span class="n">delay</span><span class="p">(</span><span class="n">time</span><span class="p">);</span> </span><span id="L-207"><a name="L-207"></a> <span class="n">stop_driving</span><span class="p">();</span> </span><span id="L-208"><a name="L-208"></a><span class="p">}</span> </span><span id="L-209"><a name="L-209"></a> </span><span id="L-210"><a name="L-210"></a><span class="c1">//turns right at full speed</span> </span><span id="L-211"><a name="L-211"></a><span class="c1">//for use in auto</span> </span><span id="L-212"><a name="L-212"></a><span class="kt">void</span> <span class="nf">turn_right</span><span class="p">(</span><span class="kt">int</span> <span class="n">time</span><span class="p">){</span> </span><span id="L-213"><a name="L-213"></a> <span class="n">LEFT_MOTOR_ONE</span><span class="p">.</span><span class="n">write</span><span class="p">(</span><span class="mi">180</span><span class="p">);</span> </span><span id="L-214"><a name="L-214"></a> <span class="n">LEFT_MOTOR_TWO</span><span class="p">.</span><span class="n">write</span><span class="p">(</span><span class="mi">180</span><span class="p">);</span> </span><span id="L-215"><a name="L-215"></a> <span class="n">RIGHT_MOTOR_ONE</span><span class="p">.</span><span class="n">write</span><span class="p">(</span><span class="mi">180</span><span class="p">);</span> </span><span id="L-216"><a name="L-216"></a> <span class="n">RIGHT_MOTOR_TWO</span><span class="p">.</span><span class="n">write</span><span class="p">(</span><span class="mi">180</span><span class="p">);</span> </span><span id="L-217"><a name="L-217"></a> <span class="n">delay</span><span class="p">(</span><span class="n">time</span><span class="p">);</span> </span><span id="L-218"><a name="L-218"></a> <span class="n">stop_driving</span><span class="p">();</span> </span><span id="L-219"><a name="L-219"></a><span class="p">}</span> </span><span id="L-220"><a name="L-220"></a> </span><span id="L-221"><a name="L-221"></a><span class="c1">//turns left at full speed</span> </span><span id="L-222"><a name="L-222"></a><span class="c1">//for use in auto</span> </span><span id="L-223"><a name="L-223"></a><span class="kt">void</span> <span class="nf">turn_left</span><span class="p">(</span><span class="kt">int</span> <span class="n">time</span><span class="p">){</span> </span><span id="L-224"><a name="L-224"></a> <span class="n">LEFT_MOTOR_ONE</span><span class="p">.</span><span class="n">write</span><span class="p">(</span><span class="mi">0</span><span class="p">);</span> </span><span id="L-225"><a name="L-225"></a> <span class="n">LEFT_MOTOR_TWO</span><span class="p">.</span><span class="n">write</span><span class="p">(</span><span class="mi">0</span><span class="p">);</span> </span><span id="L-226"><a name="L-226"></a> <span class="n">RIGHT_MOTOR_ONE</span><span class="p">.</span><span class="n">write</span><span class="p">(</span><span class="mi">0</span><span class="p">);</span> </span><span id="L-227"><a name="L-227"></a> <span class="n">RIGHT_MOTOR_TWO</span><span class="p">.</span><span class="n">write</span><span class="p">(</span><span class="mi">0</span><span class="p">);</span> </span><span id="L-228"><a name="L-228"></a> <span class="n">delay</span><span class="p">(</span><span class="n">time</span><span class="p">);</span> </span><span id="L-229"><a name="L-229"></a> <span class="n">stop_driving</span><span class="p">();</span> </span><span id="L-230"><a name="L-230"></a><span class="p">}</span> </span><span id="L-231"><a name="L-231"></a> </span><span id="L-232"><a name="L-232"></a><span class="c1">//stops driving</span> </span><span id="L-233"><a name="L-233"></a><span class="c1">//for use in auto and teleop</span> </span><span id="L-234"><a name="L-234"></a><span class="kt">void</span> <span class="nf">stop_driving</span><span class="p">(){</span> </span><span id="L-235"><a name="L-235"></a> <span class="n">LEFT_MOTOR_ONE</span><span class="p">.</span><span class="n">write</span><span class="p">(</span><span class="mi">90</span><span class="p">);</span> </span><span id="L-236"><a name="L-236"></a> <span class="n">LEFT_MOTOR_TWO</span><span class="p">.</span><span class="n">write</span><span class="p">(</span><span class="mi">90</span><span class="p">);</span> </span><span id="L-237"><a name="L-237"></a> <span class="n">RIGHT_MOTOR_ONE</span><span class="p">.</span><span class="n">write</span><span class="p">(</span><span class="mi">90</span><span class="p">);</span> </span><span id="L-238"><a name="L-238"></a> <span class="n">RIGHT_MOTOR_TWO</span><span class="p">.</span><span class="n">write</span><span class="p">(</span><span class="mi">90</span><span class="p">);</span> </span><span id="L-239"><a name="L-239"></a><span class="p">}</span> </span><span id="L-240"><a name="L-240"></a> </span><span id="L-241"><a name="L-241"></a> </span><span id="L-242"><a name="L-242"></a> </span><span id="L-243"><a name="L-243"></a><span class="c1">// a tricky recursive function to move the servo more slowly</span> </span><span id="L-244"><a name="L-244"></a><span class="kt">void</span> <span class="nf">move_arm</span><span class="p">(</span><span class="kt">double</span> <span class="n">toPosition</span><span class="p">,</span> <span class="kt">double</span> <span class="n">fromPosition</span><span class="p">,</span> <span class="kt">double</span> <span class="n">time</span><span class="p">){</span> </span><span id="L-245"><a name="L-245"></a> <span class="kt">double</span> <span class="n">dpms</span> <span class="o">=</span> <span class="p">(</span><span class="n">toPosition</span><span class="o">-</span><span class="n">fromPosition</span><span class="p">)</span> <span class="o">/</span> <span class="p">(</span><span class="n">time</span> <span class="o">*</span> <span class="mf">1.0</span><span class="p">);</span> </span><span id="L-246"><a name="L-246"></a> <span class="k">if</span> <span class="p">(</span><span class="n">time</span> <span class="o">==</span> <span class="mi">0</span><span class="p">)</span> </span><span id="L-247"><a name="L-247"></a> <span class="k">return</span><span class="p">;</span> </span><span id="L-248"><a name="L-248"></a> <span class="k">else</span><span class="p">{</span> </span><span id="L-249"><a name="L-249"></a> <span class="n">delay</span><span class="p">(</span><span class="mi">1</span><span class="p">);</span> </span><span id="L-250"><a name="L-250"></a> <span class="n">SWEEPER_SERVO</span><span class="p">.</span><span class="n">write</span><span class="p">(</span><span class="n">fromPosition</span><span class="o">+</span><span class="n">dpms</span><span class="p">);</span> </span><span id="L-251"><a name="L-251"></a> <span class="n">move_arm</span><span class="p">(</span><span class="n">toPosition</span><span class="p">,</span> <span class="n">fromPosition</span><span class="o">+</span><span class="n">dpms</span><span class="p">,</span> <span class="n">time</span><span class="o">-</span><span class="mi">1</span><span class="p">);</span> </span><span id="L-252"><a name="L-252"></a> <span class="p">}</span> </span><span id="L-253"><a name="L-253"></a><span class="p">}</span> </span><span id="L-254"><a name="L-254"></a> </span><span id="L-255"><a name="L-255"></a> </span><span id="L-256"><a name="L-256"></a><span class="c1">//move the arm in super fast</span> </span><span id="L-257"><a name="L-257"></a><span class="kt">void</span> <span class="nf">arm_in</span><span class="p">(){</span> </span><span id="L-258"><a name="L-258"></a> <span class="n">SWEEPER_SERVO</span><span class="p">.</span><span class="n">write</span><span class="p">(</span><span class="mi">0</span><span class="p">);</span> </span><span id="L-259"><a name="L-259"></a><span class="p">}</span> </span><span id="L-260"><a name="L-260"></a> </span><span id="L-261"><a name="L-261"></a><span class="c1">//move the arm out super fast</span> </span><span id="L-262"><a name="L-262"></a><span class="kt">void</span> <span class="nf">arm_out</span><span class="p">(){</span> </span><span id="L-263"><a name="L-263"></a> <span class="n">SWEEPER_SERVO</span><span class="p">.</span><span class="n">write</span><span class="p">(</span><span class="mi">180</span><span class="p">);</span> </span><span id="L-264"><a name="L-264"></a><span class="p">}</span> </span><span id="L-265"><a name="L-265"></a> </span><span id="L-266"><a name="L-266"></a> </span><span id="L-267"><a name="L-267"></a> </span><span id="L-268"><a name="L-268"></a> </span><span id="L-269"><a name="L-269"></a> </span><span id="L-270"><a name="L-270"></a> </span><span id="L-271"><a name="L-271"></a> </span><span id="L-272"><a name="L-272"></a><span class="c1">// the function to allow operator control for the input number of seconds</span> </span><span id="L-273"><a name="L-273"></a><span class="kt">void</span> <span class="nf">teleop</span><span class="p">(</span><span class="kt">unsigned</span> <span class="kt">long</span> <span class="n">time</span><span class="p">)</span> <span class="p">{</span> </span><span id="L-274"><a name="L-274"></a> <span class="kt">unsigned</span> <span class="kt">long</span> <span class="n">startTime</span><span class="o">=</span><span class="n">millis</span><span class="p">();</span> </span><span id="L-275"><a name="L-275"></a> <span class="n">time</span><span class="o">=</span><span class="n">time</span> <span class="o">*</span> <span class="mi">1000</span><span class="p">;</span> </span><span id="L-276"><a name="L-276"></a> <span class="k">while</span> <span class="p">(</span><span class="n">millis</span><span class="p">()</span><span class="o">-</span><span class="n">startTime</span><span class="o">&lt;</span><span class="n">time</span><span class="p">)</span> <span class="p">{</span> </span><span id="L-277"><a name="L-277"></a> <span class="n">poll</span><span class="p">();</span> </span><span id="L-278"><a name="L-278"></a> </span><span id="L-279"><a name="L-279"></a> <span class="n">LEFT_MOTOR_ONE</span><span class="p">.</span><span class="n">write</span><span class="p">(</span><span class="mi">180</span><span class="o">-</span><span class="n">LEFT_INPUT</span><span class="p">);</span> </span><span id="L-280"><a name="L-280"></a> <span class="n">LEFT_MOTOR_TWO</span><span class="p">.</span><span class="n">write</span><span class="p">(</span><span class="mi">180</span><span class="o">-</span><span class="n">LEFT_INPUT</span><span class="p">);</span> </span><span id="L-281"><a name="L-281"></a> </span><span id="L-282"><a name="L-282"></a> <span class="n">RIGHT_MOTOR_ONE</span><span class="p">.</span><span class="n">write</span><span class="p">(</span><span class="n">RIGHT_INPUT</span><span class="p">);</span> </span><span id="L-283"><a name="L-283"></a> <span class="n">RIGHT_MOTOR_TWO</span><span class="p">.</span><span class="n">write</span><span class="p">(</span><span class="n">RIGHT_INPUT</span><span class="p">);</span> </span><span id="L-284"><a name="L-284"></a> </span><span id="L-285"><a name="L-285"></a> <span class="n">LIFT_MOTOR_TWO</span><span class="p">.</span><span class="n">write</span><span class="p">(</span><span class="n">LIFT_INPUT</span><span class="o">?</span><span class="mi">0</span><span class="o">:</span><span class="mi">90</span><span class="p">);</span> </span><span id="L-286"><a name="L-286"></a> <span class="n">LIFT_MOTOR_ONE</span><span class="p">.</span><span class="n">write</span><span class="p">(</span><span class="n">LIFT_INPUT</span><span class="o">?</span><span class="mi">180</span><span class="o">:</span><span class="mi">90</span><span class="p">);</span> </span><span id="L-287"><a name="L-287"></a> </span><span id="L-288"><a name="L-288"></a> <span class="k">if</span> <span class="p">(</span><span class="n">arm_toggled</span><span class="p">){</span> </span><span id="L-289"><a name="L-289"></a> <span class="n">arm_toggled</span> <span class="o">=</span> <span class="nb">false</span><span class="p">;</span> </span><span id="L-290"><a name="L-290"></a> <span class="n">SWEEPER_SERVO</span><span class="p">.</span><span class="n">write</span><span class="p">(</span><span class="n">requested_sweeper_position</span><span class="p">);</span> </span><span id="L-291"><a name="L-291"></a> <span class="p">}</span> </span><span id="L-292"><a name="L-292"></a> </span><span id="L-293"><a name="L-293"></a> <span class="p">}</span> </span><span id="L-294"><a name="L-294"></a><span class="p">}</span> </span></pre></div> </td></tr></table>