/* Code to tune parameters on the fly...
       Much faster than tuning by hand and recompiling */

    /* The control segment is below */

    if ( c == 'u' )
    {
      kp *= 2.0;
      KI.off();
      cout << "kp is " << kp << endl;
      KI.on();
    }

    if ( c == 'd' )
    {
      kp /= 2.0;
      KI.off();
      cout << "kp is " << kp << endl;
      KI.on();
    }

    if ( c == '1' )
    {
      kp *= -1.0;
      KI.off();
      cout << "kp is " << kp << endl;
      KI.on();
    }


    if ( c == '+' )
    {
      ka *= 2.0;
      KI.off();
      cout << "ka is " << ka << endl;
      KI.on();
    }

    if ( c == '-' )
    {
      ka /= 2.0;
      KI.off();
      cout << "ka is " << ka << endl;
      KI.on();
    }

    if ( c == '0' )
    {
      ka *= -1.0;
      KI.off();
      cout << "ka is " << ka << endl;
      KI.on();
    }

    if ( c == 's' )
    {
      KI.off();
      cout << "Left:  " << State[L] << "  from #" << L-17 << endl;
      cout << "Right: " << State[R] << "  from #" << R-17 << endl;
      for (int i=0 ; i<16 ; ++i)
         cout << "Sonar " << i << " is " << State[17+i] << endl;
      KI.on();
    }

        // NOTE: you're safer using sonars 4 and 12 here than
        // the sonars with minimum value on either side...

        error = State[L] - State[R];
        aerror = State[R+1]-State[R-1];

        if (error > 0)
        {
          if (error > 10 && aerror < 10)
            rv = kp*error;
          else if (error > 10 && aerror >= 10)
            rv = 0;  // already getting there
          else // error <= 10
          {
            // turn so that aerror <= error
            // for a smooth "landing"
            if (aerror > error)
              rv = ka*aerror;
            else if (aerror > 0)
              rv = 0;  // already smooth enough
            else // the robot's either not turned or 
                 // turned the wrong way!
              rv = kp*error;
          }
        }
        // other cases omitted

        sprintf(str,
        "sending %d on error %d aerror %d  (L,R) = (%d,%d) (R+1,R-1) = (%d,%d) \n",
              (int)(rv), (int)(error), (int)(aerror), 
              State[L],State[R], State[R+1], State[R-1]);
        KI.off();
        cout << str << endl;
        KI.on();

        vm(tv,(int)rv,(int)rv);