{"id":721,"date":"2014-12-03T09:15:52","date_gmt":"2014-12-03T09:15:52","guid":{"rendered":"http:\/\/dronesonen.hibu.no\/?p=721"},"modified":"2014-12-03T09:15:52","modified_gmt":"2014-12-03T09:15:52","slug":"everything-works","status":"publish","type":"post","link":"https:\/\/dronesonen.usn.no\/?p=721","title":{"rendered":"Everything works!"},"content":{"rendered":"<p>Over the course of the last few weeks, we have found a solution for focusing the camera and take pictures with the two buttons on the handle, these uses an interrupt function to not disturb the rest of the loop with delays. While working on this we found out we needed two more buttons, but we solved this by using the third button (the one on the joystick), to have 4 functions on 3 buttons. one function if we press joystick + focus button and another function if we press joystick + shutter button.<\/p>\n<p>There were a few problems recently, for example when we put the arduino board in it&#8217;s 3d printed case, the programs loop suddenly stopped, this has been fixed by making a new box (better size of the box). another problem we had, were the gyro sending out wrong output, after some troubleshooting we found out it was a wire on the joystick that had broken off.<\/p>\n<p>All of the components has been mounted to the gimbal, and after some tuning, we have a ready product. We shot a video of the gimbal in action. After some video editing and we have a movie ready for the presentation this Thursday. You can see the movie below.<\/p>\n<p>https:\/\/www.youtube.com\/watch?v=3esw502DGr0<\/p>\n<p>We also made a video of the extra functions we made for the gimbal, it can be seen below.<\/p>\n<p>https:\/\/www.youtube.com\/watch?v=k5N8FFzYRpU<\/p>\n<p>While working on this project, we found some things that could be improved and some features we would have liked to have.<\/p>\n<ul>\n<li>Replace the servos with step motors for smoother and more accurate movements<\/li>\n<li>Send our drawings of the 3d printed bearing supports to be manufactured in metal. This is to make the camera shake less, and support higher weights\/movements<\/li>\n<li>It would also be nice to get the compass function to work properly, instead of our homemade function that works against the gyroscope buildup of yaw values.<\/li>\n<li>The two batteries could be changed out with a battery package and a voltage devider, for easy charging.<\/li>\n<li>A screen mounted on the gimbal, showing the liveview from the camera.<\/li>\n<\/ul>\n<p>&nbsp;<\/p>\n<p>The final Fritzing drawing is shown below.<\/p>\n<p><img decoding=\"async\" src=\"http:\/\/dronesonen.usn.no\/wp-content\/uploads\/2014\/12\/Gimbal_bb-rev5.png\" alt=\"Fritzing rev5\" \/><\/p>\n<p>The final Arduino code is shown below.<\/p>\n<div style=\"height: 700px;width: 800px;overflow: scroll\">\n<pre>  <span style=\"color: #004a43\">#<\/span><span style=\"color: #004a43\">include <\/span><span style=\"color: #800000\">\"<\/span><span style=\"color: #40015a\">Wire.h<\/span><span style=\"color: #800000\">\"<\/span>\r\n  <span style=\"color: #004a43\">\u00a0\u00a0#<\/span><span style=\"color: #004a43\">include <\/span><span style=\"color: #800000\">\"<\/span><span style=\"color: #40015a\">I2Cdev.h<\/span><span style=\"color: #800000\">\"<\/span>\r\n  <span style=\"color: #004a43\">\u00a0\u00a0#<\/span><span style=\"color: #004a43\">include <\/span><span style=\"color: #800000\">\"<\/span><span style=\"color: #40015a\">MPU6050.h<\/span><span style=\"color: #800000\">\"<\/span>\r\n  <span style=\"color: #004a43\">\u00a0\u00a0#<\/span><span style=\"color: #004a43\">include <\/span><span style=\"color: #800000\">\"<\/span><span style=\"color: #40015a\">Servo.h<\/span><span style=\"color: #800000\">\"<\/span>\r\n  \r\n  <span style=\"color: #696969\">\/*This code was made to function with a custom-made Gimball with an attached DSLR-camera. It's made to<\/span>\r\n  <span style=\"color: #696969\">\u00a0\u00a0\u00a0*function with a 9-axis MPU motion-sensor, equipped with a gyroscope, an accelerometer, and a compass.*\/<\/span>\r\n  \r\n  <span style=\"color: #696969\">\/\/Set up some pins.<\/span>\r\n  <span style=\"color: #800000;font-weight: bold\">int<\/span> joystickVerticalPin <span style=\"color: #808030\">=<\/span> A0<span style=\"color: #800080\">;<\/span>      \r\n  <span style=\"color: #800000;font-weight: bold\">int<\/span> joystickHorizontalPin <span style=\"color: #808030\">=<\/span> A1<span style=\"color: #800080\">;<\/span>\r\n  <span style=\"color: #800000;font-weight: bold\">int<\/span> focusButton <span style=\"color: #808030\">=<\/span> <span style=\"color: #008c00\">2<\/span><span style=\"color: #800080\">;<\/span>\r\n  <span style=\"color: #800000;font-weight: bold\">int<\/span> shutterButton <span style=\"color: #808030\">=<\/span> <span style=\"color: #008c00\">3<\/span><span style=\"color: #800080\">;<\/span>\r\n  <span style=\"color: #800000;font-weight: bold\">int<\/span> focusPin <span style=\"color: #808030\">=<\/span> <span style=\"color: #008c00\">4<\/span><span style=\"color: #800080\">;<\/span>\r\n  <span style=\"color: #800000;font-weight: bold\">int<\/span> shutterPin <span style=\"color: #808030\">=<\/span> <span style=\"color: #008c00\">5<\/span><span style=\"color: #800080\">;<\/span>\r\n  <span style=\"color: #800000;font-weight: bold\">int<\/span> joyButtonPin <span style=\"color: #808030\">=<\/span> <span style=\"color: #008c00\">6<\/span><span style=\"color: #800080\">;<\/span>\r\n  \r\n  MPU6050 mpu<span style=\"color: #800080\">;<\/span>                     <span style=\"color: #696969\">\/\/The MPU6050-object is a reference to our 9-axis gyroscope.<\/span>\r\n  \r\n  Servo servoPitch<span style=\"color: #800080\">;<\/span>                <span style=\"color: #696969\">\/\/This servo-object refers to the servo responsible for the pitch-angle.<\/span>\r\n  Servo servoRoll<span style=\"color: #800080\">;<\/span>                 <span style=\"color: #696969\">\/\/This servo-object refers to the servo responsible for the roll-angle.<\/span>\r\n  Servo servoYaw<span style=\"color: #800080\">;<\/span>                  <span style=\"color: #696969\">\/\/This servo-object refers to the servo responsible for the yaw-angle.<\/span>\r\n  \r\n  <span style=\"color: #800000;font-weight: bold\">int<\/span> pitchPrevious<span style=\"color: #800080\">;<\/span>               <span style=\"color: #696969\">\/\/Stores the pitch-angle from the previous loop for reference in the next iteration.<\/span>\r\n  <span style=\"color: #800000;font-weight: bold\">int<\/span> rollPrevious<span style=\"color: #800080\">;<\/span>                <span style=\"color: #696969\">\/\/Stores the roll-angle from the previous loop for reference in the next iteration.<\/span>\r\n  <span style=\"color: #800000;font-weight: bold\">int<\/span> yawPrevious<span style=\"color: #800080\">;<\/span>                 <span style=\"color: #696969\">\/\/Stores the yaw-angle from the previous loop for reference in the next iteration.<\/span>\r\n\r\n  <span style=\"color: #800000;font-weight: bold\">const<\/span> <span style=\"color: #800000;font-weight: bold\">int<\/span> maxPitch <span style=\"color: #808030\">=<\/span> <span style=\"color: #008c00\">140<\/span><span style=\"color: #800080\">;<\/span>        <span style=\"color: #696969\">\/\/Limits the maximum pitch-angle.  <\/span>\r\n  <span style=\"color: #800000;font-weight: bold\">const<\/span> <span style=\"color: #800000;font-weight: bold\">int<\/span> maxRoll <span style=\"color: #808030\">=<\/span> <span style=\"color: #008c00\">140<\/span><span style=\"color: #800080\">;<\/span>         <span style=\"color: #696969\">\/\/Limits the maximum roll-angle.  <\/span>\r\n  <span style=\"color: #800000;font-weight: bold\">const<\/span> <span style=\"color: #800000;font-weight: bold\">int<\/span> maxYaw <span style=\"color: #808030\">=<\/span> <span style=\"color: #008c00\">100<\/span><span style=\"color: #800080\">;<\/span>          <span style=\"color: #696969\">\/\/Limits the maximum yaw-angle.<\/span>\r\n  <span style=\"color: #800000;font-weight: bold\">const<\/span> <span style=\"color: #800000;font-weight: bold\">int<\/span> minPitch <span style=\"color: #808030\">=<\/span> <span style=\"color: #008c00\">58<\/span><span style=\"color: #800080\">;<\/span>         <span style=\"color: #696969\">\/\/Limits the minimum pitch-angle. <\/span>\r\n  <span style=\"color: #800000;font-weight: bold\">const<\/span> <span style=\"color: #800000;font-weight: bold\">int<\/span> minRoll <span style=\"color: #808030\">=<\/span> <span style=\"color: #008c00\">63<\/span><span style=\"color: #800080\">;<\/span>          <span style=\"color: #696969\">\/\/Limits the minimum roll-angle. <\/span>\r\n  <span style=\"color: #800000;font-weight: bold\">const<\/span> <span style=\"color: #800000;font-weight: bold\">int<\/span> minYaw <span style=\"color: #808030\">=<\/span> <span style=\"color: #008c00\">10<\/span><span style=\"color: #800080\">;<\/span>           <span style=\"color: #696969\">\/\/Limits the minimum yaw-angle.<\/span>\r\n  <span style=\"color: #800000;font-weight: bold\">const<\/span> <span style=\"color: #800000;font-weight: bold\">int<\/span> zeroPitch <span style=\"color: #808030\">=<\/span> <span style=\"color: #008c00\">105<\/span><span style=\"color: #800080\">;<\/span>       <span style=\"color: #696969\">\/\/Neutral pitch.<\/span>\r\n  <span style=\"color: #800000;font-weight: bold\">const<\/span> <span style=\"color: #800000;font-weight: bold\">int<\/span> zeroRoll <span style=\"color: #808030\">=<\/span> <span style=\"color: #008c00\">102<\/span><span style=\"color: #800080\">;<\/span>        <span style=\"color: #696969\">\/\/Neutral roll.<\/span>\r\n  <span style=\"color: #800000;font-weight: bold\">const<\/span> <span style=\"color: #800000;font-weight: bold\">int<\/span> zeroYaw <span style=\"color: #808030\">=<\/span> <span style=\"color: #008c00\">58<\/span><span style=\"color: #800080\">;<\/span>          <span style=\"color: #696969\">\/\/Neutral yaw.<\/span>\r\n  \r\n  boolean yawLocked <span style=\"color: #808030\">=<\/span> <span style=\"color: #800000;font-weight: bold\">false<\/span><span style=\"color: #800080\">;<\/span>       <span style=\"color: #696969\">\/\/True if the yaw-rotation is deactivated.<\/span>\r\n  \r\n  <span style=\"color: #800000;font-weight: bold\">const<\/span> <span style=\"color: #800000;font-weight: bold\">float<\/span> percentGyro <span style=\"color: #808030\">=<\/span> <span style=\"color: #008000\">0.90<\/span><span style=\"color: #800080\">;<\/span>  <span style=\"color: #696969\">\/\/The percentage of the gyro-signal that is used in the complimentary filter computations.<\/span>\r\n  <span style=\"color: #800000;font-weight: bold\">const<\/span> <span style=\"color: #800000;font-weight: bold\">float<\/span> percentAcc <span style=\"color: #808030\">=<\/span> <span style=\"color: #008000\">0.10<\/span><span style=\"color: #800080\">;<\/span>   <span style=\"color: #696969\">\/\/The percentage of the accelerometer-signal that is used in the complimentary filter computations.<\/span>\r\n    \r\n  <span style=\"color: #800000;font-weight: bold\">float<\/span> joystickVertical <span style=\"color: #808030\">=<\/span> <span style=\"color: #008c00\">0<\/span><span style=\"color: #800080\">;<\/span>      <span style=\"color: #696969\">\/\/This holds the additional pitch-angle caused by joystick-tilt.<\/span>\r\n  <span style=\"color: #800000;font-weight: bold\">float<\/span> joystickHorizontal <span style=\"color: #808030\">=<\/span> <span style=\"color: #008c00\">0<\/span><span style=\"color: #800080\">;<\/span>    <span style=\"color: #696969\">\/\/This holds the additional roll-angle caused by joystick-pan.<\/span>\r\n  \r\n  <span style=\"color: #800000;font-weight: bold\">unsigned<\/span> <span style=\"color: #800000;font-weight: bold\">long<\/span> focusTimer<span style=\"color: #800080\">;<\/span>        <span style=\"color: #696969\">\/\/Timer to know when to stop the focus-signal.<\/span>\r\n  <span style=\"color: #800000;font-weight: bold\">unsigned<\/span> <span style=\"color: #800000;font-weight: bold\">long<\/span> shutterTimer<span style=\"color: #800080\">;<\/span>      <span style=\"color: #696969\">\/\/Timer to know when to stop the shutter-signal.<\/span>\r\n  boolean focusing<span style=\"color: #800080\">;<\/span>                <span style=\"color: #696969\">\/\/True if the camera is focusing.<\/span>\r\n  boolean shuttering<span style=\"color: #800080\">;<\/span>              <span style=\"color: #696969\">\/\/True if the camera is taking a picture.<\/span>\r\n  \r\n  <span style=\"color: #800000;font-weight: bold\">const<\/span> <span style=\"color: #800000;font-weight: bold\">int<\/span> adjustmentCoefficient <span style=\"color: #808030\">=<\/span> <span style=\"color: #008000\">0.3<\/span><span style=\"color: #800080\">;<\/span> <span style=\"color: #696969\">\/\/Used for adjusting and tweaking the written servo-angle based on current angle.<\/span>\r\n  \r\n  <span style=\"color: #800000;font-weight: bold\">unsigned<\/span> <span style=\"color: #800000;font-weight: bold\">long<\/span> timer<span style=\"color: #800080\">;<\/span>                   <span style=\"color: #696969\">\/\/The timer measures the time between each gyroscope-reading. Used in the Kalman-filter.<\/span>\r\n  \r\n  \r\n  <span style=\"color: #800000;font-weight: bold\">void<\/span> setup<span style=\"color: #808030\">(<\/span><span style=\"color: #808030\">)<\/span> \r\n  <span style=\"color: #800080\">{<\/span>\r\n      Wire<span style=\"color: #808030\">.<\/span>begin<span style=\"color: #808030\">(<\/span><span style=\"color: #808030\">)<\/span><span style=\"color: #800080\">;<\/span>\r\n      Serial<span style=\"color: #808030\">.<\/span>begin<span style=\"color: #808030\">(<\/span><span style=\"color: #008c00\">38400<\/span><span style=\"color: #808030\">)<\/span><span style=\"color: #800080\">;<\/span>\r\n  \r\n      <span style=\"color: #696969\">\/\/Initialize the MPU and attach the servos to their respective pins.<\/span>\r\n      Serial<span style=\"color: #808030\">.<\/span>println<span style=\"color: #808030\">(<\/span><span style=\"color: #800000\">\"<\/span><span style=\"color: #0000e6\">Initialize MPU<\/span><span style=\"color: #800000\">\"<\/span><span style=\"color: #808030\">)<\/span><span style=\"color: #800080\">;<\/span>\r\n      mpu<span style=\"color: #808030\">.<\/span>initialize<span style=\"color: #808030\">(<\/span><span style=\"color: #808030\">)<\/span><span style=\"color: #800080\">;<\/span>\r\n      Serial<span style=\"color: #808030\">.<\/span>println<span style=\"color: #808030\">(<\/span>mpu<span style=\"color: #808030\">.<\/span>testConnection<span style=\"color: #808030\">(<\/span><span style=\"color: #808030\">)<\/span> <span style=\"color: #800080\">?<\/span> <span style=\"color: #800000\">\"<\/span><span style=\"color: #0000e6\">Connected<\/span><span style=\"color: #800000\">\"<\/span> <span style=\"color: #800080\">:<\/span> <span style=\"color: #800000\">\"<\/span><span style=\"color: #0000e6\">Connection failed<\/span><span style=\"color: #800000\">\"<\/span><span style=\"color: #808030\">)<\/span><span style=\"color: #800080\">;<\/span>\r\n      servoPitch<span style=\"color: #808030\">.<\/span>attach<span style=\"color: #808030\">(<\/span><span style=\"color: #008c00\">9<\/span><span style=\"color: #808030\">)<\/span><span style=\"color: #800080\">;<\/span>\r\n      servoRoll<span style=\"color: #808030\">.<\/span>attach<span style=\"color: #808030\">(<\/span><span style=\"color: #008c00\">10<\/span><span style=\"color: #808030\">)<\/span><span style=\"color: #800080\">;<\/span>\r\n      servoYaw<span style=\"color: #808030\">.<\/span>attach<span style=\"color: #808030\">(<\/span><span style=\"color: #008c00\">11<\/span><span style=\"color: #808030\">)<\/span><span style=\"color: #800080\">;<\/span>\r\n      \r\n      attachInterrupt<span style=\"color: #808030\">(<\/span><span style=\"color: #008c00\">0<\/span><span style=\"color: #808030\">,<\/span> focus<span style=\"color: #808030\">,<\/span> RISING<span style=\"color: #808030\">)<\/span><span style=\"color: #800080\">;<\/span>\r\n      attachInterrupt<span style=\"color: #808030\">(<\/span><span style=\"color: #008c00\">1<\/span><span style=\"color: #808030\">,<\/span> shutter<span style=\"color: #808030\">,<\/span> RISING<span style=\"color: #808030\">)<\/span><span style=\"color: #800080\">;<\/span>\r\n      pinMode<span style=\"color: #808030\">(<\/span>focusPin<span style=\"color: #808030\">,<\/span> OUTPUT<span style=\"color: #808030\">)<\/span><span style=\"color: #800080\">;<\/span>\r\n      pinMode<span style=\"color: #808030\">(<\/span>shutterPin<span style=\"color: #808030\">,<\/span> OUTPUT<span style=\"color: #808030\">)<\/span><span style=\"color: #800080\">;<\/span>\r\n      pinMode<span style=\"color: #808030\">(<\/span>focusButton<span style=\"color: #808030\">,<\/span> INPUT<span style=\"color: #808030\">)<\/span><span style=\"color: #800080\">;<\/span>\r\n      pinMode<span style=\"color: #808030\">(<\/span>shutterButton<span style=\"color: #808030\">,<\/span> INPUT<span style=\"color: #808030\">)<\/span><span style=\"color: #800080\">;<\/span>\r\n      pinMode<span style=\"color: #808030\">(<\/span>joyButtonPin<span style=\"color: #808030\">,<\/span> INPUT<span style=\"color: #808030\">)<\/span><span style=\"color: #800080\">;<\/span>\r\n      focusing <span style=\"color: #808030\">=<\/span> <span style=\"color: #800000;font-weight: bold\">false<\/span><span style=\"color: #800080\">;<\/span>\r\n      shuttering <span style=\"color: #808030\">=<\/span> <span style=\"color: #800000;font-weight: bold\">false<\/span><span style=\"color: #800080\">;<\/span>\r\n      \r\n      <span style=\"color: #696969\">\/\/Initialize the coordinates to fit the starting position.<\/span>\r\n      reset<span style=\"color: #808030\">(<\/span><span style=\"color: #808030\">)<\/span><span style=\"color: #800080\">;<\/span>\r\n  <span style=\"color: #800080\">}<\/span>\r\n  \r\n  \r\n  <span style=\"color: #800000;font-weight: bold\">void<\/span> loop<span style=\"color: #808030\">(<\/span><span style=\"color: #808030\">)<\/span> \r\n  <span style=\"color: #800080\">{<\/span>\r\n      <span style=\"color: #696969\">\/\/Create an array to store the pitch, roll, and yaw respectively, after they've been read and computed.<\/span>\r\n      <span style=\"color: #800000;font-weight: bold\">int<\/span> orientation<span style=\"color: #808030\">[<\/span><span style=\"color: #008c00\">3<\/span><span style=\"color: #808030\">]<\/span><span style=\"color: #800080\">;<\/span>             \r\n    \r\n      <span style=\"color: #696969\">\/\/Get the motion-data, fill the array.<\/span>\r\n      readCoordinates<span style=\"color: #808030\">(<\/span>orientation<span style=\"color: #808030\">)<\/span><span style=\"color: #800080\">;<\/span>\r\n      \r\n      <span style=\"color: #696969\">\/\/Check for user-input via joystick.<\/span>\r\n      readJoystick<span style=\"color: #808030\">(<\/span><span style=\"color: #808030\">)<\/span><span style=\"color: #800080\">;<\/span>\r\n\r\n       <span style=\"color: #696969\">\/\/Re-adjust the signal to make up for physical off-set.<\/span>\r\n      <span style=\"color: #696969\">\/\/A positive offset will increase the pitch and vice versa.<\/span>\r\n      <span style=\"color: #800000;font-weight: bold\">int<\/span> offsetPitch <span style=\"color: #808030\">=<\/span> orientation<span style=\"color: #808030\">[<\/span><span style=\"color: #008c00\">0<\/span><span style=\"color: #808030\">]<\/span> <span style=\"color: #808030\">-<\/span> zeroPitch<span style=\"color: #800080\">;<\/span>\r\n      <span style=\"color: #800000;font-weight: bold\">int<\/span> offsetRoll <span style=\"color: #808030\">=<\/span> orientation<span style=\"color: #808030\">[<\/span><span style=\"color: #008c00\">1<\/span><span style=\"color: #808030\">]<\/span> <span style=\"color: #808030\">-<\/span> zeroRoll<span style=\"color: #800080\">;<\/span>\r\n\r\n      <span style=\"color: #696969\">\/\/Calculate the increased value based on current angle.<\/span>\r\n      orientation<span style=\"color: #808030\">[<\/span><span style=\"color: #008c00\">0<\/span><span style=\"color: #808030\">]<\/span> <span style=\"color: #808030\">=<\/span> orientation<span style=\"color: #808030\">[<\/span><span style=\"color: #008c00\">0<\/span><span style=\"color: #808030\">]<\/span> <span style=\"color: #808030\">+<\/span> <span style=\"color: #808030\">(<\/span>offsetPitch <span style=\"color: #808030\">*<\/span> adjustmentCoefficient<span style=\"color: #808030\">)<\/span><span style=\"color: #800080\">;<\/span>\r\n      orientation<span style=\"color: #808030\">[<\/span><span style=\"color: #008c00\">1<\/span><span style=\"color: #808030\">]<\/span> <span style=\"color: #808030\">=<\/span> orientation<span style=\"color: #808030\">[<\/span><span style=\"color: #008c00\">1<\/span><span style=\"color: #808030\">]<\/span> <span style=\"color: #808030\">+<\/span> <span style=\"color: #808030\">(<\/span>offsetRoll <span style=\"color: #808030\">*<\/span> adjustmentCoefficient<span style=\"color: #808030\">)<\/span><span style=\"color: #800080\">;<\/span>\r\n      \r\n      <span style=\"color: #696969\">\/\/Keep the signals for the servos within the angles we have set.<\/span>\r\n      <span style=\"color: #696969\">\/\/We can use constrain() here instead, most likely.<\/span>\r\n      orientation<span style=\"color: #808030\">[<\/span><span style=\"color: #008c00\">0<\/span><span style=\"color: #808030\">]<\/span> <span style=\"color: #808030\">=<\/span> constrain<span style=\"color: #808030\">(<\/span>orientation<span style=\"color: #808030\">[<\/span><span style=\"color: #008c00\">0<\/span><span style=\"color: #808030\">]<\/span><span style=\"color: #808030\">,<\/span> minPitch<span style=\"color: #808030\">,<\/span> maxPitch<span style=\"color: #808030\">)<\/span><span style=\"color: #800080\">;<\/span>\r\n      orientation<span style=\"color: #808030\">[<\/span><span style=\"color: #008c00\">1<\/span><span style=\"color: #808030\">]<\/span> <span style=\"color: #808030\">=<\/span> constrain<span style=\"color: #808030\">(<\/span>orientation<span style=\"color: #808030\">[<\/span><span style=\"color: #008c00\">1<\/span><span style=\"color: #808030\">]<\/span><span style=\"color: #808030\">,<\/span> minRoll<span style=\"color: #808030\">,<\/span> maxRoll<span style=\"color: #808030\">)<\/span><span style=\"color: #800080\">;<\/span>\r\n      orientation<span style=\"color: #808030\">[<\/span><span style=\"color: #008c00\">2<\/span><span style=\"color: #808030\">]<\/span> <span style=\"color: #808030\">=<\/span> constrain<span style=\"color: #808030\">(<\/span>orientation<span style=\"color: #808030\">[<\/span><span style=\"color: #008c00\">2<\/span><span style=\"color: #808030\">]<\/span><span style=\"color: #808030\">,<\/span> minYaw<span style=\"color: #808030\">,<\/span> maxYaw<span style=\"color: #808030\">)<\/span><span style=\"color: #800080\">;<\/span>\r\n      \r\n      <span style=\"color: #696969\">\/\/Store the current angles for reference in the next loop.<\/span>\r\n      pitchPrevious <span style=\"color: #808030\">=<\/span> orientation<span style=\"color: #808030\">[<\/span><span style=\"color: #008c00\">0<\/span><span style=\"color: #808030\">]<\/span><span style=\"color: #800080\">;<\/span>\r\n      rollPrevious <span style=\"color: #808030\">=<\/span> orientation<span style=\"color: #808030\">[<\/span><span style=\"color: #008c00\">1<\/span><span style=\"color: #808030\">]<\/span><span style=\"color: #800080\">;<\/span>\r\n      \r\n      <span style=\"color: #696969\">\/\/Add together the gyroscpoe data and the user-input from the joystick.<\/span>\r\n      <span style=\"color: #800000;font-weight: bold\">int<\/span> pitchFinal <span style=\"color: #808030\">=<\/span> orientation<span style=\"color: #808030\">[<\/span><span style=\"color: #008c00\">0<\/span><span style=\"color: #808030\">]<\/span> <span style=\"color: #808030\">+<\/span> joystickVertical<span style=\"color: #800080\">;<\/span>\r\n      <span style=\"color: #800000;font-weight: bold\">int<\/span> rollFinal <span style=\"color: #808030\">=<\/span> orientation<span style=\"color: #808030\">[<\/span><span style=\"color: #008c00\">1<\/span><span style=\"color: #808030\">]<\/span><span style=\"color: #800080\">;<\/span>\r\n      <span style=\"color: #800000;font-weight: bold\">int<\/span> yawFinal <span style=\"color: #808030\">=<\/span> yawPrevious<span style=\"color: #800080\">;<\/span>\r\n      \r\n      <span style=\"color: #800000;font-weight: bold\">if<\/span> <span style=\"color: #808030\">(<\/span>yawLocked <span style=\"color: #808030\">=<\/span><span style=\"color: #808030\">=<\/span> <span style=\"color: #800000;font-weight: bold\">false<\/span><span style=\"color: #808030\">)<\/span>\r\n      <span style=\"color: #800080\">{<\/span>\r\n        yawPrevious <span style=\"color: #808030\">=<\/span> orientation<span style=\"color: #808030\">[<\/span><span style=\"color: #008c00\">2<\/span><span style=\"color: #808030\">]<\/span><span style=\"color: #800080\">;<\/span>\r\n      <span style=\"color: #800080\">}<\/span>\r\n      \r\n      yawFinal <span style=\"color: #808030\">=<\/span><span style=\"color: #808030\">+<\/span> joystickHorizontal<span style=\"color: #800080\">;<\/span>\r\n      \r\n      <span style=\"color: #696969\">\/\/Write the final angles to the servos.<\/span>\r\n      servoPitch<span style=\"color: #808030\">.<\/span>write<span style=\"color: #808030\">(<\/span>pitchFinal<span style=\"color: #808030\">)<\/span><span style=\"color: #800080\">;<\/span>\r\n      servoRoll<span style=\"color: #808030\">.<\/span>write<span style=\"color: #808030\">(<\/span>rollFinal<span style=\"color: #808030\">)<\/span><span style=\"color: #800080\">;<\/span>\r\n      servoYaw<span style=\"color: #808030\">.<\/span>write<span style=\"color: #808030\">(<\/span>yawFinal<span style=\"color: #808030\">)<\/span><span style=\"color: #800080\">;<\/span>\r\n      \r\n      <span style=\"color: #696969\">\/\/Print the values to the monitor (for testing).<\/span>\r\n      printToMonitor<span style=\"color: #808030\">(<\/span><span style=\"color: #808030\">)<\/span><span style=\"color: #800080\">;<\/span>\r\n      \r\n      <span style=\"color: #696969\">\/\/Update the timer.<\/span>\r\n      timer <span style=\"color: #808030\">=<\/span> micros<span style=\"color: #808030\">(<\/span><span style=\"color: #808030\">)<\/span><span style=\"color: #800080\">;<\/span>\r\n      \r\n      <span style=\"color: #696969\">\/\/If the camera is focusing, check its timer.<\/span>\r\n      <span style=\"color: #696969\">\/\/If 0.5 seconds has passed, stop the focusing.<\/span>\r\n      <span style=\"color: #800000;font-weight: bold\">if<\/span> <span style=\"color: #808030\">(<\/span>focusing <span style=\"color: #808030\">=<\/span><span style=\"color: #808030\">=<\/span> <span style=\"color: #800000;font-weight: bold\">true<\/span> <span style=\"color: #808030\">&amp;<\/span><span style=\"color: #808030\">&amp;<\/span> <span style=\"color: #808030\">(<\/span>timer <span style=\"color: #808030\">-<\/span> focusTimer<span style=\"color: #808030\">)<\/span> <span style=\"color: #808030\">&gt;<\/span><span style=\"color: #808030\">=<\/span> <span style=\"color: #008c00\">500000<\/span><span style=\"color: #808030\">)<\/span>\r\n      <span style=\"color: #800080\">{<\/span>\r\n         digitalWrite<span style=\"color: #808030\">(<\/span>focusPin<span style=\"color: #808030\">,<\/span> LOW<span style=\"color: #808030\">)<\/span><span style=\"color: #800080\">;<\/span>\r\n         focusing <span style=\"color: #808030\">=<\/span> <span style=\"color: #800000;font-weight: bold\">false<\/span><span style=\"color: #800080\">;<\/span>\r\n      <span style=\"color: #800080\">}<\/span>\r\n      \r\n      <span style=\"color: #696969\">\/\/If the camera is taking a picture, check the timer.<\/span>\r\n      <span style=\"color: #696969\">\/\/If 0.5 seconds has passed, set the shutter signal to low again.<\/span>\r\n      <span style=\"color: #800000;font-weight: bold\">if<\/span> <span style=\"color: #808030\">(<\/span>shuttering <span style=\"color: #808030\">=<\/span><span style=\"color: #808030\">=<\/span> <span style=\"color: #800000;font-weight: bold\">true<\/span> <span style=\"color: #808030\">&amp;<\/span><span style=\"color: #808030\">&amp;<\/span> <span style=\"color: #808030\">(<\/span>timer <span style=\"color: #808030\">-<\/span> shutterTimer<span style=\"color: #808030\">)<\/span> <span style=\"color: #808030\">&gt;<\/span><span style=\"color: #808030\">=<\/span> <span style=\"color: #008c00\">500000<\/span><span style=\"color: #808030\">)<\/span>\r\n      <span style=\"color: #800080\">{<\/span>\r\n         digitalWrite<span style=\"color: #808030\">(<\/span>shutterPin<span style=\"color: #808030\">,<\/span> LOW<span style=\"color: #808030\">)<\/span><span style=\"color: #800080\">;<\/span>\r\n         shuttering <span style=\"color: #808030\">=<\/span> <span style=\"color: #800000;font-weight: bold\">false<\/span><span style=\"color: #800080\">;<\/span>\r\n      <span style=\"color: #800080\">}<\/span>\r\n  <span style=\"color: #800080\">}<\/span>\r\n  \r\n  \r\n  \r\n  <span style=\"color: #696969\">\/*This function reads the values from the 9-axis gyroscope, performs the necessary computations<\/span>\r\n<span style=\"color: #696969\">\u00a0\u00a0to apply the Kamlan-filter to the recieved values, and then maps the final values into the range<\/span>\r\n<span style=\"color: #696969\">\u00a0\u00a0of angles that our servos are compatible for.*\/<\/span>\r\n  <span style=\"color: #800000;font-weight: bold\">void<\/span> readCoordinates<span style=\"color: #808030\">(<\/span><span style=\"color: #800000;font-weight: bold\">int<\/span> orientation<span style=\"color: #808030\">[<\/span><span style=\"color: #808030\">]<\/span><span style=\"color: #808030\">)<\/span>\r\n  <span style=\"color: #800080\">{<\/span>\r\n      <span style=\"color: #696969\">\/\/Some intermediate values to store the raw motion-data.<\/span>\r\n      int16_t ax<span style=\"color: #808030\">,<\/span> ay<span style=\"color: #808030\">,<\/span> az<span style=\"color: #800080\">;<\/span>\r\n      int16_t gx<span style=\"color: #808030\">,<\/span> gy<span style=\"color: #808030\">,<\/span> gz<span style=\"color: #800080\">;<\/span>\r\n      int16_t cx<span style=\"color: #808030\">,<\/span> cy<span style=\"color: #808030\">,<\/span> cz<span style=\"color: #800080\">;<\/span>\r\n      \r\n      <span style=\"color: #800000;font-weight: bold\">int<\/span> pitchGyro<span style=\"color: #800080\">;<\/span>                  <span style=\"color: #696969\">\/\/Stores the mapped gyro-signals for the pitch-angle.<\/span>\r\n      <span style=\"color: #800000;font-weight: bold\">int<\/span> pitchAccel<span style=\"color: #800080\">;<\/span>                 <span style=\"color: #696969\">\/\/Stores the mapped accelerator-signals for the pitch-angle.<\/span>\r\n      <span style=\"color: #800000;font-weight: bold\">int<\/span> rollGyro<span style=\"color: #800080\">;<\/span>                   <span style=\"color: #696969\">\/\/Stores the mapped gyro-signals for the roll-angle.<\/span>\r\n      <span style=\"color: #800000;font-weight: bold\">int<\/span> rollAccel<span style=\"color: #800080\">;<\/span>                  <span style=\"color: #696969\">\/\/Stores the mapped accelerator-signals for the roll-angle.<\/span>\r\n      <span style=\"color: #800000;font-weight: bold\">int<\/span> yawGyro<span style=\"color: #800080\">;<\/span>                    <span style=\"color: #696969\">\/\/Stores the mapped gyro-signals for the yaw-angle.         <\/span>\r\n      <span style=\"color: #800000;font-weight: bold\">int<\/span> yawAccel<span style=\"color: #800080\">;<\/span>                   <span style=\"color: #696969\">\/\/Stores the mapped accelerator-signals for the yaw-angle. -&gt; REMOVE?<\/span>\r\n      <span style=\"color: #800000;font-weight: bold\">int<\/span> yawCompass<span style=\"color: #800080\">;<\/span>                 <span style=\"color: #696969\">\/\/Stores the mapped compass-signals for the yaw-angle.<\/span>\r\n    \r\n      <span style=\"color: #696969\">\/\/Get gyroscope-, accelerometer-, and compass-data.<\/span>\r\n      <span style=\"color: #696969\">\/\/getMotion9 calls getMotion6, then reads and stores the compass-data.<\/span>\r\n      mpu<span style=\"color: #808030\">.<\/span>getMotion9<span style=\"color: #808030\">(<\/span><span style=\"color: #808030\">&amp;<\/span>ax<span style=\"color: #808030\">,<\/span> <span style=\"color: #808030\">&amp;<\/span>ay<span style=\"color: #808030\">,<\/span> <span style=\"color: #808030\">&amp;<\/span>az<span style=\"color: #808030\">,<\/span> <span style=\"color: #808030\">&amp;<\/span>gx<span style=\"color: #808030\">,<\/span> <span style=\"color: #808030\">&amp;<\/span>gy<span style=\"color: #808030\">,<\/span> <span style=\"color: #808030\">&amp;<\/span>gz<span style=\"color: #808030\">,<\/span> <span style=\"color: #808030\">&amp;<\/span>cx<span style=\"color: #808030\">,<\/span> <span style=\"color: #808030\">&amp;<\/span>cy<span style=\"color: #808030\">,<\/span> <span style=\"color: #808030\">&amp;<\/span>cz<span style=\"color: #808030\">)<\/span><span style=\"color: #800080\">;<\/span>\r\n      pitchAccel <span style=\"color: #808030\">=<\/span> <span style=\"color: #603000\">map<\/span><span style=\"color: #808030\">(<\/span>ax<span style=\"color: #808030\">,<\/span> <span style=\"color: #808030\">-<\/span><span style=\"color: #008c00\">17000<\/span><span style=\"color: #808030\">,<\/span> <span style=\"color: #008c00\">17000<\/span><span style=\"color: #808030\">,<\/span> <span style=\"color: #008c00\">0<\/span><span style=\"color: #808030\">,<\/span> <span style=\"color: #008c00\">179<\/span><span style=\"color: #808030\">)<\/span><span style=\"color: #800080\">;<\/span>\r\n      pitchGyro <span style=\"color: #808030\">=<\/span> <span style=\"color: #603000\">map<\/span><span style=\"color: #808030\">(<\/span>gx<span style=\"color: #808030\">,<\/span> <span style=\"color: #808030\">-<\/span><span style=\"color: #008c00\">33000<\/span><span style=\"color: #808030\">,<\/span> <span style=\"color: #008c00\">33000<\/span><span style=\"color: #808030\">,<\/span> <span style=\"color: #008c00\">0<\/span><span style=\"color: #808030\">,<\/span> <span style=\"color: #008c00\">179<\/span><span style=\"color: #808030\">)<\/span><span style=\"color: #800080\">;<\/span>\r\n      \r\n      rollAccel <span style=\"color: #808030\">=<\/span> <span style=\"color: #603000\">map<\/span><span style=\"color: #808030\">(<\/span>ay<span style=\"color: #808030\">,<\/span> <span style=\"color: #808030\">-<\/span><span style=\"color: #008c00\">17000<\/span><span style=\"color: #808030\">,<\/span> <span style=\"color: #008c00\">17000<\/span><span style=\"color: #808030\">,<\/span> <span style=\"color: #008c00\">179<\/span><span style=\"color: #808030\">,<\/span> <span style=\"color: #008c00\">0<\/span><span style=\"color: #808030\">)<\/span><span style=\"color: #800080\">;<\/span>\r\n      rollGyro <span style=\"color: #808030\">=<\/span> <span style=\"color: #603000\">map<\/span><span style=\"color: #808030\">(<\/span>gy<span style=\"color: #808030\">,<\/span> <span style=\"color: #808030\">-<\/span><span style=\"color: #008c00\">33000<\/span><span style=\"color: #808030\">,<\/span> <span style=\"color: #008c00\">33000<\/span><span style=\"color: #808030\">,<\/span> <span style=\"color: #008c00\">179<\/span><span style=\"color: #808030\">,<\/span> <span style=\"color: #008c00\">0<\/span><span style=\"color: #808030\">)<\/span><span style=\"color: #800080\">;<\/span>\r\n      \r\n      <span style=\"color: #696969\">\/\/Accellerator does not measure yaw.<\/span>\r\n      yawGyro <span style=\"color: #808030\">=<\/span> <span style=\"color: #603000\">map<\/span><span style=\"color: #808030\">(<\/span>gz<span style=\"color: #808030\">,<\/span> <span style=\"color: #808030\">-<\/span><span style=\"color: #008c00\">17000<\/span><span style=\"color: #808030\">,<\/span> <span style=\"color: #008c00\">17000<\/span><span style=\"color: #808030\">,<\/span> <span style=\"color: #008c00\">0<\/span><span style=\"color: #808030\">,<\/span> <span style=\"color: #008c00\">179<\/span><span style=\"color: #808030\">)<\/span><span style=\"color: #800080\">;<\/span>\r\n      <span style=\"color: #696969\">\/\/yawCompass = map(cz, -33000, 33000, 0, 179);<\/span>\r\n      \r\n      <span style=\"color: #696969\">\/\/Apply the complimentary filter.<\/span>\r\n      <span style=\"color: #696969\">\/\/As the accellerometer does not measure the yaw\/z-axis movement, we instead utilize the compass data.<\/span>\r\n      orientation<span style=\"color: #808030\">[<\/span><span style=\"color: #008c00\">0<\/span><span style=\"color: #808030\">]<\/span> <span style=\"color: #808030\">=<\/span> <span style=\"color: #808030\">(<\/span>percentGyro<span style=\"color: #808030\">*<\/span><span style=\"color: #808030\">(<\/span>pitchPrevious<span style=\"color: #808030\">+<\/span><span style=\"color: #808030\">(<\/span>pitchGyro<span style=\"color: #808030\">*<\/span><span style=\"color: #808030\">(<\/span><span style=\"color: #800000;font-weight: bold\">double<\/span><span style=\"color: #808030\">)<\/span><span style=\"color: #808030\">(<\/span>micros<span style=\"color: #808030\">(<\/span><span style=\"color: #808030\">)<\/span><span style=\"color: #808030\">-<\/span>timer<span style=\"color: #808030\">)<\/span><span style=\"color: #808030\">\/<\/span><span style=\"color: #008c00\">1000000<\/span><span style=\"color: #808030\">)<\/span><span style=\"color: #808030\">)<\/span><span style=\"color: #808030\">)<\/span><span style=\"color: #808030\">+<\/span><span style=\"color: #808030\">(<\/span>percentAcc<span style=\"color: #808030\">*<\/span>pitchAccel<span style=\"color: #808030\">)<\/span><span style=\"color: #800080\">;<\/span>\r\n      orientation<span style=\"color: #808030\">[<\/span><span style=\"color: #008c00\">1<\/span><span style=\"color: #808030\">]<\/span> <span style=\"color: #808030\">=<\/span> <span style=\"color: #808030\">(<\/span>percentGyro<span style=\"color: #808030\">*<\/span><span style=\"color: #808030\">(<\/span>rollPrevious<span style=\"color: #808030\">+<\/span><span style=\"color: #808030\">(<\/span>rollGyro<span style=\"color: #808030\">*<\/span><span style=\"color: #808030\">(<\/span><span style=\"color: #800000;font-weight: bold\">double<\/span><span style=\"color: #808030\">)<\/span><span style=\"color: #808030\">(<\/span>micros<span style=\"color: #808030\">(<\/span><span style=\"color: #808030\">)<\/span><span style=\"color: #808030\">-<\/span>timer<span style=\"color: #808030\">)<\/span><span style=\"color: #808030\">\/<\/span><span style=\"color: #008c00\">1000000<\/span><span style=\"color: #808030\">)<\/span><span style=\"color: #808030\">)<\/span><span style=\"color: #808030\">)<\/span><span style=\"color: #808030\">+<\/span><span style=\"color: #808030\">(<\/span>percentAcc<span style=\"color: #808030\">*<\/span>rollAccel<span style=\"color: #808030\">)<\/span><span style=\"color: #800080\">;<\/span>\r\n      orientation<span style=\"color: #808030\">[<\/span><span style=\"color: #008c00\">2<\/span><span style=\"color: #808030\">]<\/span> <span style=\"color: #808030\">=<\/span>  <span style=\"color: #808030\">(<\/span>yawPrevious<span style=\"color: #808030\">+<\/span><span style=\"color: #808030\">(<\/span>yawGyro<span style=\"color: #808030\">*<\/span><span style=\"color: #808030\">(<\/span><span style=\"color: #800000;font-weight: bold\">double<\/span><span style=\"color: #808030\">)<\/span><span style=\"color: #808030\">(<\/span>micros<span style=\"color: #808030\">(<\/span><span style=\"color: #808030\">)<\/span><span style=\"color: #808030\">-<\/span>timer<span style=\"color: #808030\">)<\/span><span style=\"color: #808030\">\/<\/span><span style=\"color: #008c00\">1000000<\/span><span style=\"color: #808030\">)<\/span><span style=\"color: #808030\">)<\/span><span style=\"color: #808030\">-<\/span><span style=\"color: #008000\">1.65<\/span><span style=\"color: #800080\">;<\/span>\r\n  <span style=\"color: #800080\">}<\/span>\r\n  \r\n  \r\n  <span style=\"color: #696969\">\/*Simply reads the position of the joystick and alters the signal to makes sure it corresponds to the physical position.<\/span>\r\n<span style=\"color: #696969\">\u00a0\u00a0Also makes sure the joystick can't steer the servo past its set maximum angle.*\/<\/span>\r\n  <span style=\"color: #800000;font-weight: bold\">void<\/span> readJoystick<span style=\"color: #808030\">(<\/span><span style=\"color: #808030\">)<\/span>\r\n  <span style=\"color: #800080\">{<\/span>\r\n      <span style=\"color: #696969\">\/\/Read the joystick-position and alter it. +\/-3 is to make 0 its signal for the resting position.<\/span>\r\n      <span style=\"color: #696969\">\/\/ -512 to provide equal +\/- numbers. Ignore the signal if the maximum angle has been reached.<\/span>\r\n      <span style=\"color: #800000;font-weight: bold\">float<\/span> stickYposition <span style=\"color: #808030\">=<\/span> <span style=\"color: #808030\">(<\/span><span style=\"color: #808030\">(<\/span>analogRead<span style=\"color: #808030\">(<\/span>joystickVerticalPin<span style=\"color: #808030\">)<\/span> <span style=\"color: #808030\">-<\/span> <span style=\"color: #008c00\">512<\/span><span style=\"color: #808030\">)<\/span> <span style=\"color: #808030\">\/<\/span> <span style=\"color: #808030\">-<\/span><span style=\"color: #008c00\">50<\/span><span style=\"color: #808030\">)<\/span><span style=\"color: #808030\">*<\/span><span style=\"color: #008000\">0.080<\/span><span style=\"color: #800080\">;<\/span><span style=\"color: #3f5fbf\">\/\/\/5;<\/span>\r\n      <span style=\"color: #800000;font-weight: bold\">float<\/span> stickXposition <span style=\"color: #808030\">=<\/span> <span style=\"color: #808030\">(<\/span><span style=\"color: #808030\">(<\/span>analogRead<span style=\"color: #808030\">(<\/span>joystickHorizontalPin<span style=\"color: #808030\">)<\/span> <span style=\"color: #808030\">-<\/span> <span style=\"color: #008c00\">512<\/span><span style=\"color: #808030\">)<\/span> <span style=\"color: #808030\">\/<\/span> <span style=\"color: #008c00\">50<\/span><span style=\"color: #808030\">)<\/span><span style=\"color: #808030\">*<\/span><span style=\"color: #008000\">0.04<\/span><span style=\"color: #800080\">;<\/span><span style=\"color: #3f5fbf\">\/\/\/5; <\/span>\r\n      \r\n      <span style=\"color: #800000;font-weight: bold\">if<\/span> <span style=\"color: #808030\">(<\/span><span style=\"color: #808030\">(<\/span>pitchPrevious <span style=\"color: #808030\">+<\/span> joystickVertical <span style=\"color: #808030\">+<\/span> stickYposition<span style=\"color: #808030\">)<\/span> <span style=\"color: #808030\">&gt;<\/span><span style=\"color: #808030\">=<\/span> maxPitch<span style=\"color: #808030\">)<\/span>\r\n      <span style=\"color: #800080\">{<\/span>\r\n        stickYposition <span style=\"color: #808030\">=<\/span> <span style=\"color: #008c00\">0<\/span><span style=\"color: #800080\">;<\/span>\r\n        joystickVertical <span style=\"color: #808030\">-<\/span><span style=\"color: #808030\">=<\/span> <span style=\"color: #008000\">0.4<\/span><span style=\"color: #800080\">;<\/span>\r\n      <span style=\"color: #800080\">}<\/span>\r\n      <span style=\"color: #800000;font-weight: bold\">else<\/span> <span style=\"color: #800000;font-weight: bold\">if<\/span> <span style=\"color: #808030\">(<\/span><span style=\"color: #808030\">(<\/span>pitchPrevious <span style=\"color: #808030\">+<\/span> joystickVertical <span style=\"color: #808030\">+<\/span> stickYposition<span style=\"color: #808030\">)<\/span> <span style=\"color: #808030\">&lt;<\/span><span style=\"color: #808030\">=<\/span> <span style=\"color: #808030\">(<\/span>minPitch<span style=\"color: #808030\">)<\/span><span style=\"color: #808030\">)<\/span>\r\n      <span style=\"color: #800080\">{<\/span>\r\n        stickYposition <span style=\"color: #808030\">=<\/span> <span style=\"color: #008c00\">0<\/span><span style=\"color: #800080\">;<\/span>\r\n        joystickVertical <span style=\"color: #808030\">+<\/span><span style=\"color: #808030\">=<\/span> <span style=\"color: #008000\">0.4<\/span><span style=\"color: #800080\">;<\/span>\r\n      <span style=\"color: #800080\">}<\/span>\r\n      \r\n      <span style=\"color: #800000;font-weight: bold\">if<\/span> <span style=\"color: #808030\">(<\/span><span style=\"color: #808030\">(<\/span>yawPrevious <span style=\"color: #808030\">+<\/span> joystickHorizontal <span style=\"color: #808030\">+<\/span> stickXposition<span style=\"color: #808030\">)<\/span> <span style=\"color: #808030\">&gt;<\/span><span style=\"color: #808030\">=<\/span> maxYaw<span style=\"color: #808030\">)<\/span>\r\n      <span style=\"color: #800080\">{<\/span>\r\n        stickXposition <span style=\"color: #808030\">=<\/span> <span style=\"color: #008c00\">0<\/span><span style=\"color: #800080\">;<\/span>\r\n        joystickHorizontal <span style=\"color: #808030\">-<\/span><span style=\"color: #808030\">=<\/span> <span style=\"color: #008000\">0.4<\/span><span style=\"color: #800080\">;<\/span>\r\n      <span style=\"color: #800080\">}<\/span>\r\n      <span style=\"color: #800000;font-weight: bold\">else<\/span> <span style=\"color: #800000;font-weight: bold\">if<\/span> <span style=\"color: #808030\">(<\/span><span style=\"color: #808030\">(<\/span>yawPrevious <span style=\"color: #808030\">+<\/span> joystickHorizontal <span style=\"color: #808030\">+<\/span> stickXposition<span style=\"color: #808030\">)<\/span> <span style=\"color: #808030\">&lt;<\/span><span style=\"color: #808030\">=<\/span> <span style=\"color: #808030\">(<\/span>minYaw<span style=\"color: #808030\">)<\/span><span style=\"color: #808030\">)<\/span>\r\n      <span style=\"color: #800080\">{<\/span>\r\n        stickXposition <span style=\"color: #808030\">=<\/span> <span style=\"color: #008c00\">0<\/span><span style=\"color: #800080\">;<\/span>\r\n        joystickHorizontal <span style=\"color: #808030\">+<\/span><span style=\"color: #808030\">=<\/span> <span style=\"color: #008000\">0.4<\/span><span style=\"color: #800080\">;<\/span>\r\n      <span style=\"color: #800080\">}<\/span>\r\n      \r\n      <span style=\"color: #696969\">\/\/Add the input to the offset caused by the joystick.<\/span>\r\n      joystickVertical <span style=\"color: #808030\">+<\/span><span style=\"color: #808030\">=<\/span> stickYposition<span style=\"color: #800080\">;<\/span>\r\n      joystickHorizontal <span style=\"color: #808030\">+<\/span><span style=\"color: #808030\">=<\/span> stickXposition<span style=\"color: #800080\">;<\/span>\r\n  <span style=\"color: #800080\">}<\/span>\r\n  \r\n  <span style=\"color: #696969\">\/\/Resets the gimbals zero-angle position based on the current one.Might need to use accelerator-data here.<\/span>\r\n  <span style=\"color: #696969\">\/\/Maybe change this and use it for initializing the position on start-up as well.<\/span>\r\n  <span style=\"color: #800000;font-weight: bold\">void<\/span> reset<span style=\"color: #808030\">(<\/span><span style=\"color: #808030\">)<\/span>\r\n  <span style=\"color: #800080\">{<\/span>\r\n      joystickVertical <span style=\"color: #808030\">=<\/span> <span style=\"color: #008c00\">0<\/span><span style=\"color: #800080\">;<\/span>\r\n      joystickHorizontal <span style=\"color: #808030\">=<\/span> <span style=\"color: #008c00\">0<\/span><span style=\"color: #800080\">;<\/span>\r\n      servoPitch<span style=\"color: #808030\">.<\/span>write<span style=\"color: #808030\">(<\/span>zeroPitch<span style=\"color: #808030\">)<\/span><span style=\"color: #800080\">;<\/span>\r\n      servoRoll<span style=\"color: #808030\">.<\/span>write<span style=\"color: #808030\">(<\/span>zeroRoll<span style=\"color: #808030\">)<\/span><span style=\"color: #800080\">;<\/span>\r\n      servoYaw<span style=\"color: #808030\">.<\/span>write<span style=\"color: #808030\">(<\/span>zeroYaw<span style=\"color: #808030\">)<\/span><span style=\"color: #800080\">;<\/span>\r\n      pitchPrevious <span style=\"color: #808030\">=<\/span> zeroPitch<span style=\"color: #800080\">;<\/span>\r\n      rollPrevious <span style=\"color: #808030\">=<\/span> zeroRoll<span style=\"color: #800080\">;<\/span>\r\n      yawPrevious <span style=\"color: #808030\">=<\/span> zeroYaw<span style=\"color: #800080\">;<\/span>\r\n  <span style=\"color: #800080\">}<\/span>\r\n  \r\n  <span style=\"color: #696969\">\/\/Interupt routine for the camera-focus.<\/span>\r\n  <span style=\"color: #800000;font-weight: bold\">void<\/span> focus<span style=\"color: #808030\">(<\/span><span style=\"color: #808030\">)<\/span>\r\n  <span style=\"color: #800080\">{<\/span>\r\n    <span style=\"color: #800000;font-weight: bold\">if<\/span> <span style=\"color: #808030\">(<\/span>digitalRead<span style=\"color: #808030\">(<\/span>joyButtonPin<span style=\"color: #808030\">)<\/span> <span style=\"color: #808030\">=<\/span><span style=\"color: #808030\">=<\/span> HIGH<span style=\"color: #808030\">)<\/span>\r\n    <span style=\"color: #800080\">{<\/span>\r\n      digitalWrite<span style=\"color: #808030\">(<\/span>focusPin<span style=\"color: #808030\">,<\/span> HIGH<span style=\"color: #808030\">)<\/span><span style=\"color: #800080\">;<\/span>\r\n      focusing <span style=\"color: #808030\">=<\/span> <span style=\"color: #800000;font-weight: bold\">true<\/span><span style=\"color: #800080\">;<\/span>\r\n      focusTimer <span style=\"color: #808030\">=<\/span> micros<span style=\"color: #808030\">(<\/span><span style=\"color: #808030\">)<\/span><span style=\"color: #800080\">;<\/span>\r\n    <span style=\"color: #800080\">}<\/span>\r\n    <span style=\"color: #800000;font-weight: bold\">else<\/span>\r\n    <span style=\"color: #800080\">{<\/span>\r\n      reset<span style=\"color: #808030\">(<\/span><span style=\"color: #808030\">)<\/span><span style=\"color: #800080\">;<\/span>\r\n    <span style=\"color: #800080\">}<\/span>\r\n  <span style=\"color: #800080\">}<\/span>\r\n  \r\n  <span style=\"color: #696969\">\/\/Interupt routine for the camera-shutter.<\/span>\r\n  <span style=\"color: #800000;font-weight: bold\">void<\/span> shutter<span style=\"color: #808030\">(<\/span><span style=\"color: #808030\">)<\/span>\r\n  <span style=\"color: #800080\">{<\/span>\r\n    <span style=\"color: #800000;font-weight: bold\">if<\/span> <span style=\"color: #808030\">(<\/span>digitalRead<span style=\"color: #808030\">(<\/span>joyButtonPin<span style=\"color: #808030\">)<\/span> <span style=\"color: #808030\">=<\/span><span style=\"color: #808030\">=<\/span> HIGH<span style=\"color: #808030\">)<\/span>\r\n    <span style=\"color: #800080\">{<\/span>\r\n      digitalWrite<span style=\"color: #808030\">(<\/span>shutterPin<span style=\"color: #808030\">,<\/span> HIGH<span style=\"color: #808030\">)<\/span><span style=\"color: #800080\">;<\/span>\r\n      shuttering <span style=\"color: #808030\">=<\/span> <span style=\"color: #800000;font-weight: bold\">true<\/span><span style=\"color: #800080\">;<\/span>\r\n      shutterTimer <span style=\"color: #808030\">=<\/span> micros<span style=\"color: #808030\">(<\/span><span style=\"color: #808030\">)<\/span><span style=\"color: #800080\">;<\/span>\r\n    <span style=\"color: #800080\">}<\/span>\r\n    <span style=\"color: #800000;font-weight: bold\">else<\/span>\r\n    <span style=\"color: #800080\">{<\/span>\r\n      yawLocked <span style=\"color: #808030\">=<\/span> <span style=\"color: #808030\">!<\/span>yawLocked<span style=\"color: #800080\">;<\/span>\r\n    <span style=\"color: #800080\">}<\/span>\r\n\r\n  <span style=\"color: #800080\">}<\/span>\r\n  \r\n  <span style=\"color: #696969\">\/\/Prints values to the screen. For testing and monitoring.<\/span>\r\n  <span style=\"color: #800000;font-weight: bold\">void<\/span> printToMonitor<span style=\"color: #808030\">(<\/span><span style=\"color: #808030\">)<\/span>\r\n  <span style=\"color: #800080\">{<\/span>\r\n      Serial<span style=\"color: #808030\">.<\/span>print<span style=\"color: #808030\">(<\/span><span style=\"color: #800000\">\"<\/span><span style=\"color: #0000e6\">Pitch:<\/span><span style=\"color: #800000\">\"<\/span><span style=\"color: #808030\">)<\/span><span style=\"color: #800080\">;<\/span>\r\n      Serial<span style=\"color: #808030\">.<\/span>print<span style=\"color: #808030\">(<\/span>pitchPrevious<span style=\"color: #808030\">)<\/span><span style=\"color: #800080\">;<\/span>\r\n      Serial<span style=\"color: #808030\">.<\/span>print<span style=\"color: #808030\">(<\/span><span style=\"color: #800000\">\"<\/span><span style=\"color: #0f69ff\">\\t<\/span><span style=\"color: #0000e6\">Roll:<\/span><span style=\"color: #800000\">\"<\/span><span style=\"color: #808030\">)<\/span><span style=\"color: #800080\">;<\/span>\r\n      Serial<span style=\"color: #808030\">.<\/span>print<span style=\"color: #808030\">(<\/span>rollPrevious<span style=\"color: #808030\">)<\/span><span style=\"color: #800080\">;<\/span>\r\n      Serial<span style=\"color: #808030\">.<\/span>print<span style=\"color: #808030\">(<\/span><span style=\"color: #800000\">\"<\/span><span style=\"color: #0f69ff\">\\t<\/span><span style=\"color: #0000e6\">Yaw:<\/span><span style=\"color: #800000\">\"<\/span><span style=\"color: #808030\">)<\/span><span style=\"color: #800080\">;<\/span>\r\n      Serial<span style=\"color: #808030\">.<\/span>println<span style=\"color: #808030\">(<\/span>yawPrevious<span style=\"color: #808030\">)<\/span><span style=\"color: #800080\">;<\/span>\r\n  <span style=\"color: #800080\">}<\/span>\r\n<\/pre>\n<\/div>\n","protected":false},"excerpt":{"rendered":"<p>Over the course of the last few weeks, we have found a solution for focusing the camera and take pictures with the two buttons on the handle, these uses an interrupt function to not disturb the rest of the loop with delays. While working on this we found out we needed two more buttons, but [&hellip;]<\/p>\n","protected":false},"author":29,"featured_media":828,"comment_status":"open","ping_status":"open","sticky":false,"template":"","format":"standard","meta":{"footnotes":""},"categories":[14],"tags":[],"class_list":["post-721","post","type-post","status-publish","format-standard","has-post-thumbnail","hentry","category-smart-systems-2014-dslr-gimball"],"_links":{"self":[{"href":"https:\/\/dronesonen.usn.no\/index.php?rest_route=\/wp\/v2\/posts\/721","targetHints":{"allow":["GET"]}}],"collection":[{"href":"https:\/\/dronesonen.usn.no\/index.php?rest_route=\/wp\/v2\/posts"}],"about":[{"href":"https:\/\/dronesonen.usn.no\/index.php?rest_route=\/wp\/v2\/types\/post"}],"author":[{"embeddable":true,"href":"https:\/\/dronesonen.usn.no\/index.php?rest_route=\/wp\/v2\/users\/29"}],"replies":[{"embeddable":true,"href":"https:\/\/dronesonen.usn.no\/index.php?rest_route=%2Fwp%2Fv2%2Fcomments&post=721"}],"version-history":[{"count":17,"href":"https:\/\/dronesonen.usn.no\/index.php?rest_route=\/wp\/v2\/posts\/721\/revisions"}],"predecessor-version":[{"id":826,"href":"https:\/\/dronesonen.usn.no\/index.php?rest_route=\/wp\/v2\/posts\/721\/revisions\/826"}],"wp:featuredmedia":[{"embeddable":true,"href":"https:\/\/dronesonen.usn.no\/index.php?rest_route=\/wp\/v2\/media\/828"}],"wp:attachment":[{"href":"https:\/\/dronesonen.usn.no\/index.php?rest_route=%2Fwp%2Fv2%2Fmedia&parent=721"}],"wp:term":[{"taxonomy":"category","embeddable":true,"href":"https:\/\/dronesonen.usn.no\/index.php?rest_route=%2Fwp%2Fv2%2Fcategories&post=721"},{"taxonomy":"post_tag","embeddable":true,"href":"https:\/\/dronesonen.usn.no\/index.php?rest_route=%2Fwp%2Fv2%2Ftags&post=721"}],"curies":[{"name":"wp","href":"https:\/\/api.w.org\/{rel}","templated":true}]}}