 
 void RFduinoBLE_onDisconnect() {
   state = SBTSailbotModelStateRecoveryMode;
+  timeOnTackSequence = 0; // reset time on current tack
+  lastAutomaticUpdate = millis();
+  automaticHeading = angleSubtractf(heading, M_PI);
   Serial.println("Bluetooth disconnect");
 }
 

  Wire.begin();
  remoteState = SBTSailbotModelStateManualControl;
  windDirection = -1.0;
  tackAngle = 90.0 * M_PI / 180.0;
  tackTime = 10.0 * 1000.0; // 20 seconds
  calibratedWind = false;
  imu = RTIMU::createIMU(&settings);                        // create the imu object



  tillerServo.attach(2);
  sheetServo.attach(3);
  setRudder(0.0);
  setSheet(1.0);

  RFduinoBLE.advertisementInterval = 675;
  RFduinoBLE.advertisementData = "Sailbot";
  RFduinoBLE.begin();
  RFduinoBLE.txPowerLevel = +4;

  lastDataPacketSent = millis();
  lastAutomaticUpdate = millis();
}

// Rudder is from -1.0 to 1.0
void setRudder(float newRudder) {
  rudder = newRudder;
  // Setting the servo to zero does not work so we add 0.4 radians
  tillerServo.write((-rudder * MAX_RUDDER + MAX_RUDDER + 0.4) * 180.0 / M_PI);
}

// Sheet is from 0.0 to 1.0 where 0 is close hauled
void setSheet(float newSheet) {
  sheet = newSheet;
  float adjustedSheet = (SHEET_MAX - SHEET_MIN) * (1.0 - sheet) + SHEET_MIN;
  sheet = 2.0 * atan(sqrt( (-pow(SHEET_ARM_TO_EXIT, 2.0) + pow(adjustedSheet, 2.0)) / (pow(SHEET_ARM_TO_EXIT, 2.0) + 4.0 * SHEET_ARM_TO_EXIT * SHEET_ARM_LENGTH + 4.0 * pow(SHEET_ARM_LENGTH, 2.0) - pow(adjustedSheet, 2.0)))) * 180.0 / M_PI;
  sheetServo.write(sheet + 10.0);
}

void loop() {
