Advertisement
kr0k0f4nt

Arduino Nano - Bayangtoys X21 Gimbal Control

Sep 20th, 2017
1,411
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C 7.36 KB | None | 0 0
  1. /*  
  2.  *  Bayangtoys X21 BGC Gimbal Control for Ardunio
  3.  *  by kr0k0f4nt (2017) - Version 1.1
  4.  *  
  5.  *  Version 1.1 (2017-09-20)
  6.  *  - Added SPEED to SETTINGS to Control MODE 1
  7.  *  - Pressing the Video Button will now always set next Movement on Photo Button to upwards
  8.  *  
  9.  *  Version 1.0 (2017-09-19)
  10.  *  - Initial Release
  11.  *  
  12.  *  PPM Processing based on Code by Sijjim (https://forum.arduino.cc/index.php?topic=182681.0)
  13.  *  Inspired by Muhammad Imam Zulkarnaen (https://www.youtube.com/watch?v=pYitT60Frjc)
  14.  */
  15.  
  16. // *********************** SETTINGS ****************************
  17. /*
  18.  * Angle can be 0-2000, but only Values between 1500-2000 make sense
  19.  * 1000 = Camera looks straight up to the drone body, lower values make it look backwards
  20.  * 1500 = Neutal Setting looking straight forward
  21.  * 2000 = Cameras looks straight down
  22.  */
  23. #define MAX           2000     // Max Angle
  24. #define STD           1650     // Standard Angle for MODE 1
  25. #define MIN           1500     // Min Angle
  26. #define STEP          25       // Steps for MODE 2, 500 = 90 degrees, so 25 is about 5 degrees per Step
  27. #define SPEED         5        // Speed for MODE 1 when pressing Photo Button, 1 is slowest to 10 fastest
  28.  
  29. /*
  30.  * Interrputs are not available on all Pins, for the Nano only on 2 & 3
  31.  */
  32. #define PPM_PIN       2        // PIN with X21 Signal
  33.  
  34. /*
  35.  * This only needs to be a Pin that is capable of PWM, for the Nano this is 3,5-6,9-11
  36.  */
  37. #define GIMBAL_PIN    6       // PIN with Gimbal Signal
  38.  
  39. /*
  40.  * Setting Up Pin Pair for MODE Control
  41.  */
  42. #define MODE_PIN_OUT  7        // MODE Pin Output
  43. #define MODE_PIN_IN   8        // MODE Pin Input
  44. #define MODE_1_FIXED  1        // Mode for fixed Angles on Video Button
  45. #define MODE_2_STEP   2        // Mode for Stepping Angles video Video (Down) and Photo Up
  46.  
  47. /*
  48.  * This will set the number of channels in the PPM signal
  49.  */
  50.  
  51. #define CHANNELS        3         // X21 works with 3 Channels
  52. #define CHANNEL_SIGNAL  3         // X21 Signal is on Channel 3
  53.  
  54. /*
  55.  * These are the Signal average Values for the Channel 3, which varies on the Video/Photo Buttons being pressed
  56.  */
  57.  
  58. #define SIGNAL_BASE    500
  59. #define SIGNAL_PHOTO   1100
  60. #define SIGNAL_VIDEO   1600
  61.  
  62. // ****************** GLOBAL VARIABLES ******************
  63.  
  64. #include <Servo.h>
  65. Servo Gimbal;
  66.  
  67. // State Variables for handling Signal toggles
  68. boolean VideoMode = false;
  69. int LastSignal = SIGNAL_BASE;
  70.  
  71. int GimbalMode = 0;
  72. int GimbalState = STD;
  73.  
  74. // Variables for MODE 1 Control by Photo Button
  75. int GimbalSteps = 0;
  76. int GimbalLastSteps = SPEED;
  77.  
  78. // Variables for PPM Processing
  79. volatile int Values[CHANNELS + 1] = {0};
  80.  
  81. // *****************************************************
  82.  
  83. void setup() {
  84.   // Serial Communcication Output
  85.   Serial.begin(115200);
  86.  
  87.   // Settings up MODE pins
  88.   pinMode(PPM_PIN, INPUT);
  89.   pinMode(MODE_PIN_OUT, OUTPUT);
  90.   pinMode(MODE_PIN_IN, INPUT_PULLUP);
  91.   digitalWrite(MODE_PIN_OUT, LOW);
  92.  
  93.   // Setting up Gimbal
  94.   Gimbal.attach(GIMBAL_PIN);
  95.   Gimbal.writeMicroseconds(GimbalState);
  96. }
  97.  
  98. void loop(){
  99.  
  100.   // MODE Selection
  101.   if (digitalRead(MODE_PIN_IN) == HIGH) {
  102.     if (GimbalMode != MODE_1_FIXED) {
  103.       GimbalMode = MODE_1_FIXED;
  104.       Serial.println("MODE 1 - Fixed Angles");
  105.     }
  106.   } else {
  107.     if (GimbalMode != MODE_2_STEP) {
  108.       GimbalMode = MODE_2_STEP;
  109.       Serial.println("MODE 2 - Stepping Angles");
  110.     }
  111.   }
  112.  
  113.   // Wait for Sync on Signal
  114.   while(pulseIn(PPM_PIN, HIGH) < 2500){}
  115.  
  116.   // Processing PPM Signal
  117.   for (int Channel = 1; Channel <= CHANNELS; Channel++) {
  118.     Values[Channel] = pulseIn(PPM_PIN, HIGH);
  119.   }
  120.  
  121.   // Determinig the Sigal for easier handling
  122.   int Signal = Values[CHANNEL_SIGNAL];
  123.   if (Signal <= 1800 && Signal >= 1500) {
  124.     Signal = SIGNAL_VIDEO;
  125.   } else if (Signal <= 1200 && Signal >= 900) {
  126.     Signal = SIGNAL_PHOTO;
  127.   } else {
  128.     Signal = SIGNAL_BASE;
  129.   }
  130.  
  131.  
  132.   // Only do something whenever Signal changes
  133.   if (LastSignal != Signal) {
  134.    
  135.     /*
  136.      *  Figuring out which Key was actually pressed ...
  137.      *  This is a bit tricky as there are only 3 Signal States and the SIGNAL_BASE is shared by both Buttons
  138.      *  To solve this we need to maintain the State of the VideoMode as well as the LastSignal.
  139.     */
  140.    
  141.     int KeyPressed;
  142.    
  143.     switch(Signal) {
  144.       case SIGNAL_VIDEO:
  145.         if (VideoMode) {
  146.           VideoMode = false;
  147.           KeyPressed = SIGNAL_VIDEO;
  148.         } else {
  149.           VideoMode = true;
  150.           KeyPressed = SIGNAL_VIDEO;  
  151.         }
  152.         break;
  153.       case SIGNAL_PHOTO:
  154.         KeyPressed = SIGNAL_PHOTO;
  155.         break;
  156.       case SIGNAL_BASE:
  157.         if (VideoMode == true && LastSignal == SIGNAL_VIDEO) {
  158.           VideoMode = false;
  159.           KeyPressed = SIGNAL_VIDEO;
  160.         } else if (VideoMode == false && LastSignal == SIGNAL_VIDEO) {
  161.           VideoMode = true;
  162.           KeyPressed = SIGNAL_VIDEO;
  163.         } else {
  164.           KeyPressed = SIGNAL_PHOTO;
  165.         }
  166.         break;
  167.     }
  168.  
  169.     DebugPrintStates(KeyPressed, Signal, VideoMode);
  170.  
  171.     if (GimbalMode == MODE_1_FIXED) {
  172.       // MODE 1 - Using Fixed Values on Video Button / Toggle Control on Photo
  173.  
  174.       // Toggle between Standard and Maximum Angle with Video Mode
  175.       if (KeyPressed == SIGNAL_VIDEO) {
  176.         // Stop current movement and set next direction to upwards
  177.         GimbalSteps = 0;
  178.         GimbalLastSteps = SPEED;
  179.         // Video Mode represents the Angles on the Remote, it toggels between MAX and STD Settings
  180.         if (VideoMode) {
  181.           GimbalState = MAX;
  182.         } else {            
  183.           GimbalState = STD;
  184.         }
  185.       } else {
  186.         // Photo Button controls the Gimbal by starting or stopping the motion and cycle direction each time
  187.         if (GimbalSteps != 0) {
  188.           // Gimbal is currently moving ... Stop now!
  189.           GimbalSteps = 0;
  190.         } else {
  191.           // Gimbal is stopped ... Start moving and switch direction!
  192.           GimbalSteps = GimbalLastSteps * (-1);
  193.           GimbalLastSteps = GimbalSteps;
  194.         }
  195.       }
  196.      
  197.     } else {
  198.       // MODE 2 - Using Photo Button for Step Up / Video Button for Down
  199.  
  200.       if (KeyPressed == SIGNAL_VIDEO) {
  201.         // Moving the Gimbal downwards ...
  202.         if (GimbalState + STEP <= MAX) {
  203.           GimbalState += STEP;
  204.         } else {
  205.           GimbalState = MAX;
  206.         }
  207.       } else {
  208.         // Moving the Gimbal upwards ...
  209.         if (GimbalState - STEP >= MIN) {
  210.           GimbalState -= STEP;
  211.         } else {
  212.           GimbalState = MIN;
  213.         }
  214.       }
  215.        
  216.     }
  217.  
  218.     // Overwrite Last Signal
  219.     LastSignal = Signal;
  220.    
  221.   }
  222.  
  223.   // Write GimbalState but maintain MIN/MAX Angles and stop movment if exceeded
  224.   if (GimbalState + GimbalSteps > MAX) {
  225.     GimbalState = MAX;
  226.     GimbalSteps = 0;
  227.   } else if (GimbalState + GimbalSteps < MIN) {
  228.     GimbalState = MIN;
  229.     GimbalSteps = 0;
  230.   } else {
  231.     Gimbal.writeMicroseconds(GimbalState += GimbalSteps);
  232.   }
  233.  
  234. }
  235.  
  236. void DebugPrintStates(int KeyPressed, int Signal, boolean VideoMode) {
  237.     Serial.print("Key pressed: ");
  238.     if (KeyPressed == SIGNAL_VIDEO) {
  239.       Serial.print("VIDEO - ");
  240.       Serial.println(Signal);
  241.     } else {
  242.       Serial.print("PHOTO - ");
  243.       Serial.println(Signal);
  244.     }
  245.     Serial.print("Video Mode: ");
  246.     if (VideoMode) {
  247.       Serial.println("ON");
  248.     } else {
  249.       Serial.println("OFF");
  250.     }  
  251. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement