Mecanum on Ice

/*

TODO: FIX UR GODDAMN FOD

*/

​

console.clear(); // my debugging savior

​

let rotated;

​

let driveVectors = [];

let wheelPosVectors = [];

let steeringCoefficients = [];

let steeringNormalVectors = [];

​

let wheelPositions = Array(4).fill(0);

const robotWidth = 30;

const robotHeight = 36;

​

let position;

let angle = 0;

let velocity;

let angularVel = 0;

​

let driveDirection;

let turnRate = 0;

​

const staticFrictionCoef = 0.2;

const slidingFrictionCoef = 0.1 * staticFrictionCoef;

​

const robotWeight = 1;

​

const wheelRadius = 2;

​

​

​

​

​

​

function setup() {

  createCanvas(windowWidth, windowHeight);

  background(100,100,255);

  rotated = createVector(0,0)

  driveVectors = [

    createVector(1,-1).normalize(),

    createVector(-1,-1).normalize(),

    createVector(-1,-1).normalize(),

    createVector(1,-1).normalize()

  ];

  const wheelX = robotWidth/2 + 3;

  const wheelY = robotHeight/2 - 3;

  wheelPosVectors = [

    createVector(-wheelX,-wheelY + 5),

    createVector(wheelX,-wheelY - 5),

    createVector(-wheelX,wheelY + 5),

    createVector(wheelX,wheelY - 5),

  ];

  for (let i in wheelPosVectors) {    

    steeringNormalVectors[i] = p5.Vector.normalize(wheelPosVectors[i]).rotate(HALF_PI);

  } 

  for (let i in steeringNormalVectors) {

    steeringCoefficients[i] = p5.Vector.dot(steeringNormalVectors[i], driveVectors[i]) / wheelPosVectors[i].mag();

  }

  position = createVector(width/2, height/2);

  driveDirection = createVector(0,0);

  velocity = createVector(0,0);

  //console.log(steeringCoefficients);

  console.warn = (e) =>  {}

}

​

​

function draw() {

​

  background(200,230,255, 100);

  // console.clear();

  driveDirection.x = 3 * (-keyDown("A") + keyDown("D"));

  driveDirection.y = 3 * (-keyDown("W") + keyDown("S"));

  driveDirection.rotate(-angle)

  turnRate = 0.09 * (-keyDown("Q") + keyDown("E"));

  let wheelRotationTorques = calculateRotationRates();

  let oldRotationTorques = calculateInertialTorques(velocity, angularVel);

  let {speed, rotation} = calculateRobotMovements(wheelRotationTorques, oldRotationTorques);

  // console.log(turnRate, angularVel);

  velocity = speed.copy();

  angularVel = rotation;

  position.add (velocity);

  angle += angularVel;

  push();

  translate(position.x, position.y);

  rotate(angle);

  // scale(3)

  rectMode(CENTER);

  drawRobot();

  // rotate(-angle)

  stroke(0,255,0);

  line(0,0, driveDirection.x * 10, driveDirection.y * 10);

  stroke(255,0,0);

  // const rotated = velocity.copy().rotate(-angle)

  line(0,0, rotated.x * 10, rotated.y * 10);

  pop();

  for(let i = 0; i < oldRotationTorques.length; i++) {

    rect(

      width-10-10*(i+1),

      height*0.5,

      10, 50*oldRotationTorques[i]

    );

  }

  for(let i = 0; i < wheelRotationTorques.length; i++) {

    rect(

      width-50-10*(i+1),

      height*0.5,

      10, 50*wheelRotationTorques[i]

    );

  }

  push()

  let compassPos = Math.max(robotWidth, robotHeight);

  translate(compassPos +20, compassPos +20);

  strokeWeight(0);

  fill(200,200, 255, 100)

  circle(0,0, compassPos + 50);

  circle(0,0, compassPos + 40);

  strokeWeight(1);

  rotate(angle);

  fill (60,195,60);

  strokeWeight(0)

  triangle(0,-46, -5, -40, 5,-40);

  drawRobot();

  pop()

  push ();

  let isOverX = position.x < 0 || position.x > width; 

  let isOverY = position.y < 0 || position.y > height;

  if(isOverX || isOverY) {

    translate(clamp(position.x, 10, width-10), clamp(position.y, 10, height-10));

    scale(Math.sign(position.x), Math.sign(position.y));

    if(isOverX && isOverY) {

      triangle(3.5,3.5, -3,1,1,-3);

    }

    else if(isOverX) {

      triangle(6,0, 0,4,0,-4);

    }

    else if(isOverY) {

      triangle(0,6, 4,0,-4,0);

    } 

  }

  pop();

}

​

function clamp(x,a,b) {return Math.max(Math.min(x,b), a)}

​

​

function drawRobot() {

  strokeWeight(1)

  rectMode(CENTER);

  for(let i in wheelPosVectors) {

    fill(255,255,0);

    rect (wheelPosVectors[i].x - 8*Math.sign(wheelPosVectors[i].x), wheelPosVectors[i].y, 10, 5);

    fill (30)

    rect (wheelPosVectors[i].x - 10*Math.sign(wheelPosVectors[i].x), wheelPosVectors[i].y, 8, 6);

    // fill(30);

    push();

    translate(wheelPosVectors[i].x, wheelPosVectors[i].y);

    rect (0,0, 5, 10);

    scale(-Math.sign(wheelPosVectors[i].x), Math.sign(wheelPosVectors[i].y))

    stroke(255,255,0);

    strokeWeight(1.5);

    const numLines = 4;

    for(let ln =0; ln < numLines; ln++) {

      let pos = map(ln, 0, numLines-1, 0,10);

      pos += wheelPositions[i] * -Math.sign(wheelPosVectors[i].y);

      pos = ((pos % 10.1) + 10.1) % 10.1;

      pos -= 5;

      // console.log(pos)

      let angle = map (pos, -5,5, 0,PI);

      line(-1.9,1.9*sin(angle)+pos,1.9,-1.9*sin(angle)+pos);

    }

    pop();

  }

  fill(155);

  rect(0, 0, robotWidth, 7)

  rect(-robotWidth*0.5 + 4, 0, 5, robotHeight)

  rect(robotWidth*0.5 - 4, 0, 5, robotHeight);

  // triangle(0,-10, -7, 5, 7, 5);

}

​

function calculateRotationRates () {

  let wheelRotationTorques = new Array(4).fill(0);

  // 

  for(let i in wheelRotationTorques) {

    let rotatedDriveVector = p5.Vector.rotate(driveVectors[i], angle);

    wheelRotationTorques[i] = (p5.Vector.dot(driveDirection, driveVectors[i]) + turnRate / steeringCoefficients[i]) / wheelRadius;    

  // // // console.log(wheelRotationTorques)

  }

  let maxTorque = Math.abs(wheelRotationTorques[0]);

  for(let i = 1; i < wheelRotationTorques.length; i++) {

    if(Math.abs(wheelRotationTorques[i]) > maxTorque) {maxTorque = Math.abs(wheelRotationTorques[i]);}

  }

  if(maxTorque < 3) {return wheelRotationTorques;}    

  for(let i in wheelRotationTorques) {

    wheelRotationTorques[i] /= maxTorque;

    wheelRotationTorques[i] *= 3;

  }

  return wheelRotationTorques;

​

}

function calculateInertialTorques (velocity, angularVel) {

  let oldRotationTorques = [];

  let rotatedVel = velocity.copy();

  rotatedVel.rotate(-angle)

  rotated= rotatedVel;

  for (let i in driveVectors) { 

    let rotatedDriveVector = p5.Vector.rotate(driveVectors[i], -angle);

    // console.log(p5.Vector.dot(velocity, rotatedDriveVector));

    oldRotationTorques[i] = (p5.Vector.dot(rotatedVel, driveVectors[i]) + angularVel / steeringCoefficients[i]) / wheelRadius;

  }

  return oldRotationTorques

}

​

function calculateRobotMovements (wheelRotationTorques, oldRotationTorques) {

  // // console.log(oldRotationTorques?.join(", "));

  let speed = createVector(0,0);

  let rotation = 0;

  let driveForceVectors = [];

​

  for (let i in driveVectors) {

    wheelPositions[i] += wheelRotationTorques[i];

    let torqueDifference = (wheelRotationTorques[i] - oldRotationTorques[i]);

    let frictionTorque = calculateFriction(torqueDifference);

    // let torqueApplied = wheelRotationTorques[i]; 

    let torqueApplied = oldRotationTorques[i]+frictionTorque;

    driveForceVectors[i] = driveVectors[i].copy()//.rotate(-angle);

    driveForceVectors[i].mult(torqueApplied * wheelRadius);

    // driveForceVectors[i].mult();

    speed.add(driveForceVectors[i]);

    // console.log("needed torque: " + wheelRotationddTorques[i]);

    // console.log("old torque: " +  oldRotationTorque);

    // console.log("friction: " + frictionTorque);

    // let rotatedSteeringNormal = steeringNormalVectors[i].copy().rotate(angle);

    rotation += torqueApplied * wheelRadius * steeringCoefficients[i];

  }

    // 

  speed.mult(0.5).rotate(angle);

  rotation *= 0.25 ;

  // rotation /= robotWeight;

  // console.log(rotation);

  // 

  return {speed, rotation};

}

​

function calculateFriction(torqueDifference) {

  // return 1;

  // console.log(torqueDifference, robotWeight * staticFrictionCoef)

  let effectiveWeight = robotWeight + getNormalDistRandom(0.3);

  if(abs(torqueDifference) < effectiveWeight * staticFrictionCoef) {

    return torqueDifference;

  }

  let direction = Math.sign(torqueDifference);

  if(direction == 0) {direction = 1}

  return (direction)*effectiveWeight*slidingFrictionCoef;

}

​

function getNormalDistRandom(stdev) {

  const u1 = Math.random();

  const u2 = Math.random();

  return stdev * Math.sqrt(-2*Math.log(u1)) * Math.cos(2*Math.PI*u2);

}

​

​

let keysDown = [];

​

document.addEventListener("keydown", e => {

  if(!keysDown.includes(e.keyCode)) {

    keysDown.push(e.keyCode);

  }

})

​

document.addEventListener("keyup", e => {

  keysDown.splice(keysDown.indexOf(e.keyCode), 1);

})

​

function keyDown (key) {

  return keysDown.includes(key.charCodeAt(0));

}