First Vex Cortex Robot

main.c

#pragma config(Motor,  port6,           rightFront,      tmotorNormal, openLoop)
#pragma config(Motor,  port7,           rightBack,     tmotorNormal, openLoop)
#pragma config(Motor,  port8,           leftFront,     tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port9,           leftBack,    tmotorNormal, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/*
 * @author: Andrew Horsman
 * @description: First Vex robot with the Cortex microcontroller.
 *  ~ 2x16 LCD
 *  ~ Four 393 motor's
 *  ~ VEXnet Joystick/Accel control
 *  ~ Accelerometer
 *  ~ Two side bumpers
 *  ~ Light sensor
 *  ~ Front and back ultrasonic sensor
 */
 
// Joystick accelerometer. 
#define JOY_ACCEL_NOISE_THRESH 10

// Joystick control values.
#define JOYSTICK 0
#define JOYSTICK_ACCEL 1

// Drive train side values.
#define LEFT 0
#define RIGHT 1

// Drive train speed tolerance.
#define TOLERANCE 10

// Ramp input (joystick accelerometer)
#define RAMP_THRESHOLD 100
#define RAMP_MIN 20

// Globals
int joyAccelX, joyAccelY;

// Functions
int deadband(int value) {
  if (value >= -10 && value <= 10)
    return 0;
  else
    return value;
}

bool isPastAccelThreshold(int a, int b) {
  return (abs(a - b) > JOY_ACCEL_NOISE_THRESH);
}

bool isControlMethod(int assertion) {
  hogCPU();
  if (assertion == JOYSTICK) {
    return (vexRT[Btn8D] != 1);
  } else if (assertion == JOYSTICK_ACCEL) {
    return (vexRT[Btn8D] == 1);
  } else {
    return true;
  }
  releaseCPU();
}

bool close(int a, int b) {
  int minA = a - TOLERANCE;
  int maxA = a + TOLERANCE;
  int minB = b - TOLERANCE;
  int maxB = b + TOLERANCE;

  return ((a > minB && a < maxB) ||
          (b > minA && b < maxA));
}

bool isTiltForward()   { return (joyAccelY < 0); }
bool isTiltBackwards() { return (joyAccelY > 0); }
bool isTurnedLeft()    { return (joyAccelX < 0); }
bool isTurnedRight()   { return (joyAccelX > 0); }

int ramp(int value) {
  int multiplier = (value < 0) ? -1 : 1;
  int temp = abs(value);
  if (temp <= RAMP_THRESHOLD)
    temp = temp / 2 + RAMP_MIN;
  else
    temp = 127;
    
  return temp * multiplier;
}

// Tasks
task updateJoystickAcceleration() {
  int tempX = 0;
  int tempY = 0;
  int prevX, prevY;
  while (true) {
    prevX = tempX;
    prevY = tempY;
    hogCPU();
    tempX = ramp(vexRT[AccelX]);
    tempY = ramp(vexRT[AccelY]);

    if (!isPastAccelThreshold(prevX, tempX))
      joyAccelX = tempX;

    if (!isPastAccelThreshold(prevY, tempY))
      joyAccelY = tempY;

    releaseCPU();
    wait1Msec(15);
  }
}

task main() {
  StartTask(updateJoystickAcceleration);
  int leftY, rightY;

  while (true) {
    hogCPU();
    if (isControlMethod(JOYSTICK)) {
      leftY  = vexRT[Ch3];
      rightY = vexRT[Ch2];
    } else if (isControlMethod(JOYSTICK_ACCEL)) {
      // Using the accelerometer in the joystick is essentially arcade drive.
      leftY  = (-1 * joyAccelY) + joyAccelX;
      rightY = (-1 * joyAccelY) - joyAccelX;
    }
    leftY  = deadband(leftY);
    rightY = deadband(rightY);
    if (close(leftY, rightY)) {
      int temp = (leftY + rightY) / 2;
      leftY  = temp;
      rightY = temp;
    }
    releaseCPU();

    motor[rightFront] = rightY;
    motor[rightBack]  = rightY;
    motor[leftFront]  = leftY;
    motor[leftBack]   = leftY;
  }
}