SHARE
TWEET

Shadow_Q85.ino

joymonkey Feb 19th, 2017 225 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. // =======================================================================================
  2. //                 SHADOW :  Small Handheld Arduino Droid Operating Wand
  3. // =======================================================================================
  4. //                          Last Revised Date: 10/05/14
  5. //                             Written By: KnightShade
  6. //                        Inspired by the PADAWAN by danf
  7. // Extra! Extra! Now with completely untested R/C output mode intended for Q85 guinea pigs
  8. //                           (Q85 stuff added by Paul Murphy, mostly stolen from Brad/BHD)
  9. // =======================================================================================
  10. //
  11. //         This program is free software: you can redistribute it and/or modify it .
  12. //         This program is distributed in the hope that it will be useful,
  13. //         but WITHOUT ANY WARRANTY; without even the implied warranty of
  14. //         MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  15. //
  16. // =======================================================================================
  17. //   Note: You will need a Arduino Mega 1280/2560 to run this sketch,
  18. //   as a normal Arduino (Uno, Duemilanove etc.) doesn't have enough SRAM and FLASH
  19. //
  20. //   This is written to be a UNIVERSAL Sketch - supporting multiple controller options
  21. //      - Single PS3 Move Navigation
  22. //      - Pair of PS3 Move Navigation
  23. //      - Android Phone (Limited Controls)
  24. //      Future Planned Enhancements:
  25. //      - XBox 360 Controller  (Why not, these also uses the USB Host Shield)
  26. //      - PS3 Dual Shock Controller
  27. //      - PS4 Dual Shock Controller
  28. //
  29. //   PS3 Bluetooth library - developed by Kristian Lauszus (kristianl@tkjelectronics.com)
  30. //   For more information visit my blog: http://blog.tkjelectronics.dk/ or
  31. //
  32. //   Holoprojector Support:
  33. //      Legacy Holoprojector Support:  was based on Padawan, using a second Arduino (Teeces)
  34. //          This used a Padawan Dome sketch that was loaded to the Teeces Logics.
  35. //          It leveraged the EasyTransfer libraries by Bill Porter
  36. //          Legacy support will likely be deprecated and removed in time
  37. //      Long Term Holoprojector Support:
  38. //          SHADOW control will be isolated from particular logic hardware.
  39. //          We will migrate to I2C dome commands with PWM support:
  40. //          Holoprojector Servos and LEDs will be driven by:  
  41. //            http://www.adafruit.com/product/815
  42. //            This can drive 6 servos, and 3 LEDs.  PWM will allow for LED brightness "flicker"
  43. //  
  44. //   Sabertooth (Foot Drive):
  45. //         Set Sabertooth 2x32 or 2x25 Dip Switches: 1 and 2 Down, All Others Up
  46. //
  47. //   SyRen 10 Dome Drive:
  48. //         For SyRen packetized Serial Set Switches: 1, 2 and 4 Down, All Others Up
  49. //         NOTE:  Support for SyRen Simple Serial has been removed, due to problems.
  50. //         Please contact DimensionEngineering to get an RMA to flash your firmware
  51. //         Some place a 10K ohm resistor between S1 & GND on the SyRen 10 itself
  52. //
  53. // =======================================================================================
  54. //
  55. // ---------------------------------------------------------------------------------------
  56. //                          User Settings
  57. // ---------------------------------------------------------------------------------------
  58.  
  59. #define FOOT_CONTROLLER 1 //0 for Sabertooth Serial or 1 for individual R/C output (for Q85 motors with 1 controller for each foot, or Sabertooth Mode 2 Independant Mixing)
  60.  
  61. String PS3MoveNavigatonPrimaryMAC ="00:00:00:00:00:00"; //address of my navigation controller
  62.  
  63.  
  64. byte drivespeed1 = 70;   //set these 3 to whatever speeds work for you. 0-stop, 127-full speed. (L2 button released)
  65. byte drivespeed2 = 127;  //Recommend beginner: 50 to 75, experienced: 100 to 127, I like 100.   (L2 button held)
  66.                          //70 will map to 140, 127 will map to 180 for the Q85's
  67.  
  68. byte turnspeed = 75; //50;     // the higher this number the faster it will spin in place, lower - easier to control.
  69.                          // Recommend beginner: 40 to 50, experienced: 50 $ up, I like 75
  70.  
  71. byte domespeed = 100;    // If using a speed controller for the dome, sets the top speed
  72.                          // Use a number up to 127 for serial
  73.  
  74. byte ramping = 6; //3;        // Ramping- the lower this number the longer R2 will take to speedup or slow down,
  75.                          // change this by increments of 1
  76.  
  77. byte joystickFootDeadZoneRange = 15;  // For controllers that centering problems, use the lowest number with no drift
  78. byte joystickDomeDeadZoneRange = 10;  // For controllers that centering problems, use the lowest number with nfo drift
  79. byte driveDeadBandRange = 10;     // Used to set the Sabertooth DeadZone for foot motors
  80.  
  81. int invertTurnDirection = -1;   //This may need to be set to 1 for some configurations
  82.  
  83. //#define TEST_CONROLLER   //Support coming soon
  84. #define SHADOW_DEBUG       //uncomment this for console DEBUG output
  85. #define SHADOW_VERBOSE     //uncomment this for console VERBOSE output
  86.  
  87. // ---------------------------------------------------------------------------------------
  88. //                          Drive Controller Settings
  89. // ---------------------------------------------------------------------------------------
  90.  
  91. int motorControllerBaudRate = 9600; // Set the baud rate for the Syren motor controller
  92.                                     // for packetized options are: 2400, 9600, 19200 and 38400
  93.                                    
  94. #define SYREN_ADDR         129      // Serial Address for Dome Syren
  95. #define SABERTOOTH_ADDR    128      // Serial Address for Foot Sabertooth
  96.  
  97. #define leftFootPin 44  //connect this pin to motor controller for left foot (R/C mode)
  98. #define rightFootPin 45 //connect this pin to motor controller for right foot (R/C mode)
  99.  
  100.  
  101. // ---------------------------------------------------------------------------------------
  102. //                          Sound Settings
  103. // ---------------------------------------------------------------------------------------
  104. //Uncomment one line based on your sound system
  105. //#define SOUND_CFSOUNDIII     //Original system tested with SHADOW
  106. #define SOUND_MP3TRIGGER   //Code Tested by Dave C. and Marty M.
  107. //#define SOUND_ROGUE_RMP3   //Support coming soon
  108. //#define SOUND_RASBERRYPI   //Support coming soon
  109.  
  110. //#define EXTRA_SOUNDS
  111.  
  112.  
  113. // ---------------------------------------------------------------------------------------
  114. //                          Dome Control System
  115. // ---------------------------------------------------------------------------------------
  116. //Uncomment one line based on your Dome Control
  117. #define DOME_I2C_ADAFRUIT       //Current SHADOW configuration used with R-Series Logics
  118. //#define DOME_SERIAL_TEECES    //Original system tested with SHADOW
  119. //#define DOME_I2C_TEECES       //Untested Nov 2014
  120.  
  121.  
  122.  
  123.  
  124. // ---------------------------------------------------------------------------------------
  125. //                          Utility Arm Settings
  126. // ---------------------------------------------------------------------------------------
  127.  
  128. //Utility Arm Contribution by Dave C.
  129. //TODO:  Move PINS to upper part of Mega for Shield purposes
  130. const int UTILITY_ARM_BOTTOM_PIN  = 9;
  131. const int UTILITY_ARM_TOP_PIN   = 11;
  132.  
  133. int utilArmClosedPos = 0;    // variable to store the servo closed position
  134. int utilArmOpenPos = 140;    // variable to store the servo Opened position
  135.  
  136. // Check value, open = true, closed = false
  137. boolean isUtilArmTopOpen = false;    
  138. boolean isUtilArmBottomOpen = false;
  139.  
  140. int UtilArmBottomPos = 0;
  141. int UtilArmTopPos = 0;
  142.  
  143. const int UTIL_ARM_TOP = 1;
  144. const int UTIL_ARM_BOTTOM = 2;
  145.  
  146. // ---------------------------------------------------------------------------------------
  147. //                          LED Settings
  148. // ---------------------------------------------------------------------------------------
  149.  
  150. //Coin Slot LED Contribution by Dave C.
  151. //TODO:  Move PINS to upper part of Mega for Shield purposes
  152. #define numberOfCoinSlotLEDs 3
  153. int COIN_SLOT_LED_PINS[] = { 2, 3, 4 }; // LED pins to use.
  154. long nextCoinSlotLedFlash[numberOfCoinSlotLEDs]; // Array indicating which LED to flash next.
  155. int coinSlotLedState[numberOfCoinSlotLEDs]; // Array indicating the state of the LED's.
  156.  
  157.  
  158. // ---------------------------------------------------------------------------------------
  159. //                          Libraries
  160. // ---------------------------------------------------------------------------------------
  161. #include <PS3BT.h>
  162. #include <SPP.h>
  163. #include <usbhub.h>
  164. // Satisfy IDE, which only needs to see the include statment in the ino.
  165. #ifdef dobogusinclude
  166. #include <spi4teensy3.h>
  167. #endif
  168. #include <Sabertooth.h>
  169. #include <Servo.h>
  170. #include <LedControl.h>
  171.  
  172. #ifdef DOME_SERIAL_TEECES    
  173. #include <EasyTransfer.h>
  174. #endif
  175.  
  176. #ifdef DOME_I2C_TEECES    
  177. #include <EasyTransferI2C.h>
  178. #endif
  179.  
  180. #ifdef DOME_I2C_ADAFRUIT    
  181. #include <Wire.h>
  182. #include <Adafruit_PWMServoDriver.h>
  183. //#include <Servos.h>  //Attempted to use the "SlowServo library from BHD.... had issues
  184. #endif
  185.  
  186. //This is the traditional sound controler that has been used with PADAWAN
  187. #ifdef SOUND_MP3TRIGGER
  188. #include <MP3Trigger.h>
  189. MP3Trigger trigger;
  190. #endif
  191.  
  192. //Custom written Libraryy for the old CFSoundIII to emulate 12 button remote
  193. //CFSoundIII needs a supporting CFSOUND.BAS version running on the CFSoundIII
  194. #ifdef SOUND_CFSOUNDIII
  195. #include <CFSoundIII.h>
  196. CFSoundIII cfSound;
  197. #endif
  198.  
  199. //#ifdef  SOUND_ROGUE_RMP3
  200. //TODO:add rMP3 support
  201. //#endif
  202. //#ifdef  SOUND_RASBERRYPI
  203. //TODO:add Raspberry Pi Sound support
  204. //#endif
  205.  
  206.  
  207. // ---------------------------------------------------------------------------------------
  208. //                          Variables
  209. // ---------------------------------------------------------------------------------------
  210.  
  211. long previousDomeMillis = millis();
  212. long previousFootMillis = millis();
  213. long currentMillis = millis();
  214. int serialLatency = 25;   //This is a delay factor in ms to prevent queueing of the Serial data.
  215.                           //25ms seems approprate for HardwareSerial, values of 50ms or larger are needed for Softare Emulation
  216.  
  217. #if FOOT_CONTROLLER == 0
  218. Sabertooth *ST=new Sabertooth(SABERTOOTH_ADDR, Serial2); //
  219. #endif
  220. Sabertooth *SyR=new Sabertooth(SYREN_ADDR, Serial2);
  221.  
  222.  
  223. #ifdef DOME_SERIAL_TEECES    
  224.     EasyTransfer ET;
  225. #endif
  226.  
  227. #ifdef DOME_I2C_TEECES    
  228.     EasyTransferI2C ET;
  229. #endif
  230.  
  231. #if defined(DOME_SERIAL_TEECES) || defined(DOME_I2C_TEECES)
  232.     struct SEND_DATA_STRUCTURE
  233.     {
  234.         //put your variable definitions here for the data you want to send
  235.         //THIS MUST BE EXACTLY THE SAME ON THE OTHER ARDUINO
  236.         int hpx; // hp movement
  237.         int hpy; // hp movement
  238.         int hpl; // hp light
  239.         int hpa; // hp automation
  240.         int dsp; // 100=no change, 0=random, 1=alarm, 4=whistle, 5=leia, 6=short circut, 10=EQ, 11=alarm2,
  241.     };         // 21=speed1, 22=speed2, 23=speed3, 24=logics+, 25=logics-
  242.     SEND_DATA_STRUCTURE domeData;//give a name to the group of data
  243. #endif
  244.  
  245.  
  246. #ifdef DOME_I2C_ADAFRUIT    
  247.     const int HOLO_FRONT = 1;
  248.     const int HOLO_BACK = 2;
  249.     const int HOLO_TOP = 3;
  250.    
  251.     const int HOLO_DELAY = 20000; //up to 20 second delay
  252.     const int PWM_OFF = 4095;  //setting from Adafruit
  253.    
  254.     const int HOLO_FRONT_RED_PWM_PIN     = 0;
  255.     const int HOLO_FRONT_GREEN_PWM_PIN   = 1;
  256.     const int HOLO_FRONT_BLUE_PWM_PIN    = 2;
  257.     const int HOLO_FRONT_X_PWM_PIN       = 3;
  258.     const int HOLO_FRONT_Y_PWM_PIN       = 4;
  259.     const int HOLO_BACK_RED_PWM_PIN      = 5;
  260.     const int HOLO_BACK_GREEN_PWM_PIN    = 6;
  261.     const int HOLO_BACK_BLUE_PWM_PIN     = 7;
  262.     const int HOLO_BACK_X_PWM_PIN        = 8;
  263.     const int HOLO_BACK_Y_PWM_PIN        = 9;
  264.     const int HOLO_TOP_X_PWM_PIN        = 10;
  265.     const int HOLO_TOP_Y_PWM_PIN        = 11;
  266.     const int HOLO_TOP_RED_PWM_PIN      = 12;
  267.     const int HOLO_TOP_GREEN_PWM_PIN    = 13;
  268.     const int HOLO_TOP_BLUE_PWM_PIN     = 14;
  269.  
  270.  
  271.    const int HOLO_SERVO_CTR = 300;
  272.  
  273.     const int HOLO_FRONT_X_SERVO_MIN = 265; //250; //150;  // Issues with resin holo...
  274.     const int HOLO_FRONT_X_SERVO_MAX = 315; //350; //600;  // Issues with resin holo...
  275.     const int HOLO_FRONT_Y_SERVO_MIN = 250; //200; //150;  // Issues with resin holo...
  276.     const int HOLO_FRONT_Y_SERVO_MAX = 330; //400; //600;  // Issues with resin holo...
  277.    
  278.     const int HOLO_BACK_X_SERVO_MIN = 275; //250; //150;
  279.     const int HOLO_BACK_X_SERVO_MAX = 325; //350; //600;
  280.     const int HOLO_BACK_Y_SERVO_MIN = 250; //200; //150;
  281.     const int HOLO_BACK_Y_SERVO_MAX = 350; //400; //600;
  282.    
  283.     const int HOLO_TOP_X_SERVO_MIN = 275; //250; //150;
  284.     const int HOLO_TOP_X_SERVO_MAX = 325; //350; //600;
  285.     const int HOLO_TOP_Y_SERVO_MIN = 250; //200; //150;
  286.     const int HOLO_TOP_Y_SERVO_MAX = 350; //400; //600;
  287.    
  288.     const int HOLO_LED_OFF = 0;
  289.     const int HOLO_LED_ON = 1;
  290.     const int HOLO_LED_FLICKER = 2;    
  291.     int holoLightFrontStatus = 0;
  292.     int holoLightBackStatus = 0;
  293.     int holoLightTopStatus = 0;
  294.    
  295.     uint32_t holoFrontRandomTime = 0;
  296.     uint32_t holoBackRandomTime = 0;
  297.     uint32_t holoTopRandomTime = 0;
  298.     Adafruit_PWMServoDriver domePWM = Adafruit_PWMServoDriver();
  299. #endif
  300.  
  301.  
  302.  
  303.  
  304. ///////Setup for USB and Bluetooth Devices////////////////////////////
  305. USB Usb;
  306. //USBHub Hub1(&Usb); // Some dongles have a hub inside
  307. BTD Btd(&Usb); // You have to create the Bluetooth Dongle instance like so
  308. PS3BT *PS3Nav=new PS3BT(&Btd);
  309. PS3BT *PS3Nav2=new PS3BT(&Btd);
  310. //Used for PS3 Fault Detection
  311. uint32_t msgLagTime = 0;
  312. uint32_t lastMsgTime = 0;
  313. uint32_t currentTime = 0;
  314. uint32_t lastLoopTime = 0;
  315. int badPS3Data = 0;
  316.  
  317. SPP SerialBT(&Btd,"Astromech:R2","1977"); // Create a BT Serial device(defaults: "Arduino" and the pin to "0000" if not set)
  318. boolean firstMessage = true;
  319. String output = "";
  320.  
  321. boolean isFootMotorStopped = true;
  322. boolean isDomeMotorStopped = true;
  323.  
  324. boolean isPS3NavigatonInitialized = false;
  325. boolean isSecondaryPS3NavigatonInitialized = false;
  326.  
  327. byte vol = 50; // 0 = full volume, 255 off
  328. boolean isStickEnabled = true;
  329. byte isAutomateDomeOn = false;
  330. unsigned long automateMillis = 0;
  331. byte automateDelay = random(5,20);// set this to min and max seconds between sounds
  332. int domeAutomationTurnDirection = 20;
  333. byte action = 0;
  334. unsigned long DriveMillis = 0;
  335.  
  336. Servo UtilArmTopServo;  // create servo object to control a servo
  337. Servo UtilArmBottomServo;  // create servo object to control a servo
  338. #if FOOT_CONTROLLER ==1
  339. Servo leftFootSignal;
  340. Servo rightFootSignal;
  341. #endif
  342.  
  343. // =======================================================================================
  344. //                          Main Program
  345. // =======================================================================================
  346.  
  347. void setup()
  348. {
  349.     //Debug Serial for use with USB Debugging
  350.     Serial.begin(115200);
  351.     while (!Serial); // Wait for serial port to connect - used on Leonardo, Teensy and other boards with built-in USB CDC serial connection
  352.     if (Usb.Init() == -1)
  353.     {
  354.         Serial.print(F("\r\nOSC did not start"));
  355.         while (1); //halt
  356.     }
  357.     Serial.print(F("\r\nBluetooth Library Started"));
  358.     output.reserve(200); // Reserve 200 bytes for the output string
  359.  
  360.     //Setup for PS3
  361.     PS3Nav->attachOnInit(onInitPS3); // onInit() is called upon a new connection - you can call the function whatever you like
  362.     PS3Nav2->attachOnInit(onInitPS3Nav2);
  363.  
  364.     //The Arduino Mega has three additional serial ports:
  365.     // - Serial1 on pins 19 (RX) and 18 (TX),
  366.     // - Serial2 on pins 17 (RX) and 16 (TX),
  367.     // - Serial3 on pins 15 (RX) and 14 (TX).
  368.  
  369.     //Setup for Serial1:: Sound
  370.     #ifdef SOUND_CFSOUNDIII
  371.       cfSound.setup(&Serial1,2400);    
  372.     #endif
  373.     #ifdef SOUND_MP3TRIGGER
  374.       trigger.setup(&Serial1);
  375.       trigger.setVolume(vol);
  376.     #endif
  377.  
  378.     //Setup for Serial2:: Motor Controllers - Syren (Dome) and Sabertooth (Feet)
  379.     Serial2.begin(motorControllerBaudRate);
  380.     SyR->autobaud();
  381.     SyR->setTimeout(300);      //DMB:  How low can we go for safety reasons?  multiples of 100ms
  382.  
  383.     #if FOOT_CONTROLLER == 0
  384.     //Setup for Sabertooth / Foot Motors
  385.     ST->autobaud();          // Send the autobaud command to the Sabertooth controller(s).
  386.     ST->setTimeout(300);      //DMB:  How low can we go for safety reasons?  multiples of 100ms
  387.     ST->setDeadband(driveDeadBandRange);
  388.     #elif FOOT_CONTROLLER == 1
  389.     leftFootSignal.attach(leftFootPin);
  390.     rightFootSignal.attach(rightFootPin);
  391.     drivespeed1 = map(drivespeed1, 0, 127, 90, 180); //convert drivespeed values to something that will work for the Q85's
  392.     drivespeed2 = map(drivespeed2, 0, 127, 90, 180);
  393.     #endif
  394.     stopFeet();
  395.  
  396.     // NOTE: *Not all* Sabertooth controllers need the autobaud command.
  397.     //       It doesn't hurt anything, but V2 controllers use an
  398.     //       EEPROM setting (changeable with the function setBaudRate) to set
  399.     //       the baud rate instead of detecting with autobaud.
  400.     //
  401.     //       If you have a 2x12, 2x25 V2, 2x60 or SyRen 50, you can remove
  402.     //       the autobaud line and save yourself two seconds of startup delay.
  403.  
  404.  
  405.     #ifdef DOME_I2C_ADAFRUIT          
  406.         domePWM.begin();
  407.         domePWM.setPWMFreq(50);  // Analog servos run at ~50 Hz updates
  408.     #endif
  409.  
  410.     #ifdef DOME_SERIAL_TEECES
  411.       //Setup for Serial3:: Dome Communication Link  
  412.       ..begin(57600);//start the library, pass in the data details and the name of the serial port.
  413.       ET.begin(details(domeData), &Serial3);
  414.     #endif
  415.    
  416.     #ifdef DOME_I2C_TEECES    
  417.       Wire.begin();
  418.       ET.begin(details(domeData), &Wire);
  419.     #endif
  420.  
  421.  
  422.     //Setup for Utility Arm Servo's    
  423.     UtilArmTopServo.attach(UTILITY_ARM_TOP_PIN);  
  424.     UtilArmBottomServo.attach(UTILITY_ARM_BOTTOM_PIN);
  425.     closeUtilArm(UTIL_ARM_TOP);
  426.     closeUtilArm(UTIL_ARM_BOTTOM);
  427.    
  428.     //Setup for Coin Slot LEDs    
  429.     for(int i = 0; i<numberOfCoinSlotLEDs; i++)
  430.     {
  431.       pinMode(COIN_SLOT_LED_PINS[i],OUTPUT);
  432.       coinSlotLedState[i] = LOW;
  433.       digitalWrite(COIN_SLOT_LED_PINS[i], LOW); // all LEDs off
  434.       nextCoinSlotLedFlash[i] = millis() +random(100, 1000);
  435.     }    
  436. }
  437.  
  438. boolean readUSB()
  439. {
  440.     //The more devices we have connected to the USB or BlueTooth, the more often Usb.Task need to be called to eliminate latency.
  441.     Usb.Task();
  442.     if (PS3Nav->PS3NavigationConnected ) Usb.Task();
  443.     if (PS3Nav2->PS3NavigationConnected ) Usb.Task();
  444.     if ( criticalFaultDetect() )
  445.     {
  446.       //We have a fault condition that we want to ensure that we do NOT process any controller data!!!
  447.       flushAndroidTerminal();
  448.       return false;
  449.     }
  450.     return true;
  451. }
  452.  
  453. int whiteLED=0;
  454. void loop()
  455. {  
  456.     initAndroidTerminal();
  457.    
  458.     //Useful to enable with serial console when having controller issues.
  459.     #ifdef TEST_CONROLLER
  460.       testPSController();
  461.     #endif
  462.  
  463.     //LOOP through functions from highest to lowest priority.
  464.  
  465.     if ( !readUSB() )
  466.     {
  467.       //We have a fault condition that we want to ensure that we do NOT process any controller data!!!
  468.       return;
  469.     }
  470.     footMotorDrive();
  471.        
  472.     if ( !readUSB() )
  473.     {
  474.       //We have a fault condition that we want to ensure that we do NOT process any controller data!!!
  475.       return;
  476.     }
  477.     automateDome();
  478.     domeDrive();
  479.  
  480.     utilityArms();
  481.     holoprojector();
  482.     toggleSettings();
  483.     soundControl();
  484.     flashCoinSlotLEDs();
  485.     flushAndroidTerminal();
  486. }
  487.  
  488.  
  489. void onInitPS3()
  490. {
  491.     String btAddress = getLastConnectedBtMAC();
  492.     PS3Nav->setLedOn(LED1);
  493.     isPS3NavigatonInitialized = true;
  494.     badPS3Data = 0;
  495.     #ifdef SHADOW_DEBUG
  496.       output += "\r\nBT Address of Last connected Device when Primary PS3 Connected: ";
  497.       output += btAddress;
  498.       if (btAddress == PS3MoveNavigatonPrimaryMAC)
  499.       {
  500.           output += "\r\nWe have our primary controller connected.\r\n";
  501.       }
  502.       else
  503.       {
  504.           output += "\r\nWe have a controller connected, but it is not designated as \"primary\".\r\n";
  505.       }
  506.     #endif
  507. }
  508.  
  509. void onInitPS3Nav2()
  510. {
  511.     String btAddress = getLastConnectedBtMAC();
  512.     PS3Nav2->setLedOn(LED1);
  513.     isSecondaryPS3NavigatonInitialized = true;
  514.     badPS3Data = 0;
  515.     if (btAddress == PS3MoveNavigatonPrimaryMAC) swapPS3NavControllers();
  516.     #ifdef SHADOW_DEBUG
  517.       output += "\r\nBT Address of Last connected Device when Secondary PS3 Connected: ";
  518.       output += btAddress;
  519.       if (btAddress == PS3MoveNavigatonPrimaryMAC)
  520.       {
  521.           output += "\r\nWe have our primary controller connecting out of order.  Swapping locations\r\n";
  522.       }
  523.       else
  524.       {
  525.           output += "\r\nWe have a secondary controller connected.\r\n";
  526.       }
  527.     #endif
  528. }
  529.  
  530. String getLastConnectedBtMAC()
  531. {
  532.     String btAddress = "";
  533.     for(int8_t i = 5; i > 0; i--)
  534.     {
  535.         if (Btd.disc_bdaddr[i]<0x10)
  536.         {
  537.             btAddress +="0";
  538.         }
  539.         btAddress += String(Btd.disc_bdaddr[i], HEX);
  540.         btAddress +=(":");
  541.     }
  542.     btAddress += String(Btd.disc_bdaddr[0], HEX);
  543.     btAddress.toUpperCase();
  544.     return btAddress;
  545. }
  546.  
  547. void swapPS3NavControllers()
  548. {
  549.     PS3BT* temp = PS3Nav;
  550.     PS3Nav = PS3Nav2;
  551.     PS3Nav2 = temp;
  552.     //Correct the status for Initialization
  553.     boolean tempStatus = isPS3NavigatonInitialized;
  554.     isPS3NavigatonInitialized = isSecondaryPS3NavigatonInitialized;
  555.     isSecondaryPS3NavigatonInitialized = tempStatus;
  556.     //Must relink the correct onInit calls
  557.     PS3Nav->attachOnInit(onInitPS3);
  558.     PS3Nav2->attachOnInit(onInitPS3Nav2);
  559. }
  560.  
  561.  
  562. void initAndroidTerminal()
  563. {
  564.     //Setup for Bluetooth Serial Monitoring
  565.     if (SerialBT.connected)
  566.     {
  567.         if (firstMessage)
  568.         {
  569.             firstMessage = false;
  570.             SerialBT.println(F("Hello from S.H.A.D.O.W.")); // Send welcome message
  571.         }
  572.         //TODO:  Process input from the SerialBT
  573.         //if (SerialBT.available())
  574.         //    Serial.write(SerialBT.read());
  575.     }
  576.     else
  577.     {
  578.         firstMessage = true;
  579.     }
  580. }
  581.  
  582. void flushAndroidTerminal()
  583. {
  584.     if (output != "")
  585.     {
  586.         if (Serial) Serial.println(output);
  587.         if (SerialBT.connected)
  588.             SerialBT.println(output);
  589.             SerialBT.send();
  590.         output = ""; // Reset output string
  591.     }
  592. }
  593.  
  594.  
  595. void automateDome()
  596. {
  597.   /////////////automate
  598.     if (isAutomateDomeOn)
  599.     {
  600.         //TODO:  We have other conditions where the dome is moving
  601.         if ( abs(PS3Nav->getAnalogHat(LeftHatY)-128) > joystickFootDeadZoneRange)
  602.         {
  603.             automateMillis = millis();
  604.         }
  605.         currentMillis = millis();
  606.         if (currentMillis - automateMillis > (automateDelay*1000))
  607.         {
  608.             automateMillis = millis();
  609.             action = random(1,5);
  610.             if (action>1)
  611.             {
  612.               //DMB:  Random Sounds, and Dome movement shouldn't be tied 1:1
  613.               //TODO:  Add Sound Automation Routine
  614.               #ifdef SOUND_MP3TRIGGER
  615.               (trigger.play(random(32,52)));
  616.               #endif
  617.             }
  618.             if (action<4)
  619.             {
  620.                   Serial.println("automation of Dome");        
  621.                   rotateDome(domeAutomationTurnDirection,"Automation");
  622.                   //DMB:  The delay statement below is a critial failing (IMO) as this locks out control.
  623.                   //TODO:  Rework Dome Automation!
  624.                   delay(500);
  625.                   rotateDome(0,"Automation");
  626.                 if (domeAutomationTurnDirection>0)
  627.                 {
  628.                     domeAutomationTurnDirection = -45;
  629.                 }
  630.                 else
  631.                 {
  632.                     domeAutomationTurnDirection = 45;
  633.                 }
  634.             }
  635.             automateDelay = random(5,20);// set this to min and max seconds between sounds
  636.         }
  637.     }
  638. }
  639.  
  640.  
  641.  
  642. // =======================================================================================
  643. // //////////////////////////Process PS3 Controller Fault Detection///////////////////////
  644. // =======================================================================================
  645. boolean criticalFaultDetect()
  646. {
  647.     if (PS3Nav->PS3NavigationConnected || PS3Nav->PS3Connected)
  648.     {
  649.         lastMsgTime = PS3Nav->getLastMessageTime();
  650.         currentTime = millis();
  651.         if ( currentTime >= lastMsgTime)
  652.         {
  653.           msgLagTime = currentTime - lastMsgTime;
  654.         } else
  655.         {
  656.              #ifdef SHADOW_DEBUG
  657.                output += "Waiting for PS3Nav Controller Data\r\n";
  658.              #endif
  659.              badPS3Data++;
  660.              msgLagTime = 0;
  661.         }
  662.        
  663.         if (msgLagTime > 100 && !isFootMotorStopped)
  664.         {
  665.             #ifdef SHADOW_DEBUG
  666.               output += "It has been 100ms since we heard from the PS3 Controller\r\n";
  667.               output += "Shut downing motors, and watching for a new PS3 message\r\n";
  668.             #endif
  669.             stopFeet();
  670.             SyR->stop();
  671.             isFootMotorStopped = true;
  672.             return true;
  673.         }
  674.         if ( msgLagTime > 30000 )
  675.         {
  676.             #ifdef SHADOW_DEBUG
  677.               output += "It has been 30s since we heard from the PS3 Controller\r\n";
  678.               output += "msgLagTime:";
  679.               output += msgLagTime;
  680.               output += "  lastMsgTime:";
  681.               output += lastMsgTime;
  682.               output += "  millis:";
  683.               output += millis();            
  684.               output += "\r\nDisconnecting the controller.\r\n";
  685.             #endif
  686.             PS3Nav->disconnect();
  687.         }
  688.  
  689.         //Check PS3 Signal Data
  690.         if(!PS3Nav->getStatus(Plugged) && !PS3Nav->getStatus(Unplugged))
  691.         {
  692.             // We don't have good data from the controller.
  693.             //Wait 10ms, Update USB, and try again
  694.             delay(10);
  695.             Usb.Task();
  696.             if(!PS3Nav->getStatus(Plugged) && !PS3Nav->getStatus(Unplugged))
  697.             {
  698.                 badPS3Data++;
  699.                 #ifdef SHADOW_DEBUG
  700.                     output += "\r\nInvalid data from PS3 Controller.";
  701.                 #endif
  702.                 return true;
  703.             }
  704.         }
  705.         else if (badPS3Data > 0)
  706.         {
  707.             //output += "\r\nPS3 Controller  - Recovered from noisy connection after: ";
  708.             //output += badPS3Data;
  709.             badPS3Data = 0;
  710.         }
  711.         if ( badPS3Data > 10 )
  712.         {
  713.             #ifdef SHADOW_DEBUG
  714.                 output += "Too much bad data coming fromo the PS3 Controller\r\n";
  715.                 output += "Disconnecting the controller.\r\n";
  716.             #endif
  717.             PS3Nav->disconnect();
  718.         }
  719.     }
  720.     else if (!isFootMotorStopped)
  721.     {
  722.         #ifdef SHADOW_DEBUG      
  723.             output += "No Connected Controllers were found\r\n";
  724.             output += "Shuting downing motors, and watching for a new PS3 message\r\n";
  725.         #endif
  726.         stopFeet();
  727.         SyR->stop();
  728.         isFootMotorStopped = true;
  729.         return true;
  730.     }
  731.     return false;
  732. }
  733. // =======================================================================================
  734. // //////////////////////////END of PS3 Controller Fault Detection///////////////////////
  735. // =======================================================================================
  736.  
  737. #if FOOT_CONTROLLER == 1
  738. int leftFoot,rightFoot; //will hold foot speed values (-100 to 100)
  739. void mixBHD(byte stickX, byte stickY, byte maxDriveSpeed){  //maxDriveSpeed should be between 90 and 180
  740.     // This is BigHappyDude's mixing function, for differential (tank) style drive using two motor controllers.
  741.     // Takes a joysticks X and Y values, mixes using the diamind mix, and output a value 0-180 for left and right motors.    
  742.     // 180,180 = both feet full speed forward.
  743.     // 000,000 = both feet full speed reverse.
  744.     // 180,000 = left foot full forward, right foot full reverse (spin droid clockwise)
  745.     // 000,180 = left foot full reverse, right foot full forward (spin droid counter-clockwise)
  746.     // 090,090 = no movement
  747.     // for simplicity, we think of this diamond matrix as a range from -100 to +100 , then map the final values to servo range (0-180) at the end
  748.     //  Ramping and Speed mode applied on the droid.  
  749.     if(((stickX <= 113) || (stickX >= 141)) || ((stickY <= 113) || (stickY >= 141))){  //  if movement outside deadzone
  750.       //  Map to easy grid -100 to 100 in both axis, including deadzones.
  751.       int YDist = 0;  // set to 0 as a default value if no if used.
  752.       int XDist = 0;
  753.       if(stickY <= 113){
  754.        YDist = (map(stickY, 0, 113, 100, 1));           //  Map the up direction stick value to Drive speed
  755.       } else if(stickY >= 141){
  756.        YDist = (map(stickY, 141, 255, -1, -100));       //  Map the down direction stick value to Drive speed
  757.       }
  758.       if(stickX <= 113){
  759.        XDist = (map(stickX, 0, 113, -100, -1));       //  Map the left direction stick value to Turn speed
  760.       } else if(stickX >= 141){
  761.        XDist = (map(stickX, 141, 255, 1, 100));   //  Map the right direction stick value to Turn speed
  762.       }
  763.       //  Constrain to Diamond values.  using 2 line equations and find the intersect, boiled down to the minimum
  764.       //  This was the inspiration; https://github.com/declanshanaghy/JabberBot/raw/master/Docs/Using%20Diamond%20Coordinates%20to%20Power%20a%20Differential%20Drive.pdf
  765.       float TempYDist = YDist;
  766.       float TempXDist = XDist;
  767.       if (YDist>(XDist+100)) {  //  if outside top left.  equation of line is y=x+Max, so if y > x+Max then it is above line
  768.         // OK, the first fun bit. :)  so for the 2 lines this is always true y = m1*x + b1 and y = m2*x - b2
  769.         // y - y = m1*x + b1  - m2*x - b2  or 0 = (m1 - m2)*x + b1 - b2
  770.         // We have y = x+100 and y = ((change in y)/Change in x))x
  771.         // So:   x = -100/(1-(change in y)/Change in x)) and using y = x+100 we can find y with the new x
  772.         // Not too bad when simplified. :P
  773.         TempXDist = -100/(1-(TempYDist/TempXDist));
  774.         TempYDist = TempXDist+100;
  775.       } else if (YDist>(100-XDist)) {  //  if outside top right
  776.         // repeat intesection for y = 100 - x
  777.         TempXDist = -100/(-1-(TempYDist/TempXDist));
  778.         TempYDist = -TempXDist+100;
  779.       } else if (YDist<(-XDist-100)) {  //  if outside bottom left
  780.         // repeat intesection for y = -x - 100
  781.         TempXDist = 100/(-1-(TempYDist/TempXDist));
  782.         TempYDist = -TempXDist-100;
  783.       } else if (YDist<(XDist-100)) {  //  if outside bottom right
  784.         // repeat intesection for y = x - 100
  785.         TempXDist = 100/(1-(TempYDist/TempXDist));
  786.         TempYDist = TempXDist-100;
  787.       }
  788.       //  all coordinates now in diamond. next translate to the diamond coordinates.
  789.       //  for the left.  send ray to y = x + Max from coordinates along y = -x + b
  790.       //  find for b, solve for coordinates and resut in y then scale using y = (y - max/2)*2
  791.       float LeftSpeed = ((TempXDist+TempYDist-100)/2)+100;
  792.       LeftSpeed = (LeftSpeed-50)*2;
  793.       //  for right send ray to y = -x + Max from coordinates along y = x + b find intersction coordinates and then use the Y vaule and scale.
  794.       float RightSpeed = ((TempYDist-TempXDist-100)/2)+100;
  795.       RightSpeed = (RightSpeed-50)*2;
  796.       // this all results in a -100 to 100 range of speeds, so shift to servo range...
  797.       //  eg. for a maxDriveSpeed of 140, we'd need the value to map to between 40 and 140
  798.       //  eg. for a maxDriveSpeed of 180, we'd need the value to map to between 0 and 180
  799.       //leftFoot=map(LeftSpeed, -100, 100, (180-maxDriveSpeed), maxDriveSpeed);
  800.       //rightFoot=map(RightSpeed, -100, 100, (180-maxDriveSpeed), maxDriveSpeed);
  801.       leftFoot=map(LeftSpeed, -100, 100, maxDriveSpeed, (180-maxDriveSpeed) );
  802.       rightFoot=map(RightSpeed, -100, 100, maxDriveSpeed, (180-maxDriveSpeed) );
  803.     } else {
  804.       leftFoot=90;
  805.       rightFoot=90;
  806.     }
  807. }
  808. #endif
  809.  
  810. void stopFeet() {
  811.   #if FOOT_CONTROLLER == 0
  812.   ST->stop();
  813.   #elif FOOT_CONTROLLER == 1
  814.   leftFootSignal.write(90);
  815.   rightFootSignal.write(90);
  816.   #endif
  817. }
  818.  
  819. boolean ps3FootMotorDrive(PS3BT* myPS = PS3Nav)
  820. {
  821.   int footDriveSpeed = 0;
  822.   int stickSpeed = 0;
  823.   int turnnum = 0;
  824.  
  825.   if (isPS3NavigatonInitialized)
  826.   {
  827.       // Additional fault control.  Do NOT send additional commands to Sabertooth if no controllers have initialized.
  828.       if (!isStickEnabled)
  829.       {
  830.             #ifdef SHADOW_VERBOSE
  831.               if ( abs(myPS->getAnalogHat(LeftHatY)-128) > joystickFootDeadZoneRange)
  832.               {
  833.                 output += "Drive Stick is disabled\r\n";
  834.               }
  835.             #endif
  836.           stopFeet();
  837.           isFootMotorStopped = true;
  838.       } else if (!myPS->PS3NavigationConnected)
  839.       {
  840.           stopFeet();
  841.           isFootMotorStopped = true;
  842.       } else if ( myPS->getButtonPress(L1) )
  843.       {
  844.           //TODO:  Does this need to change this when we support dual controller, or covered by improved isStickEnabled
  845.           stopFeet();
  846.           isFootMotorStopped = true;
  847.       } else
  848.       {
  849.           //make those feet move!!!///////////////////////////////////////////////////
  850.           int joystickPosition = myPS->getAnalogHat(LeftHatY);
  851.           isFootMotorStopped = false;
  852.           #if FOOT_CONTROLLER == 0
  853.             if (myPS->getButtonPress(L2))
  854.             {
  855.               int throttle = 0;
  856.               if (joystickPosition < 127) throttle = joystickPosition - myPS->getAnalogButton(L2);
  857.               else throttle = joystickPosition + myPS->getAnalogButton(L2);
  858.               stickSpeed = (map(throttle, -255, 510, -drivespeed2, drivespeed2));                
  859.             } else
  860.             {
  861.               stickSpeed = (map(joystickPosition, 0, 255, -drivespeed1, drivespeed1));
  862.             }          
  863.  
  864.             if ( abs(joystickPosition-128) < joystickFootDeadZoneRange)              footDriveSpeed = 0;
  865.             else if (footDriveSpeed < stickSpeed)
  866.             {
  867.                 if (stickSpeed-footDriveSpeed<(ramping+1)) footDriveSpeed+=ramping;
  868.                 else footDriveSpeed = stickSpeed;
  869.             }
  870.             else if (footDriveSpeed > stickSpeed)
  871.             {
  872.                 if (footDriveSpeed-stickSpeed<(ramping+1)) footDriveSpeed-=ramping;
  873.                 else footDriveSpeed = stickSpeed;  
  874.             }
  875.            
  876.             turnnum = (myPS->getAnalogHat(LeftHatX));
  877.  
  878.             //TODO:  Is there a better algorithm here?  
  879.             if ( abs(footDriveSpeed) > 50)
  880.                 turnnum = (map(myPS->getAnalogHat(LeftHatX), 54, 200, -(turnspeed/4), (turnspeed/4)));
  881.             else if (turnnum <= 200 && turnnum >= 54)
  882.                 turnnum = (map(myPS->getAnalogHat(LeftHatX), 54, 200, -(turnspeed/3), (turnspeed/3)));
  883.             else if (turnnum > 200)
  884.                 turnnum = (map(myPS->getAnalogHat(LeftHatX), 201, 255, turnspeed/3, turnspeed));
  885.             else if (turnnum < 54)
  886.                 turnnum = (map(myPS->getAnalogHat(LeftHatX), 0, 53, -turnspeed, -(turnspeed/3)));
  887.           #endif
  888.  
  889.           currentMillis = millis();
  890.           if ( (currentMillis - previousFootMillis) > serialLatency  )
  891.           {
  892.  
  893.             #ifdef SHADOW_VERBOSE      
  894.             if ( footDriveSpeed < -driveDeadBandRange || footDriveSpeed > driveDeadBandRange)
  895.             {
  896.               output += "Driving Droid at footSpeed: ";
  897.               output += footDriveSpeed;
  898.               output += "!  DriveStick is Enabled\r\n";
  899.               output += "Joystick: ";              
  900.               output += myPS->getAnalogHat(LeftHatX);
  901.               output += "/";              
  902.               output += myPS->getAnalogHat(LeftHatY);
  903.               output += " turnnum: ";              
  904.               output += turnnum;
  905.               output += "/";              
  906.               output += footDriveSpeed;
  907.               output += " Time of command: ";              
  908.               output += millis();
  909.             }
  910.             #endif
  911.  
  912.           #if FOOT_CONTROLLER == 0
  913.             ST->turn(turnnum * invertTurnDirection);
  914.             ST->drive(footDriveSpeed);
  915.             // The Sabertooth won't act on mixed mode packet serial commands until
  916.             // it has received power levels for BOTH throttle and turning, since it
  917.             // mixes the two together to get diff-drive power levels for both motors.
  918.           #elif FOOT_CONTROLLER == 1
  919.             //Experimental Q85. Untested Madness!!! Use at your own risk and expect your droid to run away in flames.
  920.             //use BigHappyDude's mixing algorythm to get values for each foot...
  921.             if (myPS->getButtonPress(L2)) mixBHD(myPS->getAnalogHat(LeftHatX),myPS->getAnalogHat(LeftHatY),drivespeed2);
  922.             else mixBHD(myPS->getAnalogHat(LeftHatX),myPS->getAnalogHat(LeftHatY),drivespeed1);
  923.             //now we've got values for leftFoot and rightFoot, output those somehow...
  924.             leftFootSignal.write(leftFoot);
  925.             rightFootSignal.write(rightFoot);
  926.           #endif
  927.            previousFootMillis = currentMillis;
  928.           return true; //we sent a foot command  
  929.           }
  930.          
  931.       }
  932.   }
  933.   return false;
  934. }
  935.  
  936.  
  937. int ps3DomeDrive(PS3BT* myPS = PS3Nav, int controllerNumber = 1)
  938. {
  939.     int domeRotationSpeed = 0;
  940.     if (controllerNumber==1 && myPS->getButtonPress(CROSS) && !(myPS->getButtonPress(L1)) && !(myPS->getButtonPress(L2)) && !(myPS->getButtonPress(PS))  )
  941.     {
  942.       domeRotationSpeed = -75;
  943.     } else if (controllerNumber==1 && myPS->getButtonPress(CIRCLE) && !(myPS->getButtonPress(L1)) && !(myPS->getButtonPress(L2)) && !(myPS->getButtonPress(PS))  )
  944.     {
  945.       domeRotationSpeed = 75;
  946.     } else if ( (controllerNumber==1 && myPS->getButtonPress(L1)) || ( controllerNumber==2 && !myPS->getButtonPress(L1) && !myPS->getButtonPress(L2) )  )
  947.     {
  948.         int joystickPosition = myPS->getAnalogHat(LeftHatX);
  949.         domeRotationSpeed = (map(joystickPosition, 0, 255, -domespeed, domespeed));
  950.         if ( abs(joystickPosition-128) < joystickDomeDeadZoneRange )
  951.           domeRotationSpeed = 0;
  952.     }
  953.     return domeRotationSpeed;
  954. }
  955.  
  956. void rotateDome(int domeRotationSpeed, String mesg)
  957. {
  958.     //Constantly sending commands to the SyRen (Dome) is causing foot motor delay.
  959.     //Lets reduce that chatter by trying 3 things:
  960.     // 1.) Eliminate a constant stream of "don't spin" messages (isDomeMotorStopped flag)
  961.     // 2.) Add a delay between commands sent to the SyRen (previousDomeMillis timer)
  962.     // 3.) Switch to real UART on the MEGA (Likely the *CORE* issue and solution)
  963.     // 4.) Reduce the timout of the SyRen - just better for safety!
  964.    
  965.     currentMillis = millis();
  966.     if ( (!isDomeMotorStopped || domeRotationSpeed != 0) && ((currentMillis - previousDomeMillis) > (2*serialLatency) )  )
  967.     {
  968.       #ifdef SHADOW_VERBOSE      
  969.         output += "DEBUG:  Dome Rotation called by: ";
  970.         output += mesg;
  971.         if (domeRotationSpeed < 0)
  972.         {
  973.           output += "  Spinning Dome Left at speed: ";
  974.         } else if (domeRotationSpeed > 0)
  975.         {
  976.           output += "  Spinning Dome Right at speed: ";
  977.         } else
  978.         {
  979.           output += "  Stopping Dome Spin speed: ";
  980.         }    
  981.         output += domeRotationSpeed;
  982.         output += "\r\n";
  983.       #endif
  984.       if (domeRotationSpeed != 0)
  985.       {
  986.         isDomeMotorStopped = false;
  987.       } else
  988.       {
  989.         isDomeMotorStopped = true;
  990.       }
  991.       previousDomeMillis = currentMillis;      
  992.       SyR->motor(domeRotationSpeed);
  993.     }
  994. }
  995.  
  996.  
  997. #ifdef DOME_I2C_ADAFRUIT          
  998. boolean adafruitPs3Holoprojector(PS3BT* myPS = PS3Nav, int controllerNumber = 1)
  999. {
  1000.     ////turn hp light on & off
  1001.     if( !(myPS->getButtonPress(PS)) && myPS->getButtonClick(L3))
  1002.     {
  1003.         if (holoLightFrontStatus != HOLO_LED_OFF)
  1004.     {
  1005.             #ifdef SHADOW_DEBUG      
  1006.               output += "Turning Off Holo Light\r\n";
  1007.             #endif
  1008.             holoLightFrontStatus = HOLO_LED_OFF;
  1009.             holoLightOff(HOLO_FRONT_RED_PWM_PIN, HOLO_FRONT_GREEN_PWM_PIN, HOLO_FRONT_BLUE_PWM_PIN);
  1010.     } else
  1011.     {
  1012.             #ifdef SHADOW_DEBUG      
  1013.               output += "Turning On Holo Light\r\n";
  1014.             #endif
  1015.             holoLightFrontStatus = HOLO_LED_ON;
  1016.             holoLightOn(HOLO_FRONT_RED_PWM_PIN, HOLO_FRONT_GREEN_PWM_PIN, HOLO_FRONT_BLUE_PWM_PIN);
  1017.     }      
  1018.         return true;
  1019.     }
  1020.  
  1021. /////hp movement
  1022.     if (myPS->getButtonPress(PS))
  1023.     {
  1024.         if(myPS->getButtonPress(UP))
  1025.         {
  1026.             #ifdef SHADOW_DEBUG
  1027.               output += "Move Holo Up\r\n";
  1028.             #endif
  1029.             moveHoloServo(HOLO_FRONT_Y_PWM_PIN, HOLO_FRONT_Y_SERVO_MAX);
  1030.         }        
  1031.         if(myPS->getButtonPress(DOWN))
  1032.         {
  1033.             #ifdef SHADOW_DEBUG
  1034.               output += "Move Holo Down\r\n";
  1035.             #endif
  1036.             moveHoloServo(HOLO_FRONT_Y_PWM_PIN, HOLO_FRONT_Y_SERVO_MIN);
  1037.         }
  1038.         if(myPS->getButtonPress(LEFT))
  1039.         {
  1040.             #ifdef SHADOW_DEBUG
  1041.               output += "Move Holo Left\r\n";
  1042.             #endif
  1043.             moveHoloServo(HOLO_FRONT_X_PWM_PIN, HOLO_FRONT_X_SERVO_MAX );
  1044.         }
  1045.         if(myPS->getButtonPress(RIGHT))
  1046.         {
  1047.             #ifdef SHADOW_DEBUG
  1048.               output += "Move Holo Right\r\n";
  1049.             #endif
  1050.             moveHoloServo(HOLO_FRONT_X_PWM_PIN, HOLO_FRONT_X_SERVO_MIN);
  1051.         }
  1052.         if (!(myPS->getButtonPress(UP)||myPS->getButtonPress(DOWN)))
  1053.         {
  1054.             moveHoloServo(HOLO_FRONT_Y_PWM_PIN, HOLO_SERVO_CTR);
  1055.         }
  1056.         if (!(myPS->getButtonPress(RIGHT)||myPS->getButtonPress(LEFT)))
  1057.         {
  1058.             moveHoloServo(HOLO_FRONT_X_PWM_PIN, HOLO_SERVO_CTR);
  1059.         }
  1060.         return true;
  1061.     }
  1062.     if (myPS->getButtonPress(L1) &&controllerNumber == 2)
  1063.     {
  1064.             //TODO:  Analog stick - can we granually control holo?
  1065.             if(myPS->getAnalogHat(LeftHatY) < 50)
  1066.             {
  1067.                 output += "Move Holo Up\r\n";
  1068.                 moveHoloServo(HOLO_FRONT_Y_PWM_PIN, HOLO_FRONT_Y_SERVO_MAX);
  1069.             }        
  1070.             if(myPS->getAnalogHat(LeftHatY) > 200)
  1071.             {
  1072.                 output += "Move Holo Down\r\n";
  1073.                 moveHoloServo(HOLO_FRONT_Y_PWM_PIN, HOLO_FRONT_Y_SERVO_MIN);
  1074.             }
  1075.             if(myPS->getAnalogHat(LeftHatX) < 50)
  1076.             {
  1077.                 output += "Move Holo Left\r\n";
  1078.                 moveHoloServo(HOLO_FRONT_X_PWM_PIN, HOLO_FRONT_X_SERVO_MAX);
  1079.             }
  1080.             if(myPS->getAnalogHat(LeftHatX) > 200)
  1081.             {
  1082.                 output += "Move Holo Right\r\n";
  1083.                 moveHoloServo(HOLO_FRONT_X_PWM_PIN, HOLO_FRONT_X_SERVO_MIN);
  1084.             }
  1085.             if (!(myPS->getAnalogHat(LeftHatY) < 50||myPS->getAnalogHat(LeftHatY) > 200))
  1086.             {
  1087.                 //output += "Move Holo V-Center\r\n";
  1088.                 moveHoloServo(HOLO_FRONT_Y_PWM_PIN, HOLO_SERVO_CTR);
  1089.             }
  1090.             if (!(myPS->getAnalogHat(LeftHatX) < 50||myPS->getAnalogHat(LeftHatX) > 200))
  1091.             {
  1092.                 //output += "Move Holo H-Center\r\n";
  1093.                 moveHoloServo(HOLO_FRONT_X_PWM_PIN, HOLO_SERVO_CTR);
  1094.             }
  1095.             return true;        
  1096.     }
  1097.     return false;
  1098. }
  1099. #endif
  1100.  
  1101.    
  1102. #if defined(DOME_SERIAL_TEECES) || defined(DOME_I2C_TEECES)
  1103. boolean teecesPs3Holoprojector(PS3BT* myPS = PS3Nav, int controllerNumber = 1)
  1104. {
  1105.   ////turn hp light on & off
  1106.     if( !(myPS->getButtonPress(PS)) && myPS->getButtonClick(L3))
  1107.     {
  1108.         if(domeData.hpl == 1)
  1109.         {
  1110.             #ifdef SHADOW_DEBUG      
  1111.               output += "Turning Off Holo Light\r\n";
  1112.             #endif
  1113.             domeData.hpl = 0;    //if hp light is on, turn it off
  1114.             domeData.dsp = 100;
  1115.             ET.sendData();
  1116.         }
  1117.         else
  1118.         {
  1119.             #ifdef SHADOW_DEBUG      
  1120.               output += "Turning On Holo Light\r\n";
  1121.             #endif
  1122.             domeData.hpl = 1;    //turn hp light on
  1123.             domeData.dsp = 100;
  1124.             ET.sendData();
  1125.         }
  1126.         return true;
  1127.     }
  1128.  
  1129. /////hp movement
  1130.     if (myPS->getButtonPress(PS))
  1131.     {
  1132.         if(myPS->getButtonPress(UP))
  1133.         {
  1134.             #ifdef SHADOW_DEBUG
  1135.               output += "Move Holo Up\r\n";
  1136.             #endif
  1137.             domeData.hpy = 150;
  1138.             domeData.dsp = 100;
  1139.             ET.sendData();
  1140.         }        
  1141.         if(myPS->getButtonPress(DOWN))
  1142.         {
  1143.             #ifdef SHADOW_DEBUG
  1144.               output += "Move Holo Down\r\n";
  1145.             #endif
  1146.             domeData.hpy = 30;
  1147.             domeData.dsp = 100;
  1148.             ET.sendData();
  1149.         }
  1150.         if(myPS->getButtonPress(LEFT))
  1151.         {
  1152.             #ifdef SHADOW_DEBUG
  1153.               output += "Move Holo Left\r\n";
  1154.             #endif
  1155.             domeData.hpx = 30;
  1156.             domeData.dsp = 100;
  1157.             ET.sendData();
  1158.         }
  1159.         if(myPS->getButtonPress(RIGHT))
  1160.         {
  1161.             #ifdef SHADOW_DEBUG
  1162.               output += "Move Holo Right\r\n";
  1163.             #endif
  1164.             domeData.hpx = 150;
  1165.             domeData.dsp = 100;
  1166.             ET.sendData();
  1167.         }
  1168.         if (!(myPS->getButtonPress(UP)||myPS->getButtonPress(DOWN)))
  1169.         {
  1170.             domeData.hpy = 0;
  1171.             domeData.dsp = 100;
  1172.             ET.sendData();
  1173.         }
  1174.         if (!(myPS->getButtonPress(RIGHT)||myPS->getButtonPress(LEFT)))
  1175.         {
  1176.             domeData.hpx = 0;
  1177.             domeData.dsp = 100;
  1178.             ET.sendData();
  1179.         }
  1180.         return true;
  1181.     }
  1182.     if (myPS->getButtonPress(L1) &&controllerNumber == 2)
  1183.     {
  1184.             //TODO:  Analog stick - can we granually control holo?
  1185.             if(myPS->getAnalogHat(LeftHatY) < 50)
  1186.             {
  1187.                 output += "Move Holo Up\r\n";
  1188.                 domeData.hpy = 30;
  1189.                 domeData.dsp = 100;
  1190.                 ET.sendData();
  1191.             }        
  1192.             if(myPS->getAnalogHat(LeftHatY) > 200)
  1193.             {
  1194.                 output += "Move Holo Down\r\n";
  1195.                 domeData.hpy = 150;
  1196.                 domeData.dsp = 100;
  1197.                 ET.sendData();
  1198.             }
  1199.             if(myPS->getAnalogHat(LeftHatX) < 50)
  1200.             {
  1201.                 output += "Move Holo Left\r\n";
  1202.                 domeData.hpx = 30;
  1203.                 domeData.dsp = 100;
  1204.                 ET.sendData();
  1205.             }
  1206.             if(myPS->getAnalogHat(LeftHatX) > 200)
  1207.             {
  1208.                 output += "Move Holo Right\r\n";
  1209.                 domeData.hpx = 150;
  1210.                 domeData.dsp = 100;
  1211.                 ET.sendData();
  1212.             }
  1213.             if (!(myPS->getAnalogHat(LeftHatY) < 50||myPS->getAnalogHat(LeftHatY) > 200))
  1214.             {
  1215.                 //output += "Move Holo V-Center\r\n";
  1216.                 domeData.hpy = 0;
  1217.                 domeData.dsp = 100;
  1218.                 ET.sendData();
  1219.             }
  1220.             if (!(myPS->getAnalogHat(LeftHatX) < 50||myPS->getAnalogHat(LeftHatX) > 200))
  1221.             {
  1222.                 //output += "Move Holo H-Center\r\n";
  1223.                 domeData.hpx = 0;
  1224.                 domeData.dsp = 100;
  1225.                 ET.sendData();
  1226.             }
  1227.             return true;        
  1228.     }
  1229.     return false;
  1230. }
  1231. #endif
  1232.  
  1233. boolean ps3Holoprojector(PS3BT* myPS = PS3Nav, int controllerNumber = 1)
  1234. {  
  1235.       #ifdef DOME_I2C_ADAFRUIT          
  1236.           return adafruitPs3Holoprojector(myPS,controllerNumber);
  1237.       #endif
  1238.       #if defined(DOME_SERIAL_TEECES) || defined(DOME_I2C_TEECES)
  1239.           return teecesPs3Holoprojector(myPS,controllerNumber);
  1240.       #endif
  1241.       return false;
  1242. }
  1243.  
  1244.  
  1245. void ps3utilityArms(PS3BT* myPS = PS3Nav, int controllerNumber = 1)
  1246. {
  1247.   switch (controllerNumber)
  1248.     {
  1249.       case 1:
  1250.         if(myPS->getButtonPress(L1)&&myPS->getButtonClick(CROSS))
  1251.           {
  1252.               #ifdef SHADOW_DEBUG
  1253.                 output += "Opening/Closing top utility arm\r\n";
  1254.               #endif
  1255.              
  1256.                 waveUtilArm(UTIL_ARM_TOP);
  1257.           }
  1258.           if(myPS->getButtonPress(L1)&&myPS->getButtonClick(CIRCLE))
  1259.           {
  1260.               #ifdef SHADOW_DEBUG
  1261.                 output += "Opening/Closing bottom utility arm\r\n";
  1262.               #endif
  1263.              
  1264.                 waveUtilArm(UTIL_ARM_BOTTOM);
  1265.           }
  1266.         break;
  1267.       case 2:
  1268.         if (!(myPS->getButtonPress(L1)||myPS->getButtonPress(L2)||myPS->getButtonPress(PS)))
  1269.         {
  1270.           if(myPS->getButtonClick(CROSS))
  1271.           {
  1272.               #ifdef SHADOW_DEBUG
  1273.                 output += "Opening/Closing top utility arm\r\n";
  1274.               #endif
  1275.              
  1276.                 waveUtilArm(UTIL_ARM_TOP);
  1277.           }
  1278.           if(myPS->getButtonClick(CIRCLE))
  1279.           {
  1280.               #ifdef SHADOW_DEBUG
  1281.                 output += "Opening/Closing bottom utility arm\r\n";
  1282.               #endif
  1283.              
  1284.                 waveUtilArm(UTIL_ARM_BOTTOM);
  1285.           }
  1286.         }
  1287.         break;
  1288.     }
  1289. }
  1290.  
  1291. void utilityArms()
  1292. {
  1293.   if (PS3Nav->PS3NavigationConnected) ps3utilityArms(PS3Nav,1);
  1294.   if (PS3Nav2->PS3NavigationConnected) ps3utilityArms(PS3Nav2,2);
  1295. }
  1296.  
  1297. void ps3ToggleSettings(PS3BT* myPS = PS3Nav)
  1298. {
  1299.     if(myPS->getButtonPress(PS)&&myPS->getButtonClick(L3))
  1300.     {
  1301.       //Quick Shutdown of PS3 Controller
  1302.       output += "\r\nDisconnecting the controller.\r\n";
  1303.       myPS->disconnect();
  1304.     }
  1305.  
  1306.  
  1307.   //// enable / disable right stick & play sound
  1308.     if(myPS->getButtonPress(PS)&&myPS->getButtonClick(CROSS))
  1309.     {
  1310.         #ifdef SHADOW_DEBUG
  1311.           output += "Disiabling the DriveStick\r\n";
  1312.         #endif
  1313.         isStickEnabled = false;
  1314. //        trigger.play(52);
  1315.     }
  1316.     if(myPS->getButtonPress(PS)&&myPS->getButtonClick(CIRCLE))
  1317.     {
  1318.         #ifdef SHADOW_DEBUG
  1319.           output += "Enabling the DriveStick\r\n";
  1320.         #endif
  1321.         isStickEnabled = true;
  1322. ///        trigger.play(53);
  1323.     }
  1324.  
  1325.  
  1326. ////turn hp automation or automate on & off
  1327. //    if(myPS->getButtonPress(L1)&&myPS->getButtonClick(CIRCLE))
  1328. //    {
  1329. //        #ifdef SHADOW_DEBUG
  1330. //          output += "Enabling the Holo Automation\r\n";
  1331. //        #endif
  1332. //        //Turn On HP Automation
  1333. //        domeData.hpa = 1;
  1334. //        domeData.dsp = 100;
  1335. //        ET.sendData();
  1336. //    }
  1337. //    if(myPS->getButtonPress(L1)&&myPS->getButtonClick(CROSS))
  1338. //    {
  1339. //        #ifdef SHADOW_DEBUG
  1340. //          output += "Disabling the Holo Automation\r\n";
  1341. //        #endif
  1342. //        //Turn Off HP Automation
  1343. //        domeData.hpa = 0;
  1344. //        domeData.dsp = 100;
  1345. //        ET.sendData();
  1346. //    }
  1347.  
  1348.     if(myPS->getButtonPress(L2)&&myPS->getButtonClick(CROSS))
  1349.     {
  1350.         #ifdef SHADOW_DEBUG
  1351.           output += "Disabling the Dome Automation\r\n";        
  1352.         #endif
  1353.         isAutomateDomeOn = false;
  1354.         action = 0;
  1355. //        trigger.play(53);
  1356.     }
  1357.     if(myPS->getButtonPress(L2)&&myPS->getButtonClick(CIRCLE))
  1358.     {
  1359.         #ifdef SHADOW_DEBUG
  1360.           output += "Enabling the Dome Automation\r\n";
  1361.         #endif
  1362.         isAutomateDomeOn = true;
  1363. //        trigger.play(52);
  1364.     }
  1365.  
  1366.  
  1367.     /*
  1368.     ////Logic display brightness
  1369.         if(ps2x.ButtonPressed(PSB_PAD_UP))
  1370.         {
  1371.             if(ps2x.Button(PSB_L1))
  1372.             {
  1373.                 domeData.dsp = 24;
  1374.                 ET.sendData();
  1375.             }
  1376.         }
  1377.         if(ps2x.ButtonPressed(PSB_PAD_DOWN))
  1378.         {
  1379.             if(ps2x.Button(PSB_L1))
  1380.             {
  1381.                 domeData.dsp = 25;
  1382.                 ET.sendData();
  1383.             }
  1384.         }
  1385.     */
  1386.  
  1387. }
  1388.  
  1389. void processSoundCommand(char soundCommand)
  1390. {
  1391.     #ifdef SOUND_CFSOUNDIII
  1392.     //cfSound.playfile("happy.wav");  
  1393.     //cfSound.setVolume(20);
  1394.     switch (soundCommand)
  1395.     {
  1396.         case '+':
  1397.             #ifdef SHADOW_DEBUG    
  1398.               output += "Volume Up\r\n";
  1399.             #endif
  1400.             cfSound.volumeUp();
  1401.         break;
  1402.         case '-':
  1403.             #ifdef SHADOW_DEBUG
  1404.               output += "Volume Down\r\n";
  1405.             #endif
  1406.             cfSound.volumeDown();
  1407.         break;
  1408.         case '9':
  1409.             #ifdef SHADOW_DEBUG  
  1410.               output += "Sound Random OFF/Bank\r\n";
  1411.             #endif
  1412.             cfSound.sendButton('9');
  1413.         break;
  1414.         case '0':    
  1415.             #ifdef SHADOW_DEBUG
  1416.               output += "Sound Random ON/Bank\r\n";
  1417.             #endif    
  1418.             cfSound.sendButton('0');
  1419.         break;
  1420.         case '1':    
  1421.         case '2':    
  1422.         case '3':    
  1423.         case '4':    
  1424.         case '5':    
  1425.         case '6':    
  1426.         case '7':    
  1427.         case '8':
  1428.             #ifdef SHADOW_DEBUG    
  1429.               output += "Sound Button ";
  1430.               output += soundCommand;
  1431.               output += "\r\n";
  1432.             #endif
  1433.             cfSound.sendButton(soundCommand);
  1434.         break;
  1435.         default:
  1436.             #ifdef SHADOW_DEBUG
  1437.               output += "Invalid Sound Command\r\n";
  1438.             #endif
  1439.         break;
  1440.     }
  1441.     #endif
  1442.  
  1443.     #ifdef SOUND_MP3TRIGGER
  1444.     switch (soundCommand)
  1445.     {
  1446.         case '+':
  1447.             #ifdef SHADOW_DEBUG    
  1448.               output += "Volume Up\r\n";
  1449.             #endif
  1450.             if (vol>0)
  1451.             {
  1452.                 vol--;
  1453.                 trigger.setVolume(vol);
  1454.             }
  1455.         break;
  1456.         case '-':  
  1457.             #ifdef SHADOW_DEBUG
  1458.               output += "Volume Down\r\n";
  1459.             #endif
  1460.             if (vol<255)
  1461.             {
  1462.                 vol++;
  1463.                 trigger.setVolume(vol);
  1464.             }
  1465.         break;
  1466.        
  1467.         case '1':  
  1468.           #ifdef SHADOW_DEBUG    
  1469.             output += "Sound Button ";
  1470.             output += soundCommand;
  1471.             output += " - Play Sceam\r\n";
  1472.           #endif
  1473.           //Play Sceam
  1474.           trigger.play(1);  
  1475.           break;
  1476.         case '2':  
  1477.           #ifdef SHADOW_DEBUG    
  1478.             output += "Sound Button ";
  1479.             output += soundCommand;
  1480.             output += " - Play Wolf Whistle.\r\n";
  1481.           #endif        
  1482.           // Play Wolf Whistle
  1483.           trigger.play(4);
  1484.           break;
  1485.         case '3':    
  1486.           #ifdef SHADOW_DEBUG    
  1487.             output += "Sound Button ";
  1488.             output += soundCommand;
  1489.             output += " - Play Doo Doo\r\n";
  1490.           #endif        
  1491.           //Play Doo Doo
  1492.           trigger.play(3);
  1493.           break;
  1494.         case '4':    
  1495.           #ifdef SHADOW_DEBUG    
  1496.             output += "Sound Button ";
  1497.             output += soundCommand;
  1498.             output += " - Play Chortle\r\n";
  1499.           #endif        
  1500.           //Play Chortle
  1501.           trigger.play(2);
  1502.           break;
  1503.         case '5':    
  1504.           #ifdef SHADOW_DEBUG    
  1505.             output += "Sound Button ";
  1506.             output += soundCommand;
  1507.             output += " - Play Random Sentence.\r\n";
  1508.           #endif        
  1509.           // Play Random Sentence
  1510.           trigger.play(random(32,52));
  1511.           break;
  1512.         case '6':    
  1513.           #ifdef SHADOW_DEBUG    
  1514.             output += "Sound Button ";
  1515.             output += soundCommand;
  1516.             output += " - Play Random Misc.\r\n";
  1517.           #endif  
  1518.           //Play Random Misc.    
  1519.           trigger.play(random(17,25));
  1520.           break;
  1521.         case '7':    
  1522.           #ifdef SHADOW_DEBUG    
  1523.             output += "Sound Button ";
  1524.             output += soundCommand;
  1525.             output += " - Play Cantina Song.\r\n";
  1526.           #endif        
  1527.           //Play Cantina Song
  1528.           trigger.play(10);
  1529.           break;
  1530.         case '8':
  1531.             #ifdef SHADOW_DEBUG    
  1532.               output += "Sound Button ";
  1533.               output += soundCommand;
  1534.               output += " - Play Imperial March.\r\n";
  1535.             #endif
  1536.             //Play Imperial March
  1537.             trigger.play(11);
  1538.         break;
  1539.         case '9':
  1540.             #ifdef SHADOW_DEBUG    
  1541.               output += "Sound Button ";
  1542.               output += soundCommand;
  1543.               output += " - Play Let It Go.\r\n";
  1544.             #endif
  1545.             //Play Let It Go
  1546.             trigger.play(55);
  1547.         break;
  1548.         case '0':
  1549.             #ifdef SHADOW_DEBUG    
  1550.               output += "Sound Button ";
  1551.               output += soundCommand;
  1552.               output += " - Play Gangdum Style\r\n";
  1553.             #endif
  1554.             //Play Gangdum Style
  1555.             trigger.play(54);
  1556.         break;
  1557.         case 'A':
  1558.             #ifdef SHADOW_DEBUG    
  1559.               output += "Sound Button ";
  1560.               output += soundCommand;
  1561.               output += " - Play Upset a Droid\r\n";
  1562.             #endif
  1563.             //Play Upset a Droid
  1564.             trigger.play(57);
  1565.         break;
  1566.         case 'B':
  1567.             #ifdef SHADOW_DEBUG    
  1568.               output += "Sound Button ";
  1569.               output += soundCommand;
  1570.               output += " - Play Summer\r\n";
  1571.             #endif
  1572.             //Play Summer
  1573.             trigger.play(61);
  1574.         break;
  1575.         case 'C':
  1576.             #ifdef SHADOW_DEBUG    
  1577.               output += "Sound Button ";
  1578.               output += soundCommand;
  1579.               output += " - Play Everything is Awesome\r\n";
  1580.             #endif
  1581.             //Play Everything is Awesome
  1582.             trigger.play(62);
  1583.         break;
  1584.         case 'D':
  1585.             #ifdef SHADOW_DEBUG    
  1586.               output += "Sound Button ";
  1587.               output += soundCommand;
  1588.               output += " - Play What Does the Fox Say\r\n";
  1589.             #endif
  1590.             //Play What Does the Fox Say
  1591.             trigger.play(63);
  1592.         break;
  1593.         default:
  1594.             #ifdef SHADOW_DEBUG
  1595.               output += "Invalid Sound Command\r\n";
  1596.             #endif
  1597.             trigger.play(60);
  1598.   }
  1599.   #endif
  1600. }
  1601.  
  1602. void ps3soundControl(PS3BT* myPS = PS3Nav, int controllerNumber = 1)
  1603. {
  1604.  
  1605. #ifdef EXTRA_SOUNDS
  1606.     switch (controllerNumber)
  1607.     {
  1608.       case 1:
  1609. #endif
  1610.         if (!(myPS->getButtonPress(L1)||myPS->getButtonPress(L2)||myPS->getButtonPress(PS)))
  1611.         {
  1612.           if (myPS->getButtonClick(UP))          processSoundCommand('1');    
  1613.           else if (myPS->getButtonClick(RIGHT))  processSoundCommand('2');    
  1614.           else if (myPS->getButtonClick(DOWN))   processSoundCommand('3');    
  1615.           else if (myPS->getButtonClick(LEFT))   processSoundCommand('4');    
  1616.         } else if (myPS->getButtonPress(L2))
  1617.         {
  1618.           if (myPS->getButtonClick(UP))          processSoundCommand('5');    
  1619.           else if (myPS->getButtonClick(RIGHT))  processSoundCommand('6');    
  1620.           else if (myPS->getButtonClick(DOWN))   processSoundCommand('7');    
  1621.           else if (myPS->getButtonClick(LEFT))   processSoundCommand('8');    
  1622.         } else if (myPS->getButtonPress(L1))
  1623.         {
  1624.           if (myPS->getButtonClick(UP))          processSoundCommand('+');    
  1625.           else if (myPS->getButtonClick(DOWN))   processSoundCommand('-');    
  1626.           else if (myPS->getButtonClick(LEFT))   processSoundCommand('9');    
  1627.           else if (myPS->getButtonClick(RIGHT))  processSoundCommand('0');    
  1628.         }
  1629. #ifdef EXTRA_SOUNDS
  1630.         break;
  1631.       case 2:
  1632.         if (!(myPS->getButtonPress(L1)||myPS->getButtonPress(L2)||myPS->getButtonPress(PS)))
  1633.         {
  1634.           if (myPS->getButtonClick(UP))          processSoundCommand('A');    
  1635.           else if (myPS->getButtonClick(RIGHT))  processSoundCommand('B');    
  1636.           else if (myPS->getButtonClick(DOWN))   processSoundCommand('C');    
  1637.           else if (myPS->getButtonClick(LEFT))   processSoundCommand('D');    
  1638.         } else if (myPS->getButtonPress(L2))
  1639.         {
  1640.           if (myPS->getButtonClick(UP))          processSoundCommand('E');    
  1641.           else if (myPS->getButtonClick(RIGHT))  processSoundCommand('F');    
  1642.           else if (myPS->getButtonClick(DOWN))   processSoundCommand('G');    
  1643.           else if (myPS->getButtonClick(LEFT))   processSoundCommand('H');    
  1644.         } else if (myPS->getButtonPress(L1))
  1645.         {
  1646.           if (myPS->getButtonClick(UP))          processSoundCommand('I');    
  1647.           else if (myPS->getButtonClick(DOWN))   processSoundCommand('J');    
  1648.           else if (myPS->getButtonClick(LEFT))   processSoundCommand('K');    
  1649.           else if (myPS->getButtonClick(RIGHT))  processSoundCommand('L');    
  1650.         }
  1651.         break;
  1652.     }
  1653. #endif
  1654.  
  1655. }
  1656.  
  1657.  
  1658.  
  1659. void footMotorDrive()
  1660. {
  1661.   //Flood control prevention
  1662.   if ((millis() - previousFootMillis) < serialLatency) return;  
  1663.   if (PS3Nav->PS3NavigationConnected) ps3FootMotorDrive(PS3Nav);
  1664.   //TODO:  Drive control must be mutually exclusive - for safety
  1665.   //Future: I'm not ready to test that before FanExpo
  1666.   //if (PS3Nav2->PS3NavigationConnected) ps3FootMotorDrive(PS3Nav2);
  1667. }  
  1668.  
  1669. void domeDrive()
  1670. {
  1671.   //Flood control prevention
  1672.   //This is intentionally set to double the rate of the Foot Motor Latency
  1673.   if ((millis() - previousDomeMillis) < (2*serialLatency) ) return;  
  1674.  
  1675.  
  1676.   int domeRotationSpeed = 0;
  1677.   int ps3NavControlSpeed = 0;
  1678.   int ps3Nav2ControlSpeed = 0;
  1679.   if (PS3Nav->PS3NavigationConnected) ps3NavControlSpeed = ps3DomeDrive(PS3Nav,1);
  1680.   if (PS3Nav2->PS3NavigationConnected) ps3Nav2ControlSpeed = ps3DomeDrive(PS3Nav2,2);
  1681.  
  1682.   //In a two controller system, give dome priority to the secondary controller.
  1683.   //Only allow the "Primary" controller dome control if the Secondary is NOT spinnning it
  1684.  
  1685.   if ( abs(ps3Nav2ControlSpeed) > 0 )
  1686.   {
  1687.     domeRotationSpeed = ps3Nav2ControlSpeed;
  1688.   } else
  1689.   {
  1690.     domeRotationSpeed = ps3NavControlSpeed;
  1691.   }
  1692.   rotateDome(domeRotationSpeed,"Controller Move");
  1693. }  
  1694.  
  1695.  
  1696. void moveHoloServo(int pwmPIN, int pulse)
  1697. {
  1698.     domePWM.setPWM(pwmPIN, 0, pulse);
  1699. }
  1700.  
  1701. void holoLightFlicker(int pwmPINred, int pwmPINgreen, int pwmPINblue)
  1702. {
  1703.     int flicker = random(4096);
  1704.     domePWM.setPWM(pwmPINred, 0, flicker*0.75);
  1705.     domePWM.setPWM(pwmPINgreen, 0, flicker*0.75);
  1706.     domePWM.setPWM(pwmPINblue, 0, random(4096));
  1707. }
  1708.  
  1709. void holoLightOff(int pwmPINred, int pwmPINgreen, int pwmPINblue)
  1710. {
  1711.     domePWM.setPWM(pwmPINred, 0, PWM_OFF);
  1712.     domePWM.setPWM(pwmPINgreen, 0, PWM_OFF);
  1713.     domePWM.setPWM(pwmPINblue, 0, PWM_OFF);
  1714. }
  1715.  
  1716. void holoLightOn(int pwmPINred, int pwmPINgreen, int pwmPINblue)
  1717. {
  1718.     domePWM.setPWM(pwmPINred, 0, 4094);
  1719.     domePWM.setPWM(pwmPINgreen, 0, 4094);
  1720.     domePWM.setPWM(pwmPINblue, 0, 4094);
  1721. }
  1722.  
  1723. void randomHoloMovement(int holoprojector)
  1724. {
  1725.     currentMillis = millis();
  1726.  
  1727.     switch (holoprojector)
  1728.     {
  1729.       case HOLO_FRONT:  
  1730.           if (currentMillis > holoFrontRandomTime)
  1731.           {  
  1732.               holoFrontRandomTime = currentMillis + random(HOLO_DELAY);
  1733.               //TODO:  Determine range of Holoprojector X/Y better
  1734.                   //hpY=random(80,120);
  1735.                   //hpX=random(80,120);
  1736.               moveHoloServo(HOLO_FRONT_X_PWM_PIN, random(HOLO_FRONT_X_SERVO_MIN,HOLO_FRONT_X_SERVO_MAX));
  1737.               moveHoloServo(HOLO_FRONT_Y_PWM_PIN, random(HOLO_FRONT_Y_SERVO_MIN,HOLO_FRONT_Y_SERVO_MAX));
  1738.               int ledState = random(1,10);
  1739.               switch( ledState )
  1740.               {
  1741.                   case 0:
  1742.                   case 1:
  1743.                   case 2:
  1744.                   case 3:
  1745.                       holoLightFrontStatus = HOLO_LED_OFF;
  1746.                       holoLightOff(HOLO_FRONT_RED_PWM_PIN, HOLO_FRONT_GREEN_PWM_PIN, HOLO_FRONT_BLUE_PWM_PIN);
  1747.                       break;
  1748.                   case 4:
  1749.                   case 5:
  1750.                   case 6:
  1751.                   case 7:
  1752.                       holoLightFrontStatus = HOLO_LED_ON;
  1753.                       holoLightOn(HOLO_FRONT_RED_PWM_PIN, HOLO_FRONT_GREEN_PWM_PIN, HOLO_FRONT_BLUE_PWM_PIN);
  1754.                       break;
  1755.                   default:
  1756.                       holoLightFrontStatus = HOLO_LED_FLICKER;
  1757.                       break;
  1758.               }
  1759.           }
  1760.           if (holoLightFrontStatus == HOLO_LED_FLICKER)
  1761.           {
  1762.               holoLightFlicker(HOLO_FRONT_RED_PWM_PIN, HOLO_FRONT_GREEN_PWM_PIN, HOLO_FRONT_BLUE_PWM_PIN);
  1763.           }        
  1764.           break;
  1765.       case HOLO_BACK:
  1766.             if (currentMillis > holoBackRandomTime)
  1767.             {  
  1768.                 holoBackRandomTime = currentMillis + random(HOLO_DELAY*1.5);
  1769.                 //TODO:  Determine range of Holoprojector X/Y better
  1770.                     //hpY=random(80,120);
  1771.                     //hpX=random(80,120);
  1772.                 moveHoloServo(HOLO_BACK_X_PWM_PIN, random(HOLO_BACK_X_SERVO_MIN,HOLO_BACK_X_SERVO_MAX));
  1773.                 moveHoloServo(HOLO_BACK_Y_PWM_PIN, random(HOLO_BACK_Y_SERVO_MIN,HOLO_BACK_Y_SERVO_MAX));
  1774.                 int ledState = random(1,10);
  1775.                 switch( ledState )
  1776.                 {
  1777.                     case 0:
  1778.                     case 1:
  1779.                     case 2:
  1780.                     case 3:
  1781.                     case 4:
  1782.                         holoLightBackStatus = HOLO_LED_OFF;
  1783.                         holoLightOff(HOLO_BACK_RED_PWM_PIN, HOLO_BACK_GREEN_PWM_PIN, HOLO_BACK_BLUE_PWM_PIN);
  1784.                         break;
  1785.                     case 5:
  1786.                     case 6:
  1787.                     case 7:
  1788.                     case 8:
  1789.                         holoLightBackStatus = HOLO_LED_ON;
  1790.                         holoLightOn(HOLO_BACK_RED_PWM_PIN, HOLO_BACK_GREEN_PWM_PIN, HOLO_BACK_BLUE_PWM_PIN);
  1791.                         break;
  1792.                     default:
  1793.                         holoLightBackStatus = HOLO_LED_FLICKER;
  1794.                         break;
  1795.                 }
  1796.             }
  1797.             if (holoLightBackStatus == HOLO_LED_FLICKER)
  1798.             {
  1799.                 holoLightFlicker(HOLO_BACK_RED_PWM_PIN, HOLO_BACK_GREEN_PWM_PIN, HOLO_BACK_BLUE_PWM_PIN);
  1800.             }        
  1801.             break;
  1802.         case HOLO_TOP:  
  1803.             if (currentMillis > holoTopRandomTime)
  1804.             {  
  1805.                 holoTopRandomTime = currentMillis + random(HOLO_DELAY*1.5);
  1806.                 //TODO:  Determine range of Holoprojector X/Y better
  1807.                     //hpY=random(80,120);
  1808.                     //hpX=random(80,120);
  1809.                 moveHoloServo(HOLO_TOP_X_PWM_PIN, random(HOLO_TOP_X_SERVO_MIN,HOLO_TOP_X_SERVO_MAX));
  1810.                 moveHoloServo(HOLO_TOP_Y_PWM_PIN, random(HOLO_TOP_Y_SERVO_MIN,HOLO_TOP_Y_SERVO_MAX));
  1811.                 int ledState = random(1,10);
  1812.                 switch( ledState )
  1813.                 {
  1814.                     case 0:
  1815.                     case 1:
  1816.                     case 2:
  1817.                     case 3:
  1818.                     case 4:
  1819.                     case 5:
  1820.                         holoLightTopStatus = HOLO_LED_OFF;
  1821.                         holoLightOff(HOLO_TOP_RED_PWM_PIN, HOLO_TOP_GREEN_PWM_PIN, HOLO_TOP_BLUE_PWM_PIN);
  1822.                         break;
  1823.                     case 6:
  1824.                     case 7:
  1825.                     case 8:
  1826.                         holoLightTopStatus = HOLO_LED_ON;
  1827.                         holoLightOn(HOLO_TOP_RED_PWM_PIN, HOLO_TOP_GREEN_PWM_PIN, HOLO_TOP_BLUE_PWM_PIN);
  1828.                         break;
  1829.                     default:
  1830.                         holoLightTopStatus = HOLO_LED_FLICKER;
  1831.                         break;
  1832.                 }
  1833.             }
  1834.             if (holoLightTopStatus == HOLO_LED_FLICKER)
  1835.             {
  1836.                 holoLightFlicker(HOLO_TOP_RED_PWM_PIN, HOLO_TOP_GREEN_PWM_PIN, HOLO_TOP_BLUE_PWM_PIN);
  1837.             }        
  1838.             break;
  1839.       }  
  1840. }
  1841.  
  1842. void holoprojector()
  1843. {
  1844.    boolean isFrontHoloActivelyControlled = false;
  1845.    if (PS3Nav->PS3NavigationConnected)
  1846.    {
  1847.      if ( ps3Holoprojector(PS3Nav,1) )
  1848.      {
  1849.        isFrontHoloActivelyControlled = true;
  1850.      }
  1851.    }
  1852.    if (PS3Nav2->PS3NavigationConnected)
  1853.       {
  1854.      if ( ps3Holoprojector(PS3Nav2,2) )
  1855.      {
  1856.        isFrontHoloActivelyControlled = true;
  1857.      }
  1858.    }
  1859.   if (!isFrontHoloActivelyControlled) randomHoloMovement(HOLO_FRONT);
  1860.   randomHoloMovement(HOLO_BACK);
  1861.   randomHoloMovement(HOLO_TOP);
  1862. }  
  1863.  
  1864. void toggleSettings()
  1865. {
  1866.    if (PS3Nav->PS3NavigationConnected) ps3ToggleSettings(PS3Nav);
  1867.    if (PS3Nav2->PS3NavigationConnected) ps3ToggleSettings(PS3Nav2);
  1868. }  
  1869.  
  1870. void soundControl()
  1871. {
  1872.    if (PS3Nav->PS3NavigationConnected) ps3soundControl(PS3Nav,1);
  1873.    if (PS3Nav2->PS3NavigationConnected) ps3soundControl(PS3Nav2,2);
  1874.  
  1875.     // Read from cfsound, send to port USB Serial & BT Serial:
  1876.     #ifdef SOUND_CFSOUNDIII
  1877.     if (Serial1.available())
  1878.     {
  1879.       int inByte = Serial1.read();
  1880.       Serial.write(inByte);        
  1881.       if (SerialBT.connected)
  1882.       {
  1883.         SerialBT.write(inByte);
  1884.       }
  1885.     }
  1886.     #endif
  1887. }  
  1888.  
  1889.  
  1890. void openUtilArm(int arm, int position = utilArmOpenPos)
  1891. {
  1892.     //When passed a position - this can "partially" open the arms.
  1893.     //Great for more interaction
  1894.     moveUtilArm(arm, utilArmOpenPos);
  1895. }
  1896.  
  1897. void closeUtilArm(int arm)
  1898. {
  1899.     moveUtilArm(arm, utilArmClosedPos);
  1900. }
  1901.  
  1902. void waveUtilArm(int arm)
  1903. {
  1904.     switch (arm)
  1905.     {
  1906.       case UTIL_ARM_TOP:
  1907.         if(isUtilArmTopOpen == false){
  1908.           openUtilArm(UTIL_ARM_TOP);
  1909.         } else {
  1910.           closeUtilArm(UTIL_ARM_TOP);
  1911.         }
  1912.         break;
  1913.       case UTIL_ARM_BOTTOM:  
  1914.         if(isUtilArmBottomOpen == false){
  1915.           openUtilArm(UTIL_ARM_BOTTOM);
  1916.         } else {
  1917.           closeUtilArm(UTIL_ARM_BOTTOM);
  1918.         }
  1919.         break;
  1920.     }
  1921. }
  1922.  
  1923. void moveUtilArm(int arm, int position)
  1924. {
  1925.     switch (arm)
  1926.     {
  1927.       case UTIL_ARM_TOP:
  1928.         UtilArmTopServo.write(position);
  1929.         if ( position == utilArmClosedPos)
  1930.         {
  1931.           isUtilArmTopOpen = false;
  1932.         } else
  1933.         {
  1934.           isUtilArmTopOpen = true;
  1935.         }
  1936.         break;
  1937.       case UTIL_ARM_BOTTOM:  
  1938.         UtilArmBottomServo.write(position);
  1939.         if ( position == utilArmClosedPos)
  1940.         {
  1941.           isUtilArmBottomOpen = false;
  1942.         } else
  1943.         {
  1944.           isUtilArmBottomOpen = true;
  1945.         }
  1946.         break;
  1947.     }
  1948. }
  1949.  
  1950.  
  1951. void flashCoinSlotLEDs()
  1952. {
  1953.   for(int i = 0; i<numberOfCoinSlotLEDs; i++)
  1954.   {
  1955.     if(millis() > nextCoinSlotLedFlash[i])
  1956.     {
  1957.       if(coinSlotLedState[i] == LOW) coinSlotLedState[i] = HIGH;
  1958.       else coinSlotLedState[i] = LOW;
  1959.       digitalWrite(COIN_SLOT_LED_PINS[i],coinSlotLedState[i]);
  1960.       nextCoinSlotLedFlash[i] = millis()+random(100, 1000) ; // next toggle random time
  1961.     }
  1962.   }
  1963. }
  1964.  
  1965. #ifdef TEST_CONROLLER
  1966. void testPSController(PS3BT* myPS = PS3Nav)
  1967. {
  1968.     if (myPS->PS3Connected || myPS->PS3NavigationConnected) {
  1969.         if (myPS->getButtonPress(PS) && (myPS->getAnalogHat(LeftHatX) > 137 || myPS->getAnalogHat(LeftHatX) < 117 || myPS->getAnalogHat(LeftHatY) > 137 || myPS->getAnalogHat(LeftHatY) < 117 || myPS->getAnalogHat(RightHatX) > 137 || myPS->getAnalogHat(RightHatX) < 117 || myPS->getAnalogHat(RightHatY) > 137 || myPS->getAnalogHat(RightHatY) < 117)) {    
  1970.             output += "LeftHatX: ";
  1971.             output += myPS->getAnalogHat(LeftHatX);
  1972.             output += "\tLeftHatY: ";
  1973.             output += myPS->getAnalogHat(LeftHatY);
  1974.             if (myPS->PS3Connected) { // The Navigation controller only have one joystick
  1975.                 output += "\tRightHatX: ";
  1976.                 output += myPS->getAnalogHat(RightHatX);
  1977.                 output += "\tRightHatY: ";
  1978.                 output += myPS->getAnalogHat(RightHatY);
  1979.             }
  1980.         }
  1981.         //Analog button values can be read from almost all buttons
  1982.         if (myPS->getButtonPress(PS) && (myPS->getAnalogButton(L2) || myPS->getAnalogButton(R2)))
  1983.         {
  1984.             if (output != "")
  1985.                 output += "\r\n";
  1986.             output += "L2: ";
  1987.             output += myPS->getAnalogButton(L2);
  1988.             if (myPS->PS3Connected) {
  1989.                 output += "\tR2: ";
  1990.                 output += myPS->getAnalogButton(R2);
  1991.             }
  1992.         }
  1993.         if (myPS->getButtonClick(L2)) {
  1994.             output += " - L2";
  1995.             //myPS->disconnect();
  1996.         }
  1997.         if (myPS->getButtonClick(R2)) {
  1998.             output += " - R2";
  1999.             //myPS->disconnect();
  2000.         }
  2001.         if (output != "") {
  2002.             Serial.println(output);
  2003.             if (SerialBT.connected)
  2004.                 SerialBT.println(output);
  2005.             output = ""; // Reset output string
  2006.         }
  2007.         if (myPS->getButtonClick(PS)) {
  2008.             output += " - PS";
  2009.             //myPS->disconnect();
  2010.         }
  2011.         else {
  2012.             if (myPS->getButtonClick(TRIANGLE))
  2013.                 output += " - Traingle";
  2014.             if (myPS->getButtonClick(CIRCLE))
  2015.                 output += " - Circle";
  2016.             if (myPS->getButtonClick(CROSS))
  2017.                 output += " - Cross";
  2018.             if (myPS->getButtonClick(SQUARE))
  2019.                 output += " - Square";
  2020.  
  2021.             if (myPS->getButtonClick(UP)) {
  2022.                 output += " - Up";
  2023.                 if (myPS->PS3Connected) {
  2024.                     myPS->setLedOff();
  2025.                     myPS->setLedOn(LED4);
  2026.                 }
  2027.             }
  2028.             if (myPS->getButtonClick(RIGHT)) {
  2029.                 output += " - Right";
  2030.                 if (myPS->PS3Connected) {
  2031.                     myPS->setLedOff();
  2032.                     myPS->setLedOn(LED1);
  2033.                 }
  2034.             }
  2035.             if (myPS->getButtonClick(DOWN)) {
  2036.                 output += " - Down";
  2037.                 if (myPS->PS3Connected) {
  2038.                     myPS->setLedOff();
  2039.                     myPS->setLedOn(LED2);
  2040.                 }
  2041.             }
  2042.             if (myPS->getButtonClick(LEFT)) {
  2043.                 output += " - Left";
  2044.                 if (myPS->PS3Connected) {
  2045.                     myPS->setLedOff();
  2046.                     myPS->setLedOn(LED3);
  2047.                 }
  2048.             }
  2049.  
  2050.             if (myPS->getButtonClick(L1))
  2051.                 output += " - L1";
  2052.             if (myPS->getButtonClick(L3))
  2053.                 output += " - L3";
  2054.             if (myPS->getButtonClick(R1))
  2055.                 output += " - R1";
  2056.             if (myPS->getButtonClick(R3))
  2057.                 output += " - R3";
  2058.  
  2059.             if (myPS->getButtonClick(SELECT)) {
  2060.                 output += " - Select";
  2061.                 myPS->printStatusString();
  2062.             }
  2063.             if (myPS->getButtonClick(START)) {
  2064.                 output += " - Start";
  2065.             }
  2066.         }
  2067.     }          
  2068. }
  2069. #endif
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
 
Top