 
     // Update rudder and sheet
     if (state == SBTSailbotModelStateAutomaticControl) {
+      timeOnTackSequence += now - lastAutomaticUpdate;
+      if (timeOnTackSequence > tackTime * 2)
+        timeOnTackSequence = 0;
       float rudderAngle;
       float angleToWind = angleSubtractf(windDirection, automaticHeading);
       if (abs(angleToWind) < tackAngle / 2.0) {
         float tack = angleToWind > 0 ? tackAngle / 2.0 : -tackAngle / 2.0;
+        if (timeOnTackSequence > tackTime)
+          tack = -tack;
         rudderAngle = angleSubtractf(angleSubtractf(windDirection, tack), heading);
-        Serial.print("Angle towards wind: "); Serial.println(angleToWind * 180.0 / M_PI);
       } else {
         rudderAngle = angleSubtractf(automaticHeading, heading);
       }
+      lastAutomaticUpdate = now;
       setRudder(40.0 * rudderAngle / M_PI);
 
     } else if (state == SBTSailbotModelStateRecoveryMode) {

    memcpy(&buffer[14], &sheet, sizeof(sheet));

    RFduinoBLE.send((char*)&buffer, 2 + 4 * sizeof(float));
    Serial.print("Heading: "); Serial.print(heading);
    Serial.print(" Automatic heading: "); Serial.print(automaticHeading);
    Serial.print(" Wind direction: "); Serial.println(windDirection);
  }
}


  RFduinoBLE.begin();

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

void setRudder(float newAngle) {
