snipt

Ctrl+h for KB shortcuts

C++

Arduino lol

#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;
boolean robot_alive = true;
//=================================================






// initialize all of the motors and servos with thir PWM ports on the arduino
void setup(){
  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(){
  if (robot_alive){
    autonomous(20);
    teleop(150);
    exit(0);
    robot_alive = false;
  }
}



// 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 = 180;
  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;
  boolean operations_commencing = true;
  while (millis()-startTime<time) {
    if (operations_commencing){
      operation_deqube();
      operation_pick_me_up();
      stop_driving();
      drive_backwards(3600);


      operations_commencing = false;
    }
  }
}


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


//drives back and forth with the treads running attempting to pick up cubes
void operation_pick_me_up(){
  LIFT_MOTOR_TWO.write(0);
  LIFT_MOTOR_ONE.write(180);
  drive_forwards(600);
  drive_backwards(600);
  LEFT_MOTOR_ONE.write(180);
  LEFT_MOTOR_TWO.write(180);
  delay(100);
  stop_driving();
  drive_forwards(500);
  delay(6000);
  LIFT_MOTOR_TWO.write(90);
  LIFT_MOTOR_ONE.write(90);
}

//drives back at full speed
//for use in auto
void drive_backwards(int time){
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 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();
}

//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);

  }
}
https://snipt.net/embed/502c6065b404bcb3945355626e230b2c/
/raw/502c6065b404bcb3945355626e230b2c/
502c6065b404bcb3945355626e230b2c
cpp
C++
232
2019-06-25T19:40:57
True
False
False
Feb 24, 2013 at 08:30 PM
/api/public/snipt/56345/
arduino-lol
<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></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="n">boolean</span> <span class="n">robot_alive</span> <span class="o">=</span> <span class="nb">true</span><span class="p">;</span> </span><span id="L-43"><a name="L-43"></a><span class="c1">//=================================================</span> </span><span id="L-44"><a name="L-44"></a> </span><span id="L-45"><a name="L-45"></a> </span><span id="L-46"><a name="L-46"></a> </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><span id="L-50"><a name="L-50"></a><span class="c1">// initialize all of the motors and servos with thir PWM ports on the arduino</span> </span><span id="L-51"><a name="L-51"></a><span class="kt">void</span> <span class="nf">setup</span><span class="p">(){</span> </span><span id="L-52"><a name="L-52"></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-53"><a name="L-53"></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-54"><a name="L-54"></a> </span><span id="L-55"><a name="L-55"></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-56"><a name="L-56"></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-57"><a name="L-57"></a> </span><span id="L-58"><a name="L-58"></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-59"><a name="L-59"></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-60"><a name="L-60"></a> </span><span id="L-61"><a name="L-61"></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-62"><a name="L-62"></a><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><span id="L-65"><a name="L-65"></a><span class="c1">//the loop function does the following:</span> </span><span id="L-66"><a name="L-66"></a><span class="c1">// runs the autonymous code for 20s</span> </span><span id="L-67"><a name="L-67"></a><span class="c1">// runs the teleoperated code for 150s (2.5 minutes)</span> </span><span id="L-68"><a name="L-68"></a><span class="c1">// exits the program</span> </span><span id="L-69"><a name="L-69"></a><span class="kt">void</span> <span class="nf">loop</span><span class="p">(){</span> </span><span id="L-70"><a name="L-70"></a> <span class="k">if</span> <span class="p">(</span><span class="n">robot_alive</span><span class="p">){</span> </span><span id="L-71"><a name="L-71"></a> <span class="n">autonomous</span><span class="p">(</span><span class="mi">20</span><span class="p">);</span> </span><span id="L-72"><a name="L-72"></a> <span class="n">teleop</span><span class="p">(</span><span class="mi">150</span><span class="p">);</span> </span><span id="L-73"><a name="L-73"></a> <span class="n">exit</span><span class="p">(</span><span class="mi">0</span><span class="p">);</span> </span><span id="L-74"><a name="L-74"></a> <span class="n">robot_alive</span> <span class="o">=</span> <span class="nb">false</span><span class="p">;</span> </span><span id="L-75"><a name="L-75"></a> <span class="p">}</span> </span><span id="L-76"><a name="L-76"></a><span class="p">}</span> </span><span id="L-77"><a name="L-77"></a> </span><span id="L-78"><a name="L-78"></a> </span><span id="L-79"><a name="L-79"></a> </span><span id="L-80"><a name="L-80"></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-81"><a name="L-81"></a><span class="c1">// as global variables for use in other functions</span> </span><span id="L-82"><a name="L-82"></a><span class="kt">void</span> <span class="nf">poll</span><span class="p">(){</span> </span><span id="L-83"><a name="L-83"></a> <span class="c1">//the joysticks</span> </span><span id="L-84"><a name="L-84"></a> <span class="c1">//used for driving</span> </span><span id="L-85"><a name="L-85"></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-86"><a name="L-86"></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-87"><a name="L-87"></a> </span><span id="L-88"><a name="L-88"></a> <span class="c1">//the channel 6 buttons (right hand operated)</span> </span><span id="L-89"><a name="L-89"></a> <span class="c1">//used for turning the lifting motors on and off</span> </span><span id="L-90"><a name="L-90"></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-91"><a name="L-91"></a> <span class="n">LIFT_INPUT</span> <span class="o">=</span> <span class="nb">false</span><span class="p">;</span> </span><span id="L-92"><a name="L-92"></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-93"><a name="L-93"></a> <span class="n">LIFT_INPUT</span> <span class="o">=</span> <span class="nb">true</span><span class="p">;</span> </span><span id="L-94"><a name="L-94"></a> </span><span id="L-95"><a name="L-95"></a> <span class="c1">//the channel 5 buttons (left hand operated)</span> </span><span id="L-96"><a name="L-96"></a> <span class="c1">//used for opening and closing the arm</span> </span><span id="L-97"><a name="L-97"></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-98"><a name="L-98"></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-99"><a name="L-99"></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-100"><a name="L-100"></a> <span class="n">requested_sweeper_position</span> <span class="o">=</span> <span class="mi">180</span><span class="p">;</span> </span><span id="L-101"><a name="L-101"></a> <span class="k">else</span> <span class="c1">//no button</span> </span><span id="L-102"><a name="L-102"></a> <span class="n">arm_toggled</span> <span class="o">=</span> <span class="nb">true</span><span class="p">;</span> </span><span id="L-103"><a name="L-103"></a> </span><span id="L-104"><a name="L-104"></a><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><span id="L-107"><a name="L-107"></a><span class="c1">//runs the autonomous section of the code for the set time</span> </span><span id="L-108"><a name="L-108"></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-109"><a name="L-109"></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-110"><a name="L-110"></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-111"><a name="L-111"></a> <span class="n">boolean</span> <span class="n">operations_commencing</span> <span class="o">=</span> <span class="nb">true</span><span class="p">;</span> </span><span id="L-112"><a name="L-112"></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-113"><a name="L-113"></a> <span class="k">if</span> <span class="p">(</span><span class="n">operations_commencing</span><span class="p">){</span> </span><span id="L-114"><a name="L-114"></a> <span class="n">operation_deqube</span><span class="p">();</span> </span><span id="L-115"><a name="L-115"></a> <span class="n">operation_pick_me_up</span><span class="p">();</span> </span><span id="L-116"><a name="L-116"></a> <span class="n">stop_driving</span><span class="p">();</span> </span><span id="L-117"><a name="L-117"></a> <span class="n">drive_backwards</span><span class="p">(</span><span class="mi">3600</span><span class="p">);</span> </span><span id="L-118"><a name="L-118"></a> </span><span id="L-119"><a name="L-119"></a> </span><span id="L-120"><a name="L-120"></a> <span class="n">operations_commencing</span> <span class="o">=</span> <span class="nb">false</span><span class="p">;</span> </span><span id="L-121"><a name="L-121"></a> <span class="p">}</span> </span><span id="L-122"><a name="L-122"></a> <span class="p">}</span> </span><span id="L-123"><a name="L-123"></a><span class="p">}</span> </span><span id="L-124"><a name="L-124"></a> </span><span id="L-125"><a name="L-125"></a> </span><span id="L-126"><a name="L-126"></a><span class="c1">//pulls the green cubes towards the conveyor belts</span> </span><span id="L-127"><a name="L-127"></a><span class="kt">void</span> <span class="nf">operation_deqube</span><span class="p">(){</span> </span><span id="L-128"><a name="L-128"></a> <span class="n">arm_in</span><span class="p">();</span> </span><span id="L-129"><a name="L-129"></a> <span class="n">drive_backwards</span><span class="p">(</span><span class="mi">1000</span><span class="p">);</span> </span><span id="L-130"><a name="L-130"></a> <span class="n">arm_out</span><span class="p">();</span> </span><span id="L-131"><a name="L-131"></a> <span class="n">drive_forwards</span><span class="p">(</span><span class="mi">1000</span><span class="p">);</span> </span><span id="L-132"><a name="L-132"></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">1500</span><span class="p">);</span> </span><span id="L-133"><a name="L-133"></a><span class="p">}</span> </span><span id="L-134"><a name="L-134"></a> </span><span id="L-135"><a name="L-135"></a> </span><span id="L-136"><a name="L-136"></a><span class="c1">//drives back and forth with the treads running attempting to pick up cubes</span> </span><span id="L-137"><a name="L-137"></a><span class="kt">void</span> <span class="nf">operation_pick_me_up</span><span class="p">(){</span> </span><span id="L-138"><a name="L-138"></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-139"><a name="L-139"></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-140"><a name="L-140"></a> <span class="n">drive_forwards</span><span class="p">(</span><span class="mi">600</span><span class="p">);</span> </span><span id="L-141"><a name="L-141"></a> <span class="n">drive_backwards</span><span class="p">(</span><span class="mi">600</span><span class="p">);</span> </span><span id="L-142"><a name="L-142"></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-143"><a name="L-143"></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-144"><a name="L-144"></a> <span class="n">delay</span><span class="p">(</span><span class="mi">100</span><span class="p">);</span> </span><span id="L-145"><a name="L-145"></a> <span class="n">stop_driving</span><span class="p">();</span> </span><span id="L-146"><a name="L-146"></a> <span class="n">drive_forwards</span><span class="p">(</span><span class="mi">500</span><span class="p">);</span> </span><span id="L-147"><a name="L-147"></a> <span class="n">delay</span><span class="p">(</span><span class="mi">6000</span><span class="p">);</span> </span><span id="L-148"><a name="L-148"></a> <span class="n">LIFT_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-149"><a name="L-149"></a> <span class="n">LIFT_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-150"><a name="L-150"></a><span class="p">}</span> </span><span id="L-151"><a name="L-151"></a> </span><span id="L-152"><a name="L-152"></a><span class="c1">//drives back at full speed</span> </span><span id="L-153"><a name="L-153"></a><span class="c1">//for use in auto</span> </span><span id="L-154"><a name="L-154"></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-155"><a name="L-155"></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-156"><a name="L-156"></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-157"><a name="L-157"></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-158"><a name="L-158"></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-159"><a name="L-159"></a><span class="n">delay</span><span class="p">(</span><span class="n">time</span><span class="p">);</span> </span><span id="L-160"><a name="L-160"></a><span class="n">stop_driving</span><span class="p">();</span> </span><span id="L-161"><a name="L-161"></a><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="c1">//drives forward at full speed</span> </span><span id="L-164"><a name="L-164"></a><span class="c1">//for use in auto</span> </span><span id="L-165"><a name="L-165"></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-166"><a name="L-166"></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-167"><a name="L-167"></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-168"><a name="L-168"></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-169"><a name="L-169"></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-170"><a name="L-170"></a><span class="n">delay</span><span class="p">(</span><span class="n">time</span><span class="p">);</span> </span><span id="L-171"><a name="L-171"></a><span class="n">stop_driving</span><span class="p">();</span> </span><span id="L-172"><a name="L-172"></a><span class="p">}</span> </span><span id="L-173"><a name="L-173"></a> </span><span id="L-174"><a name="L-174"></a><span class="c1">//stops driving</span> </span><span id="L-175"><a name="L-175"></a><span class="c1">//for use in auto and teleop</span> </span><span id="L-176"><a name="L-176"></a><span class="kt">void</span> <span class="nf">stop_driving</span><span class="p">(){</span> </span><span id="L-177"><a name="L-177"></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-178"><a name="L-178"></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-179"><a name="L-179"></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-180"><a name="L-180"></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-181"><a name="L-181"></a><span class="p">}</span> </span><span id="L-182"><a name="L-182"></a> </span><span id="L-183"><a name="L-183"></a> </span><span id="L-184"><a name="L-184"></a> </span><span id="L-185"><a name="L-185"></a><span class="c1">// a tricky recursive function to move the servo more slowly</span> </span><span id="L-186"><a name="L-186"></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-187"><a name="L-187"></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-188"><a name="L-188"></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-189"><a name="L-189"></a> <span class="k">return</span><span class="p">;</span> </span><span id="L-190"><a name="L-190"></a> <span class="k">else</span><span class="p">{</span> </span><span id="L-191"><a name="L-191"></a> <span class="n">delay</span><span class="p">(</span><span class="mi">1</span><span class="p">);</span> </span><span id="L-192"><a name="L-192"></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-193"><a name="L-193"></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-194"><a name="L-194"></a> <span class="p">}</span> </span><span id="L-195"><a name="L-195"></a><span class="p">}</span> </span><span id="L-196"><a name="L-196"></a> </span><span id="L-197"><a name="L-197"></a> </span><span id="L-198"><a name="L-198"></a><span class="c1">//move the arm in super fast</span> </span><span id="L-199"><a name="L-199"></a><span class="kt">void</span> <span class="nf">arm_in</span><span class="p">(){</span> </span><span id="L-200"><a name="L-200"></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-201"><a name="L-201"></a><span class="p">}</span> </span><span id="L-202"><a name="L-202"></a> </span><span id="L-203"><a name="L-203"></a><span class="c1">//move the arm out super fast</span> </span><span id="L-204"><a name="L-204"></a><span class="kt">void</span> <span class="nf">arm_out</span><span class="p">(){</span> </span><span id="L-205"><a name="L-205"></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-206"><a name="L-206"></a><span class="p">}</span> </span><span id="L-207"><a name="L-207"></a> </span><span id="L-208"><a name="L-208"></a> </span><span id="L-209"><a name="L-209"></a> </span><span id="L-210"><a name="L-210"></a> </span><span id="L-211"><a name="L-211"></a> </span><span id="L-212"><a name="L-212"></a> </span><span id="L-213"><a name="L-213"></a> </span><span id="L-214"><a name="L-214"></a><span class="c1">// the function to allow operator control for the input number of seconds</span> </span><span id="L-215"><a name="L-215"></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-216"><a name="L-216"></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-217"><a name="L-217"></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-218"><a name="L-218"></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-219"><a name="L-219"></a> <span class="n">poll</span><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="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-222"><a name="L-222"></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-223"><a name="L-223"></a> </span><span id="L-224"><a name="L-224"></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-225"><a name="L-225"></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-226"><a name="L-226"></a> </span><span id="L-227"><a name="L-227"></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-228"><a name="L-228"></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-229"><a name="L-229"></a> </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 class="p">}</span> </span></pre></div> </td></tr></table>