TurretDrive.nxc

/*
 * @param in the value to constrain
 * @param limit the magnitude to constrain to
 */
int limitTo(int in, int limit){
  if(in<-limit) return -limit;
  else if(in>limit) return limit;
  else return in;
}

/*
 * @param port the motor port to run
 * @param power the speed to run it at, scaled from -100 to +100
 * @param brake whether or not to brake when power = 0
 */
inline void runMotor(byte port, int power, bool brake=false){
  if(power>100)
    power=100;
  if(power<-100)
    power=-100;
  if (power>0){
    OnFwdEx(port, power, RESET_NONE);
  }else if(power<0){
    OnRevEx(port, -power, RESET_NONE);
  }else if(brake){
    OffEx(port, RESET_NONE);
  }else{
    CoastEx(OUT_BC,RESET_NONE);
  }
}

// Direction to turn the turret to in degrees
float turretSetpoint=0;

/*
 * Turn on the drivetrain to output values corresponding to the given vectors.
 * @param y the forwards-backwards speed vector
 * @param w the rotational speed vector
 */
void drive (int y, int w) {
  if (abs(w)<2) {
    // If the rotation vector is negligible, just skip expensive computations.
    turretSetpoint = 0;
    
    runMotor(OUT_B, -y, false);
    runMotor(OUT_C, -y, false);
  } else {
    // Get basic gist of the vectors
    int left = y-w;
    int right = y+w;
    // Scale them down appropriately so that neither one is larger than 100.
    /// If we don't do this, the output tank values will not correspond to the turret.
    if (abs(left)>abs(right)) {
      if (abs(left)>100) {
        right = right*abs(left)/100;
        left = abs(left)/left*100;
      }
    } else {
      if (abs(right)>100) {
        left = left*abs(right)/100;
        right = abs(right)/right*100;
      }
    }
    // Write values out
    runMotor(OUT_B, -left, false);
    runMotor(OUT_C, -right, false);
    // Compute turning radius, measured in wheelbase width/2
    float radius = (left+right)/(right-left); 
    // compute angle to turn to.
    // The turret is the wheelbase width back from the drive axle, so use 2.0
    turretSetpoint = atan2d(2.0,radius);
  }
}

// Holds the turret at the desired setpoint
task turretRegulator() {
  ResetTachoCount(OUT_A);
  while(true){
    // Get general error
    float error = -turretSetpoint-((MotorRotationCount(OUT_A))*40.0/56.0);
    // Optimize error by subtracting or adding 180 to it until it is within the range of -90,+90.
    while(error>90) error-=180;
    while(error<-90) error+=180;
    // Set output value
    runMotor(OUT_A,error*7);

    Wait(3);
    KeepAliveType kaArgs;
  SysKeepAlive(kaArgs);
  }
}

task main() {
  start turretRegulator;

  long confidence=0;
  long closeError=0;
  long overallError=0;
  long overallDerivative=0;
  long overallSecondDerivative=0;
  while(true) {
    // Step 1: Get mailbox data
    string res;
    if(ReceiveMessage(0,true,res)==NO_ERR) {
      if (res[0]!='E') confidence = StrToNum(res); }
    if(ReceiveMessage(1,true,res)==NO_ERR) {
      if (res[0]!='E') closeError = StrToNum(res); }
    if(ReceiveMessage(2,true,res)==NO_ERR) {
      if (res[0]!='E') overallError = StrToNum(res); }
    if(ReceiveMessage(3,true,res)==NO_ERR) {
      if (res[0]!='E') overallDerivative = StrToNum(res); }
    if(ReceiveMessage(4,true,res)==NO_ERR) {
      if (res[0]!='E') overallSecondDerivative = StrToNum(res); }

    // Step 2: Calculate response... if we don't have much confidence in our reading, we should probably slow down!
    int power = confidence/12;

    if (abs(overallDerivative)>100000 && power>50)
      power = 50;
    if (power<32) power = 32;

    int steering = sign(overallError)*pow(abs(overallError),1.2)/pow(76,1.2)
            +sign(closeError)*pow(abs(closeError),1.2)/pow(220,1.2)
            +sign(overallDerivative)*pow(abs(overallDerivative),1.4)/pow(8200,1.4)
            +sign(overallSecondDerivative)*pow(abs(overallSecondDerivative),2)/pow(40000,2);
            ;

    // Step 3: Commit that...
    drive(power, limitTo(steering, power>60 ? power*1.5 : power*2.5));
  }
}