crew_dragon_iss_docking_pid_controller.js

/*
Copyright 2020 Ricardo Zilleruelo

Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation
files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy,
modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the
Software is furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all copies or substantial portions of the
Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE
WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/

/*
The controller code is designed to be injected into the SPACEX - ISS Docking Simulator (https://iss-sim.spacex.com/).
Within the simulator webpage open the javascript console.

1. Paste the code in this gist.

2. Start controller:
```javascript
let regimeController = startRegimeController(config);
let controller = startControl(config);
```

3. Stop controller:
Use following code to stop controller
```javascript
controller.stop();
regimeController.stop();
```
*/

const getDistanceValue = function(sensor) {
    return parseFloat(sensor.textContent.replace(' m', ''));
};

const getDegreeValue = function(sensor) {
    return parseFloat(sensor.textContent.replace('°', ''));
};

const getDegreeRateValue = function(sensor) {
    return parseFloat(sensor.textContent.replace(' °/s', ''));
};

const getDistanceRateValue = function(sensor) {
    return parseFloat(sensor.textContent.replace(' m/s', ''));
};

const getRadiansValue = function(sensor) {
    return parseFloat(sensor.textContent.replace('°', '')) * Math.PI / 180.0;
};

const mapAngleToFullAngle = (angle) => {
  if (0 <= angle && angle <= 180.0) {
    return angle;
  }
  return angle + 360.0;
};
 
const getDegreesFullAngle = (source) => {
  return mapAngleToFullAngle(getDegreeValue(source));
};

const pitchSource = $x('/html//div[@id="pitch"]/*[1]')[0];
const yawSource = $x('/html//div[@id="yaw"]/*[1]')[0];
const rollSource = $x('/html//div[@id="roll"]/*[1]')[0];
const xSource =  $x('/html//div[@id="x-range"]/div')[0];
const ySource =  $x('/html//div[@id="y-range"]/div')[0];
const zSource =  $x('/html//div[@id="z-range"]/div')[0];
const rollValueSource = $x('/html//div[@id="roll"]/*[1]')[0];
const rollRateSource = $x('/html//div[@id="roll"]/*[2]')[0];
const pitchValueSource = $x('/html//div[@id="pitch"]/*[1]')[0];
const pitchRateSource = $x('/html//div[@id="pitch"]/*[2]')[0];
const yawValueSource = $x('/html//div[@id="yaw"]/*[1]')[0];
const yawRateSource = $x('/html//div[@id="yaw"]/*[2]')[0];
const rangeSource = $x('/html//div[@id="range"]/*[2]')[0];
const rateSource = $x('/html//div[@id="rate"]/*[2]')[0];

const initializeTranslationController = function(config, sensor, project) {
    let lastValue = getDistanceValue(sensor);
    let lastT = window.performance.now();
    let rateUpdateIndex = 0;
    const state = {
        t: window.performance.now(),
        value: lastValue,
        error: 0.0,
        errorSum: 0.0,
        rate: 0.0,
        kp: config.kp,
        ki: config.ki,
        kd: config.kd,
    };
    const control = function() {
        state.t = window.performance.now();
        state.value = getDistanceValue(sensor);
        state.error = config.target - state.value;
        if (rateUpdateIndex % config.rateRefreshFrequency == 0) {
            state.rate = (state.value - lastValue) / (state.t - lastT) * 1000.0;
            lastValue = state.value;
            lastT = state.t;
            rateUpdateIndex = 0;
        }
        rateUpdateIndex += 1;

        state.errorSum += state.error;
        if (state.errorSum > config.maxErrorSum) {
            state.errorSum = config.maxErrorSum;
        } else if (state.errorSum < -config.maxErrorSum) {
            state.errorSum = -config.maxErrorSum;
        }
        if ((state.errorSum > 0.0 && state.error < 0) || (state.errorSum < 0.0 && state.error > 0.0)) {
            state.errorSum = 0.0;
        }
        state.kp = config.kp;
        state.ki = config.ki;
        state.kd = config.kd;
        const u = config.kp * state.error + config.ki * state.errorSum - config.kd * state.rate;
        return project(u);
    };

    return {control: control, state: state};
};

const initializeAttitudeController = function(config,  valueSensor,  rateSensor) {
    const state = {
        t: window.performance.now(),
        value: getDegreesFullAngle(valueSensor),
        error: 0.0,
        errorSum: 0.0,
        rate: getDegreeRateValue(rateSensor),
        kp: config.kp,
        ki: config.ki,
        kd: config.kd,
    };
    const control = function() {
        state.rate = getDegreeRateValue(rateSensor);
        state.value = getDegreesFullAngle(valueSensor);
        const errorA = config.target - state.value;
        const errorB = config.target + 360.0 - state.value;
        state.error = Math.abs(errorA) < Math.abs(errorB) ? errorA : errorB;
        state.errorSum += state.error;
        if (state.errorSum > config.maxErrorSum) {
            state.errorSum = config.maxErrorSum;
        } else if (state.errorSum < -config.maxErrorSum) {
            state.errorSum = -config.maxErrorSum;
        }
        if ((state.errorSum > 0.0 && state.error < 0) || (state.errorSum < 0.0 && state.error > 0.0)) {
            state.errorSum = 0.0;
        }
        state.kp = config.kp;
        state.ki = config.ki;
        state.kd = config.kd;
        const u = config.kp * state.error + config.ki * state.errorSum + config.kd * state.rate;
        return u;
    };
    return {control: control, state: state};
};

const initializeXController = function(config) {
    return initializeTranslationController(config.x, xSource, function(u) {
        const roll = getRadiansValue(rollSource);
        const pitch = getRadiansValue(pitchSource);
        const yaw = getRadiansValue(yawSource);
        const ux = u * Math.cos(yaw) * Math.cos(pitch);
        const uy = u * (Math.cos(yaw) * Math.sin(roll) * Math.sin(pitch) - Math.cos(roll) * Math.sin(yaw));
        const uz = -u * (Math.sin(roll) * Math.sin(yaw) + Math.cos(roll) * Math.cos(yaw) * Math.sin(pitch));
        return {'x': ux, 'y': uy, 'z': uz};
    });
};

const initializeYController = function(config) {
    return initializeTranslationController(config.y, ySource, function(u) {
        const roll = getRadiansValue(rollSource);
        const pitch = getRadiansValue(pitchSource);
        const yaw = getRadiansValue(yawSource);
        const ux = u * Math.cos(pitch) * Math.sin(yaw);
        const uy = u * (Math.cos(roll) * Math.cos(yaw) + Math.sin(roll) * Math.sin(yaw) * Math.sin(pitch));
        const uz = -u * (Math.cos(roll) * Math.sin(yaw) * Math.sin(pitch) - Math.cos(yaw) * Math.sin(roll));
        return {'x': ux, 'y': uy, 'z': uz};
    });
};

const initializeZController = function(config) {
    return initializeTranslationController(config.z, zSource, function(u) {
        const roll = getRadiansValue(rollSource);
        const pitch = getRadiansValue(pitchSource);
        const yaw = getRadiansValue(yawSource);
        const ux = -u * Math.sin(pitch);
        const uy = u * Math.cos(pitch) * Math.sin(roll);
        const uz = -u * Math.cos(roll) * Math.cos(pitch);
        return {'x': ux, 'y': uy, 'z': uz};
    });
};

const initializeRollController = function(config) {
    return initializeAttitudeController(config.roll,  rollValueSource, rollRateSource);
};

const initializeYawController = function(config) {
    return initializeAttitudeController(config.yaw, yawValueSource, yawRateSource);
};

const initializePitchController = function(config) {
    return initializeAttitudeController(config.pitch, pitchValueSource, pitchRateSource);
};

const startControl = function(config) {
    const log = [];
    const xController = initializeXController(config);
    const yController = initializeYController(config);
    const zController = initializeZController(config);
    let rollController = initializeRollController(config);
    let yawController = initializeYawController(config);
    let pitchController = initializePitchController(config);

    const control = function() {
        const xU = xController.control();
        const yU = yController.control();
        const zU = zController.control();
        let ux = [xU.x, yU.x, zU.x].reduce(function(a, b) {return a + b}) / 3;
        let uy = [xU.y, yU.y, zU.y].reduce(function(a, b) {return a + b}) / 3;
        let uz = [xU.z, yU.z, zU.z].reduce(function(a, b) {return a + b}) / 3;
        let rollU = rollController.control();
        let yawU = yawController.control();
        let pitchU = pitchController.control();
        const logEntry = {
            x: xController.state.value,
            xInput: 0.0,
            y: yController.state.value,
            yInput: 0.0,
            z: zController.state.value,
            zInput: 0.0,
            roll: rollController.state.value,
            rollInput: 0.0,
            yaw: yawController.state.value,
            yawInput: 0.0,
            pitch: pitchController.state.value,
            pitchInput: 0.0,
        };
        log.push(logEntry);
        if (ux > config.x.actionThreshold) {
            $('#translate-backward-button').click();
            logEntry.xInput = -1.0;
        } else if (ux < -config.x.actionThreshold) {
            $('#translate-forward-button').click();
            logEntry.xInput = 1.0;
        }
        if (uy > config.y.actionThreshold) {
            $('#translate-right-button').click();
            logEntry.yInput = 1.0;
        } else if (uy < -config.y.actionThreshold) {
            $('#translate-left-button').click();
            logEntry.yInput = -1.0;
        }
        if (uz > config.z.actionThreshold) {
            $('#translate-down-button').click();
            logEntry.zInput = -1.0;
        } else if (uz < -config.z.actionThreshold) {
            $('#translate-up-button').click();
            logEntry.zInput = 1.0;
        }
        if (rollU > config.roll.actionThreshold) {
            $('#roll-left-button').click();
            logEntry.rollInput = -1.0;
        } else if (rollU < -config.roll.actionThreshold) {
            $('#roll-right-button').click();
            logEntry.rollInput = 1.0;
        }
        if (yawU > config.yaw.actionThreshold) {
            $('#yaw-left-button').click()
            logEntry.yawInput = -1.0;
        } else if (yawU < -config.yaw.actionThreshold) {
            $('#yaw-right-button').click()
            logEntry.yawInput = 1.0;
        }
        if (pitchU > config.pitch.actionThreshold) {
            $('#pitch-up-button').click();
            logEntry.pitchInput = 1.0;
        } else if (pitchU < -config.pitch.actionThreshold) {
            $('#pitch-down-button').click();
            logEntry.pitchInput = -1.0;
        }
    };
    const interval = setInterval(control, config.controlRate);
    return {stop: () => { clearInterval(interval); }, log: log};
};

const config = {
    controlRate: 100,
    pitch: {actionThreshold: 0.01, kp: 1.0, ki: 0.05, maxErrorSum: 1.0, kd: 15.0, target: 0.0},
    roll: {actionThreshold: 0.01, kp: 1.0, ki: 0.05, maxErrorSum: 1.0, kd: 15.0, target: 0.0},
    yaw: {actionThreshold: 0.01, kp: 1.0, ki: 0.05, maxErrorSum: 1.0, kd: 15.0, target: 0.0},
    x: {actionThreshold: 0.01, kp: 1.0, ki: 0.05, maxErrorSum: 1.0, rateRefreshFrequency: 5, kd: 40.0, target: 0.3},
    y: {actionThreshold: 0.01, kp: 1.0, ki: 0.05, maxErrorSum: 1.0, rateRefreshFrequency: 5, kd: 40.0, target: 0.0},
    z: {actionThreshold: 0.01, kp: 1.0, ki: 0.05, maxErrorSum: 1.0, rateRefreshFrequency: 5, kd: 40.0, target: 0.0},
};

let regularRegime = function(config) {
    config.x.ki = 0.05;
    config.x.maxErrorSum = 1.0;
    config.x.kd = 40.0;

    config.y.ki = 0.05;
    config.y.maxErrorSum = 1.0;
    config.y.kd = 40.0;

    config.z.ki = 0.05;
    config.z.maxErrorSum = 1.0;
    config.z.kd = 40.0;
};

let dockingRegime = function(config) {
    config.x.ki = 0.001;
    config.x.maxErrorSum = 0.01;
    config.x.kd = 5.0;

    config.y.ki = 0.001;
    config.y.maxErrorSum = 0.01;
    config.y.kd = 5.0;

    config.z.ki = 0.001;
    config.z.maxErrorSum = 0.01;
    config.z.kd = 5.0;
};

let startRegimeController = function(config, dockingRange=10.0, controlRate=1000) {
    const control = function() {
        const range = getDistanceValue(rangeSource);
        if (Math.abs(range) <= dockingRange) {
            dockingRegime(config);
        } else {
            regularRegime(config);
        }
    };
    const interval = setInterval(control, controlRate);
    return {stop: () => { clearInterval(interval); }};
};