Code makes the difference between a small easy project using one thing on its own and the trouble of trying to combine different things. You can get lost in dependency hell pretty quickly.

I'm not a professional in any way so don't use my code to deliver insulin/beer with the Roomba. If my stuff doesn't blow up (when not intended) or becomes self aware and starts crying (when not intended) I'm already pretty happy. 

To clean up the mess of code I have now, I have a few considerations. I want the code to do a few things asynchronous:

• Keep the WiFi connected;
• Run the OTA handler;
• Receive and interpret commands over MQTT;
• Publish status updates over MQTT;
• Sending commands to the Roomba when needed;
• Requesting, receiving and interpreting Roomba serial data stream;
• Communication with the IMU and interpreting the results;
• Do calculations and make decisions based on location and other inputs; 

For this to work, I can't use any delays or while loops in my code. It's gonna be state machines all the way. 

To restructure everything and wrap my head around it all, I started out with some pseudocode:

Includes
    - Roomba lib
    - Extras lib
    - OTA lib
    - pub-sub lib
    - MPU lib
    - I2C lib
    - JSON lib
Constants/defines:
    - WiFi details
        - SSID
        - Password
    - OTA details
        - Hostname
        - Password
    - IMU details
        - I2C address
        - MPU offsets
            - Xgyro, Ygyro, Zgyro, Zaccel
        - Callibration settings
    - MQTT details
        - Broker details
            - Server address
            - User
            - Password
        - Set/State topic
    - Extra details
        - LED pin
        - Debug level
Global vars:
    - Flags (volatile bool):
        - IMU GPIO interrupt flag
        - In Roomba class:
            - Sensors/data updated/ready
            - Bumped/wheel drop/cliff event
            - Charging flag
        - 
    - States (char or uint8_t):
        - MQTT received command
        - In Roomba class:
            - Roomba state high level: charging, cleaning, driving, idle, off?
            - (Private) sub state
            - (Private) State for receiving data stream
            - (Private) State for sending commands
    - Roomba632 class:
        - Battery voltage
        - Battery charge
        - Current
        - Angle
        - Distance
    - MPU6050 class
        - MPU control/status vars
    - Orientation/motion vars
        - Cartesian local X,Y
        - Orientation
        - Track waypoint as JSON?
    - Timers (uint16_t):
        - Location calculation
        - In Roomba class:
            - Timer playing songs
    - WiFiCLient class
    - PubSubCLient class
Callbacks
- MQTT
    - Message received
        - check first char and set flags appropriately
    - reconnect with callback?
- IMU interrupt
    - set flag
Setup()
    - Serial start
    - WiFi start
    - OTA setup, Set on start/end/error, start OTA
    - MQTT setup callbacks, (re)connect/subscribe, start MQTT
    - I2C setup, start
    - Init IMU, callibrate, set ISR 
    - Extras
OTA handler()
    - 
Roomba handler()
    - in class:
        - public Functions set a flag (and variables) as a "request" witch is handled in order of priority:
            Clean()
            Dock()
            Spot()
            Stop()
            StartdataStream()
            StopDataStream()
            PlayMusic(song)
            Drive(with all variables)
        - private functions for stuff that's asynchronous:
            _StartDataStream()
                - Start, send request command
                - Datapackets requested:
                    - 15 Dirt detect uint8_t
                    - 22 Voltage uint16_t
                    - 23 Current int16_t
                    - 24 Battery temp int8_t
                    - 25 Battery charge uint16_t
                    - 26 Battery capacity uint16_t
                    - 35 Open interface mode uint8_t
                    - 37 Song playing? uint8_t
                    - 43 Encoder counts left int16_t
                    - 44 Encoder counts right int16_t    
            _StopDataStream()
                - Start, send command
        - High level states. Can only be in one state at the time. Only change state from within state machine:
            - IDLE
                - keep alive
                - Check charging flag
            - Charging
                - Stop 
                - Report charging state
            - Cleaning
                - Start, clean command, start datastream
                - Check charging flag
            - Driving
                - Start, full mode, evil lights, start datastream, drive
                - Check bump/stop etc flag
                - Check charging flag
            - Music
                - Start, full mode, evil lights, set timer, send song/command
        - Receiving data / stream
            (sensordatastream is one burst of max 172 bytes every 15ms at baudrate of115200 wich is plenty,
            refresrate of chargingdata is 1Hz)
            - Check if charging. Set charging flag
            - if not charging, get serial sensordata in order and update values. Check validity by checksum
        
MQTT message handler()
    - command received handler
        - States
Loop()
    - OTA handler
    - MQTT send (JSON / GPX files?)    
    - Roomba message handler
        - State machine for sending commands and receiving response
    - Roomba handler
        - State machine
            - Charging
            - Driving
            - Cleaning
            - Music
    - Location calculation / logging
    - Extras handler

 I think I'll start with the Roomba class in it's own library...