swervetest.c

#pragma config(Hubs,  S1, HTServo,  none,     none,     none)
#pragma config(Sensor, S2,     proto,          sensorI2CCustom9V)
#pragma config(Sensor, S3,     gyro,           sensorI2CHiTechnicGyro)
#pragma config(Servo,  srvo_S1_C1_1,    pod0Steer,            tServoContinuousRotation)
#pragma config(Servo,  srvo_S1_C1_2,    pod1Steer,            tServoStandard)
#pragma config(Servo,  srvo_S1_C1_3,    pod2Steer,            tServoStandard)
#pragma config(Servo,  srvo_S1_C1_4,    pod3Steer,            tServoStandard)
#pragma config(Servo,  srvo_S1_C1_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C1_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#define GYRO gyro

//#include "JoystickDriver.c"
#include "LordSwerve.c"

/*
task changer()
{
  while ( true )
  {
    for ( int i = 0; i < 4; i++ )
    {
      setModuleTarget(i, 512);
    }
    wait1Msec(2000);
    for ( int i = 0; i < 4; i++ )
    {
      setModuleTarget(i, 1512);
    }
    wait1Msec(2000);
  }
}
*/
//int speed = 127;
task main()
{
  //waitForStart();
  //bDisplayDiagnostics = false;
  //initGyro();
  initSwerveDrive();
  //StartTask(changer);
 // waitForStart();
  while ( true )
  {
         //servo[pod0Steer] = speed;
        servo[pod1Steer] = 127;
        servo[pod2Steer] = 127;
        servo[pod3Steer] = 127;
   // getJoystickSettings(joystick);
    //for ( int i = 0; i < 4; i++ )
    //{
    //  nxtDisplayString(i+1, "%i - %i - %i", i, modules[i].truePos, modules[i].Rollovers);
    //}
  /*  if ( abs(modules[3].turnPID.error) > 500 )
    {
      eraseDisplay();
      while( true )
      {
        servo[pod0Steer] = 127;
        servo[pod1Steer] = 127;
        servo[pod2Steer] = 127;
        servo[pod3Steer] = 127;

        eraseDisplay();
        nxtDisplayString(1, "Prev: %i", modules[3].lastPos);
        nxtDisplayString(2, "Cur: %i", modules[3].truePos);
      }
    }*/

   // if ( abs(joystick.joy1_x2) > 10 )
   //   chassisRotation();
   // else

  // fieldCentricCrab();

   debugPID(modules[0].turnPID);
   updateSwerveDrive();
  }

}