Balanduino dev

Balanduino wdt.diff

diff --git a/Firmware/Balanduino/Balanduino.h b/Firmware/Balanduino/Balanduino.h
index 338024d..45039f1 100644
--- a/Firmware/Balanduino/Balanduino.h
+++ b/Firmware/Balanduino/Balanduino.h
@@ -189,6 +189,12 @@ uint32_t spekConnectedTimer; // Timer used to check if the connection is dropped
 #define RC_CHAN_AUX4     7
 #endif
 
+uint8_t resetFlags __attribute__ ((section(".noinit"))); // Source: https://code.google.com/p/optiboot/issues/attachmentText?id=66&aid=660004000&name=resetFlags_appCode.cpp
+void resetFlagsInit(void) __attribute__ ((naked)) __attribute__ ((section (".init0")));
+void resetFlagsInit(void) {
+  __asm__ __volatile__ ("mov %0, r2\n" : "=r" (resetFlags) :); // Save the reset flags passed from the bootloader
+}
+
 // Encoder values
 #if defined(PIN_CHANGE_INTERRUPT_VECTOR_LEFT) && defined(PIN_CHANGE_INTERRUPT_VECTOR_RIGHT)
 const uint16_t zoneA = 8000 * 2;
diff --git a/Firmware/Balanduino/Balanduino.ino b/Firmware/Balanduino/Balanduino.ino
index a7a11d5..399c8b4 100644
--- a/Firmware/Balanduino/Balanduino.ino
+++ b/Firmware/Balanduino/Balanduino.ino
@@ -33,11 +33,16 @@
 #define ENABLE_XBOX
 #define ENABLE_ADK
 #define ENABLE_SPEKTRUM
+//#define ENABLE_WATCHDOG
 
 #include "Balanduino.h"
 #include <Arduino.h> // Standard Arduino header
 #include <Wire.h> // Official Arduino Wire library
 
+#ifdef ENABLE_WATCHDOG
+#include <avr/wdt.h> // Watchdog timer handling
+#endif
+
 #ifdef ENABLE_ADK
 #include <adk.h>
 #endif
@@ -117,6 +122,10 @@ WII Wii(&Btd); // The Wii library can communicate with Wiimotes and the Nunchuck
 #endif
 
 void setup() {
+#ifdef ENABLE_WATCHDOG
+  wdt_disable(); // Disable watchdog timer
+#endif
+
   /* Setup buzzer pin */
   buzzer::SetDirWrite();
 
@@ -257,9 +266,16 @@ void setup() {
 #endif
 
   /* Beep to indicate that it is now ready */
-  buzzer::Set();
-  delay(100);
-  buzzer::Clear();
+  uint8_t beepCount = 1;
+  if (resetFlags & (1 << WDRF)) // If watchdog timer caused the reset then beep two times
+    beepCount = 2; // Since the watchdog timer is also used to reboot the bootloader it will also boot twice when pressing the reset button
+
+  for (uint8_t i = 0; i < beepCount; i++) {
+      buzzer::Set();
+      delay(50);
+      buzzer::Clear();
+      delay(50);
+  }
 
   /* Setup timing */
   kalmanTimer = micros();
diff --git a/Firmware/Balanduino/I2C.ino b/Firmware/Balanduino/I2C.ino
index 26709fe..67a1f73 100644
--- a/Firmware/Balanduino/I2C.ino
+++ b/Firmware/Balanduino/I2C.ino
@@ -15,6 +15,8 @@
  e-mail   :  kristianl@tkjelectronics.com
 */
 
+#define WATCHDOG_TIME WDTO_2S // Set watchdog timer to 2 seconds
+
 const uint8_t IMUAddress = 0x68; // AD0 is logic low on the PCB
 const uint16_t I2C_TIMEOUT = 100; // Used to check for errors in I2C communication
 
@@ -23,6 +25,10 @@ uint8_t i2cWrite(uint8_t registerAddress, uint8_t data, bool sendStop) {
 }
 
 uint8_t i2cWrite(uint8_t registerAddress, uint8_t *data, uint8_t length, bool sendStop) {
+#ifdef ENABLE_WATCHDOG
+  wdt_reset(); // Reset watchdog timer
+  wdt_enable(WATCHDOG_TIME); // Enable watchdog timer
+#endif
   Wire.beginTransmission(IMUAddress);
   Wire.write(registerAddress);
   Wire.write(data, length);
@@ -31,10 +37,18 @@ uint8_t i2cWrite(uint8_t registerAddress, uint8_t *data, uint8_t length, bool se
     Serial.print(F("i2cWrite failed: "));
     Serial.println(rcode);
   }
+#ifdef ENABLE_WATCHDOG
+  else
+    wdt_disable(); // Disable watchdog timer
+#endif
   return rcode; // See: http://arduino.cc/en/Reference/WireEndTransmission
 }
 
 uint8_t i2cRead(uint8_t registerAddress, uint8_t *data, uint8_t nbytes) {
+#ifdef ENABLE_WATCHDOG
+  wdt_reset(); // Reset watchdog timer
+  wdt_enable(WATCHDOG_TIME); // Enable watchdog timer
+#endif
   uint32_t timeOutTimer;
   Wire.beginTransmission(IMUAddress);
   Wire.write(registerAddress);
@@ -59,5 +73,8 @@ uint8_t i2cRead(uint8_t registerAddress, uint8_t *data, uint8_t nbytes) {
       }
     }
   }
+#ifdef ENABLE_WATCHDOG
+  wdt_disable(); // Disable watchdog timer
+#endif
   return 0; // Success
 }

pan & tilt.diff

diff --git a/Firmware/Balanduino/Balanduino.h b/Firmware/Balanduino/Balanduino.h
index 338024d..a0cdd8f 100644
--- a/Firmware/Balanduino/Balanduino.h
+++ b/Firmware/Balanduino/Balanduino.h
@@ -244,6 +244,7 @@ int32_t readLeftEncoder();
 int32_t readRightEncoder();
 int32_t getWheelsPosition();
 
+void initPanTilt();
 void bindSpektrum();
 void readSpektrum(uint8_t input);
 
diff --git a/Firmware/Balanduino/Balanduino.ino b/Firmware/Balanduino/Balanduino.ino
index a7a11d5..8d35dcb 100644
--- a/Firmware/Balanduino/Balanduino.ino
+++ b/Firmware/Balanduino/Balanduino.ino
@@ -181,6 +181,10 @@ void setup() {
   // Set OC1A/OC1B on compare match when down-counting
   TCCR1A = (1 << COM1A1) | (1 << COM1B1);
 
+#ifdef ENABLE_SPEKTRUM
+  initPanTilt();
+#endif
+
 #ifdef ENABLE_USB
   if (Usb.Init() == -1) { // Check if USB Host is working
     Serial.print(F("OSC did not start"));
diff --git a/Firmware/Balanduino/Spektrum.ino b/Firmware/Balanduino/Spektrum.ino
index 4a3aa49..d328c02 100644
--- a/Firmware/Balanduino/Spektrum.ino
+++ b/Firmware/Balanduino/Spektrum.ino
@@ -17,6 +17,10 @@
 
 #ifdef ENABLE_SPEKTRUM
 
+#include <Servo.h> // Official Arduino Servo library
+Servo tilt; // Create the instances for the pan & tilt servos
+Servo pan;
+
 #define SPEKTRUM 1024 // Set to either 1024 or 2048 depending on how it should bind with the receiver
 
 #define RC_CHANS 12
@@ -39,6 +43,11 @@ uint32_t spekTimer; // Used to check the time between messages, this is used to
   #define SPEK_BIND_PULSES 5
 #endif
 
+void initPanTilt() {
+  tilt.attach(3); // Tilt servo on pin 3
+  pan.attach(4); // Pan servo on pin 4
+}
+
 // Bind code inspired by David Thompson: https://code.google.com/p/nextcopterplus/source/browse/trunk/OpenAero2/src/misc_asm.S
 void bindSpektrum() {
   spektrumBindPin::Clear();
@@ -80,6 +89,10 @@ void readSpektrum(uint8_t input) {
     spekIndex = 0;
     spekConnected = true;
     spekConnectedTimer = millis();
+
+    tilt.write(map(rcValue[RC_CHAN_THROTTLE], 1000, 2000, 0, 180)); // Update pan and tilt position
+    pan.write(map(rcValue[RC_CHAN_YAW], 1000, 2000, 0, 180));
+
 #if 0 // Set this to 1 to print the channel values
     for (uint8_t i = 0; i < RC_CHANS; i++) {
       Serial.print(rcValue[i]);