PD controller for andrey-leshenko/ISSDockingBotGame

gistfile1.txt

// Thrust types
NEGATIVE_LARGE = -2;
NEGATIVE_SMALL = -1;
NONE = 0;
POSITIVE_SMALL = 1;
POSITIVE_LARGE = 2;

//
// This function is called on each simulation step.
//
// Input: The current position, rotation and time
// Output: The control commands
//
var lastX;
var lastY;
var lastZ;

var rotKp = -1;
var rotKd = 5;
var posKp = -1;
var posKd = -100
var posKdx = -1000;
var cutoff = .2;
var closure = 0.17;

function calculateControl(
	posX,		// - forward		+ backward
	posY,		// - left			+ right
	posZ,		// - down			+ up
	yaw,		// - right			+ left
	pitch,		// - down			+ up
	roll,		// - CW				+ CCW
	yawRate,	// speed with which the yaw is changing
	pitchRate,	// speed with which the pitch is changing
	rollRate,	// speed with which the roll is changing
	time)		// in simulation steps, starts from 0
{
    var dX = posX - lastX;
    var dY = posY - lastY;
    var dZ = posZ - lastZ;
    
    var rotYawDesired = pid(rotKp, yaw, rotKd, yawRate);
    var rotPitchDesired = pid(rotKp, pitch, rotKd, pitchRate);
    var rotRollDesired = pid(rotKp, roll, rotKd, rollRate);
    var moveXDesired = pid(posKp, posX, posKdx, dX + closure / 60);
    var moveYDesired = pid(posKp, posY, posKd, dY);
    var moveZDesired = pid(posKp, posZ, posKd, dZ);

    var moveX		= convertToPulse(moveXDesired);
	var moveY		= convertToPulse(moveYDesired);
	var moveZ		= convertToPulse(moveZDesired);
	var rotYaw		= convertToPulse(rotYawDesired);
	var rotPitch	= convertToPulse(rotPitchDesired);
	var rotRoll		= convertToPulse(rotRollDesired);

    lastX = posX;
    lastY = posY;
    lastZ = posZ;
    
	// Return the control commands you want executed
	return [moveX, moveY, moveZ, rotYaw, rotPitch, rotRoll];
}

function convertToPulse(value)
{
  if (Math.abs(value) < cutoff) {
    return NONE;
  }
  if (value > 0) {
    return POSITIVE_SMALL;
  }
  else {
    return NEGATIVE_SMALL;
  }
}

function pid(kp, e, kd, de)
{
  return kp * e + kd * de;
}