void main ( void )
{
      unsigned char Bu5u; 

      while ( 1 ) // Main while loop
      {
            Bu5u = GetJoystickDigital(1,5,1) + GetJoystickDigital(1,5,2) + GetJoystickDigital(1,6,1)+GetJoystickDigital(1,6,2) ;
            if ( Bu5u > 0 ) // any shoulder buttons mean recenter joystick
            {
                  StickCenter[1] = GetJoystickAnalog( 1 , 1 ) ; // Get current position as Center
                  StickCenter[2] = GetJoystickAnalog( 1 , 2 ) ; // Get current position as Center
                  StickCenter[3] = GetJoystickAnalog( 1 , 3 ) ; // Get current position as Center
                  StickCenter[4] = GetJoystickAnalog( 1 , 4 ) ; // Get current position as Center
            }
            Stick1 = Scale_RC ( 1, 5, 9, 1.3 ) ; // direction:   Input Args: Sticknum, dead, minP, Powerscale
            Stick2 = Scale_RC ( 2, 2, 15, 1.5 ) ; // // speed
            ArmSpeed = Scale_RC ( 3, 5, 30, 1.7 ) ;
            //GripperSpeed = Scale_RC ( 4, 30, 20, 1.3 ) ;
            //JoystickDigitalToMotor ( 1 , 7 , 1 , 127 , 2 , -127 , 3 ) ;
            //JoystickDigitalToMotor ( 1 , 7 , 1 , 127 , 2 , -127 , 4 ) ;
            JoystickDigitalToMotor ( 1 , 7 , 3 , 127 , 4 , -127 , 7 ) ;
            JoystickDigitalToMotor ( 1 , 7 , 3 , 127 , 4 , -127 , 8 ) ;
            myArcade ( Stick2, Stick1 ) ; // IN(Speed, Dir) OUT(LWspeed, RWspeed)
            // Could move last limit check and setmotor to routine at end 
            SetMotor ( 1 , LWspeed ) ; // copy variables to motors just once at bottom of flow
            SetMotor ( 2 , LWspeed ) ;
            SetMotor ( 3 , ArmSpeed ) ;
            SetMotor ( 4 , ArmSpeed ) ;
            //SetMotor ( 7 , GripperSpeed ) ;
            //SetMotor ( 8 , GripperSpeed ) ;
            SetMotor ( 9 , RWspeed ) ;
            SetMotor ( 10 , RWspeed ) ;
      }
}

void myArcade ( int Speed, int Dir )
{ // Given two vars Speed,Dir, set global vars for LWspeed, RWspeed
      LWspeed = Speed + Dir ;
      RWspeed = Speed - Dir ;
      // limit checks
      if ( LWspeed < -127 )  {  LWspeed = -127 ; }
      if ( LWspeed > 127 )   {  LWspeed = 127 ;  }
      if ( RWspeed < -127 )  {  RWspeed = -127 ; }
      if ( RWspeed > 127 )   {  RWspeed = 127 ;  }
}
int Scale_RC ( unsigned char StickNum, int Deadband, int MinPower, double Exponent )
{ // input^exponent if greater than deadband( 1, 15, 20, 3)
      int StickIn; 
      float SIa; 
      int MaxPower; 

      MaxPower = 127 - cWheelMinPower  ;
      StickIn = GetJoystickAnalog( 1 , StickNum ) ;
      StickIn -= StickCenter[StickNum]  ;  // recenter!!!
      if ( Abs(StickIn) <= Deadband )  { return 0 ;       }
      SIa = Pow ( Fabs(StickIn)/MaxPower , Exponent ) ;
      SIa = SIa * MaxPower + MinPower ;
      if ( SIa > 127 ) // limit check
      {            SIa = 127 ; // only positive values at this point
      }
      if ( StickIn > 0 )
      {            return  SIa ;
      }      else      {
            return  -SIa ;
      }
}