Source: sketch.js

/*******************************************************************************
 *
 *	@file sketch.js A quick mock up of a quadruped design to test Inverse
 *  Kinematics
 *
 *	@author Omar Essilfie-Quaye <omareq08@gmail.com>
 *	@version 1.0
 *	@date 21-August-2020
 *
 ******************************************************************************/

/**
 * Instance of Robot class
 *
 * @type       {Robot}
 */
let natalya;

/**
 * Angle of rotation of the robot on imaginary platform.
 *
 * @type       {number}
 */
let viewAngle = 0;

let i = 0;
let firstState = 0;
let lastState = 2;
let state = firstState;
let gaitCounter = 0;
let x = 1;
let y = 1;
let yInc = 0.1;
let roll = 0;
let pitch = 1.25;
let rollInc = 0.25;
let pitchInc = 0.25;

/**
 * Array to control the walking gait as well as the static pose of the robot.
 * The order of the control parameters in the array are as follows:
 *
 *   [0] Walking X          [1] Walking Y            [2] Walking Rotation
 *   [3] Static Offset X    [4] Static Offset Y      [5] Static Offset Z
 *   [6] Static Offset Yaw  [7] Static Offset Pitch  [8] Static Offset Roll
 *
 *   @type      {Array<number>}
 **/
walkArray = [0.5, 0, 0,
               0, 0, 0,
               0, 0, 0];

let lfScatterChart;
let rfScatterChart;
let lbScatterChart;
let rbScatterChart;

/**
 * Function to delay execution of the current thread by the time provided.
 *
 * @param      {number}  milliseconds  The delay period in milliseconds.
 *
 * @example <caption> Example usage of the delay method. </caption>
 * console.log("Start Program");
 * delay(2000); // wait for 2 seconds
 * console.log("End Program");
 */
function delay(milliseconds) {
  const date = Date.now();
  let currentDate = null;
  do {
    currentDate = Date.now();
  } while (currentDate - date < milliseconds);
  // console.log("Delay " + milliseconds + "(ms)");
}

function preload() {
  natalya = new Robot(1000);
}

/**
 * Function to setup chart js canvases to plot the angles of all the leg servos.
 */
function chartSetup() {
    let lfCtx = document.getElementById('lfChart');
    lfScatterChart = new Chart(lfCtx, {
        type: 'line',
        data: {
            datasets: [
                {
                    label: 'LF Hip Yaw',
                    data: [{
                        x: 0,
                        y: 90
                    }],
                    pointRadius: 0,
                    backgroundColor: 'rgba(255, 0, 0, 0.2)',
                    borderColor: 'rgba(255, 0, 0, 0.65)',
                    // fill: false
                },
                {
                    label: 'LF Hip Elevation',
                    data: [{
                        x: 0,
                        y: 90
                    }],
                    pointRadius: 0,
                    backgroundColor: 'rgba(0, 255, 0, 0.2)',
                    borderColor: 'rgba(0, 255, 0, 0.65)',
                    // fill: false
                },
                {
                    label: 'LF Knee Extension',
                    data: [{
                        x: 0,
                        y: 90
                    }],
                    pointRadius: 0,
                    backgroundColor: 'rgba(0, 0, 255, 0.2)',
                    borderColor: 'rgba(0, 0, 255, 0.65)',
                    // fill: false
                }
            ]
        },
        options: {
            animation: {
                duration: 0 // general animation time
            },
            elements: {
                line: {
                    tension: 0 // disables bezier curves float value 0 to 1
                }
            },
            scales: {
                xAxes: [{
                    type: 'linear',
                    position: 'bottom',
                    scaleLabel: {
                        display: true,
                        labelString: 'Frame'
                      }
                }],
                yAxes: [{
                    min: 0,
                    max: 180,
                    scaleLabel: {
                        display: true,
                        labelString: 'Angle Degrees'
                      }}],
            },
            // showLines: false // disable for all datasets
        }
    });

    let rfCtx = document.getElementById('rfChart');
    rfScatterChart = new Chart(rfCtx, {
        type: 'line',
        data: {
            datasets: [
                {
                    label: 'RF Hip Yaw',
                    data: [{
                        x: 0,
                        y: 90
                    }],
                    pointRadius: 0,
                    backgroundColor: 'rgba(255, 0, 0, 0.2)',
                    borderColor: 'rgba(255, 0, 0, 0.65)',
                    // fill: false
                },
                {
                    label: 'RF Hip Elevation',
                    data: [{
                        x: 0,
                        y: 90
                    }],
                    pointRadius: 0,
                    backgroundColor: 'rgba(0, 255, 0, 0.2)',
                    borderColor: 'rgba(0, 255, 0, 0.65)',
                    // fill: false
                },
                {
                    label: 'RF Knee Extension',
                    data: [{
                        x: 0,
                        y: 90
                    }],
                    pointRadius: 0,
                    backgroundColor: 'rgba(0, 0, 255, 0.2)',
                    borderColor: 'rgba(0, 0, 255, 0.65)',
                    // fill: false
                }
            ]
        },
        options: {
            animation: {
                duration: 0 // general animation time
            },
            elements: {
                line: {
                    tension: 0 // disables bezier curves float value 0 to 1
                }
            },
            scales: {
                xAxes: [{
                    type: 'linear',
                    position: 'bottom',
                    scaleLabel: {
                        display: true,
                        labelString: 'Frame'
                      }
                }],
                yAxes: [{
                    min: 0,
                    max: 180,
                    scaleLabel: {
                        display: true,
                        labelString: 'Angle Degrees'
                      }}],
            },
            // showLines: false // disable for all datasets
        }
    });

    let lbCtx = document.getElementById('lbChart');
    lbScatterChart = new Chart(lbCtx, {
        type: 'line',
        data: {
            datasets: [
                {
                    label: 'LB Hip Yaw',
                    data: [{
                        x: 0,
                        y: 90
                    }],
                    pointRadius: 0,
                    backgroundColor: 'rgba(255, 0, 0, 0.2)',
                    borderColor: 'rgba(255, 0, 0, 0.65)',
                    // fill: false
                },
                {
                    label: 'LB Hip Elevation',
                    data: [{
                        x: 0,
                        y: 90
                    }],
                    pointRadius: 0,
                    backgroundColor: 'rgba(0, 255, 0, 0.2)',
                    borderColor: 'rgba(0, 255, 0, 0.65)',
                    // fill: false
                },
                {
                    label: 'LB Knee Extension',
                    data: [{
                        x: 0,
                        y: 90
                    }],
                    pointRadius: 0,
                    backgroundColor: 'rgba(0, 0, 255, 0.2)',
                    borderColor: 'rgba(0, 0, 255, 0.65)',
                    // fill: false
                }
            ]
        },
        options: {
            animation: {
                duration: 0 // general animation time
            },
            elements: {
                line: {
                    tension: 0 // disables bezier curves float value 0 to 1
                }
            },
            scales: {
                xAxes: [{
                    type: 'linear',
                    position: 'bottom',
                    scaleLabel: {
                        display: true,
                        labelString: 'Frame'
                      }
                }],
                yAxes: [{
                    min: 0,
                    max: 180,
                    scaleLabel: {
                        display: true,
                        labelString: 'Angle Degrees'
                      }}],
            },
            // showLines: false // disable for all datasets
        }
    });

    let rbCtx = document.getElementById('rbChart');
    rbScatterChart = new Chart(rbCtx, {
        type: 'line',
        data: {
            datasets: [
                {
                    label: 'RB Hip Yaw',
                    data: [{
                        x: 0,
                        y: 90
                    }],
                    pointRadius: 0,
                    backgroundColor: 'rgba(255, 0, 0, 0.2)',
                    borderColor: 'rgba(255, 0, 0, 0.65)',
                    // fill: false
                },
                {
                    label: 'RB Hip Elevation',
                    data: [{
                        x: 0,
                        y: 90
                    }],
                    pointRadius: 0,
                    backgroundColor: 'rgba(0, 255, 0, 0.2)',
                    borderColor: 'rgba(0, 255, 0, 0.65)',
                    // fill: false
                },
                {
                    label: 'RB Knee Extension',
                    data: [{
                        x: 0,
                        y: 90
                    }],
                    pointRadius: 0,
                    backgroundColor: 'rgba(0, 0, 255, 0.2)',
                    borderColor: 'rgba(0, 0, 255, 0.65)',
                    // fill: false
                }
            ]
        },
        options: {
            animation: {
                duration: 0 // general animation time
            },
            elements: {
                line: {
                    tension: 0 // disables bezier curves float value 0 to 1
                }
            },
            scales: {
                xAxes: [{
                    type: 'linear',
                    position: 'bottom',
                    scaleLabel: {
                        display: true,
                        labelString: 'Frame'
                      }
                }],
                yAxes: [{
                    min: 0,
                    max: 180,
                    scaleLabel: {
                        display: true,
                        labelString: 'Angle Degrees'
                      }}],
            },
            // showLines: false // disable for all datasets
        }
    });
}

/**
 * Function to plot the angles of all the leg servos on the chart js canvases.
 */
function chartUpdate() {
    if(frameCount % 3 == 0) {
        for(let i = 0; i < 3; i++) {
            lfScatterChart.data.datasets[i].data.push({x: frameCount, y: natalya.lf_servos_read()[i]*180.0/3.1415927});
            rfScatterChart.data.datasets[i].data.push({x: frameCount, y: natalya.rf_servos_read()[i]*180.0/3.1415927});
            lbScatterChart.data.datasets[i].data.push({x: frameCount, y: natalya.lb_servos_read()[i]*180.0/3.1415927});
            rbScatterChart.data.datasets[i].data.push({x: frameCount, y: natalya.rb_servos_read()[i]*180.0/3.1415927});
            if(rbScatterChart.data.datasets[i].data["length"] >= 155) {
                lfScatterChart.data.datasets[i].data.shift();
                rfScatterChart.data.datasets[i].data.shift();
                lbScatterChart.data.datasets[i].data.shift();
                rbScatterChart.data.datasets[i].data.shift();
            }

        }
        lfScatterChart.update(0);
        rfScatterChart.update(0);
        lbScatterChart.update(0);
        rbScatterChart.update(0);
    }
}

/**
 * p5.js setup function, creates canvas.
 */
function setup() {
	let cnvSize;
	if(windowWidth > windowHeight) {
		cnvSize = windowHeight;
	} else {
		cnvSize = 0.6 * windowWidth;
	}
	let cnv = createCanvas(cnvSize, 0.7 * cnvSize, WEBGL);
	cnv.parent('sketch');
    chartSetup();
    console.log("Set up Complete");
}

/**
 * p5.js draw function, is run every frame to create the desired animation
 */
function draw() {
    background(250);
    viewAngle += radians(0.5);
    fill(0);

    if(state == 0) {
        natalya.home(i);
    } else if(state == 1) {
        natalya.stand90(i);
    } else if(state == 2) {
        i+=0.09;
        natalya.walk(walkArray, i);
        if(i >= 1) {
            gaitCounter++;
            state--;
            if(gaitCounter > 10 * 12) {
                state++;
                gaitCounter = 0;
            }
        }
    } else if(state == 3) {
        roll += rollInc;
        if(roll < -7 || roll > 7) {
            rollInc *= -1;
        }

        pitch += pitchInc;
        if(pitch < -7 || pitch > 7) {
            pitchInc *= -1;
        }

        y+= yInc;
        x = 5;
        if(y > 20 || y <=-20){
            yInc *=-1;
        }
        i-=0.01;

        let deltaRoll = 100 * 0.5 * natalya.under_base_width * sin(radians(roll));
        let deltaPitch = 100 * 0.5 * natalya.under_base_width * sin(radians(pitch));
        let z = 6;
        let lfz = z + deltaRoll + deltaPitch;
        let rfz = z - deltaRoll + deltaPitch;
        let lbz = z + deltaRoll - deltaPitch;
        let rbz = z - deltaRoll - deltaPitch;

        let rf_angles = natalya.rf_IK([x, x, rfz]);
        let lf_angles = natalya.lf_IK([-x, x, lfz]);
        let rb_angles = natalya.rb_IK([x, -x, rbz]);
        let lb_angles = natalya.lb_IK([-x, x, lbz]);
        natalya.rf_servos_write(rf_angles);
        natalya.lf_servos_write(lf_angles);
        natalya.rb_servos_write(rb_angles);
        natalya.lb_servos_write(lb_angles);
    }

    i+=0.01;
    if(i >= 1) {
        i = 0;
        state++;
        if(state > lastState) {
            state = firstState;
            delay(250);
        }
    }

    chartUpdate();
    // rotateY(radians(-y));
    // natalya.draw(0,0,0, radians(-0.5*pitch), PI/4, radians(0.5*roll));
    natalya.draw(0,0,0, -PI/6, 0.1*viewAngle,0);//radians(roll));
    // natalya.draw(0,0,0, radians(pitch), viewAngle, radians(roll));
    // natalya.draw(0,0,0, -PI/6, viewAngle, radians(roll));
}


Documentation generated by JSDoc 3.6.3 on Sun Jun 05 2022 19:59:21 GMT+0100 (BST)