[eugenebrooks]

#include "WPILib.h"

#define BUFFERSIZE 200
static INT16 gforce_array[BUFFERSIZE];
static int gforce_index=0;
static INT32 lowVal=2000;
static INT32 highVal=-2000;

class BuiltinDefaultCode : public IterativeRobot
{
// Declare a variable to use to access the driver station object
DriverStation *m_ds; // driver station object
UINT32 m_priorPacketNumber; // keep track of the most recent packet number from the DS
UINT8 m_dsPacketsReceivedInCurrentSecond; // keep track of the ds packets received in the current second

// Declare variables for the two joysticks being used
Joystick *LeftJoyStick;
Joystick *RightJoyStick;

// Declare variables for the victors
Victor *LeftDrive;
Victor *RightDrive;
Victor *Winch;
Victor *Roller;

// Declare variables for the servos
Servo *Indicator;

// Declare variables for the analog inputs
AnalogChannel *Accelerometer;

// Local variables to count the number of periodic loops performed
UINT32 m_autoPeriodicLoops;
UINT32 m_disabledPeriodicLoops;
UINT32 m_telePeriodicLoops;

public:
/**
* Constructor for this "BuiltinDefaultCode" Class.
*
* The constructor creates all of the objects used for the different inputs and outputs of
* the robot. Essentially, the constructor defines the input/output mapping for the robot,
* providing named objects for each of the robot interfaces.
*/
BuiltinDefaultCode(void) {
printf("BuiltinDefaultCode Constructor Started\n");

// Acquire the Driver Station object
m_ds = DriverStation::GetInstance();
m_priorPacketNumber = 0;
m_dsPacketsReceivedInCurrentSecond = 0;

// Define joysticks being used at USB port #1 and USB port #2 on the Drivers Station
LeftJoyStick = new Joystick(1);
RightJoyStick = new Joystick(2);
LeftDrive = new Victor(4,1);
RightDrive = new Victor(4,2);
Winch = new Victor(4,3);
Roller = new Victor(4,4);

// Define servos
Indicator = new Servo(4,10);

// Define analog inputs
Accelerometer = new AnalogChannel(/*Slot*/ 1,/*Port*/ 1);

// Initialize counters to record the number of loops completed in autonomous and teleop modes
m_autoPeriodicLoops = 0;
m_disabledPeriodicLoops = 0;
m_telePeriodicLoops = 0;

printf("BuiltinDefaultCode Constructor Completed\n");
}


/********************************** Init Routines *************************************/

void RobotInit(void) {
// Actions which would be performed once (and only once) upon initialization of the
// robot would be put here.

printf("RobotInit() completed.\n");
}

void DisabledInit(void) {
m_disabledPeriodicLoops = 0; // Reset the loop counter for disabled mode
}

void AutonomousInit(void) {
m_autoPeriodicLoops = 0; // Reset the loop counter for autonomous mode
}

void TeleopInit(void) {
m_telePeriodicLoops = 0; // Reset the loop counter for teleop mode
m_dsPacketsReceivedInCurrentSecond = 0; // Reset the number of dsPackets in current second
}

/********************************** Periodic Routines *************************************/

void DisabledPeriodic(void) {
// feed the user watchdog at every period when disabled
GetWatchdog().Feed();

// increment the number of disabled periodic loops completed
m_disabledPeriodicLoops++;

// while disabled, printout the duration of current disabled mode in seconds
if ((m_disabledPeriodicLoops % (UINT32)GetLoopsPerSec()) == 0) {
printf("Disabled seconds: %d\r\n", (m_disabledPeriodicLoops / (UINT32)GetLoopsPerSec()));
}

}

void AutonomousPeriodic(void) {
// feed the user watchdog at every period when in autonomous
GetWatchdog().Feed();

m_autoPeriodicLoops++;
}


void TeleopPeriodic(void) {
// feed the user watchdog at every period when in autonomous
GetWatchdog().Feed();

// increment the number of teleop periodic loops completed
m_telePeriodicLoops++;

/*
* NOTE: Anything placed here will be called on each iteration of the periodic loop.
* Since the default speed of the loop is 200 Hz, code should only really be placed here
* for I/O that can respond at a 200Hz rate. (e.g. the Jaguar speed controllers
*/

// put 200Hz Jaguar control here

#define SINGLE_LOW_LIMIT 460
#define SINGLE_HIGH_LIMIT 504

gforce_array[gforce_index] = Accelerometer->GetValue();
#ifdef NOTDEF
if(gforce_array[gforce_index]>SINGLE_HIGH_LIMIT) {
gforce_array[gforce_index]=SINGLE_HIGH_LIMIT;
}
else if(gforce_array[gforce_index]<SINGLE_LOW_LIMIT) {
gforce_array[gforce_index]=SINGLE_LOW_LIMIT;
}
#endif
if(gforce_array[gforce_index]>highVal) {
highVal = gforce_array[gforce_index];
}
else if(gforce_array[gforce_index]<lowVal){
lowVal = gforce_array[gforce_index];
}
gforce_index++;
if(gforce_index >= BUFFERSIZE)
gforce_index = 0;

if ((m_telePeriodicLoops % 2) == 0) {
/*
* Code placed in here will be called on every alternate iteration of the periodic loop.
* Assuming the default 200 Hz loop speed, code should only really be placed here for
* I/O that can respond at a 100Hz rate. (e.g. the Victor speed controllers)
*/

#define ROLLERUP ((m_ds->GetDigitalIn(1)) == 0)
#define ROLLERDOWN ((m_ds->GetDigitalIn(2)) == 0)
#define LIFTUP ((m_ds->GetDigitalIn(3)) == 0)
#define LIFTDOWN ((m_ds->GetDigitalIn(4)) == 0)

// put 100Hz Victor control here
LeftDrive->Set(LeftJoyStick->GetY());
RightDrive->Set(RightJoyStick->GetY());

if(ROLLERUP) {
Roller->Set(0.3);
}
else if(ROLLERDOWN) {
Roller->Set(-0.3);
}
else {
Roller->Set(0.0);
}
if(LIFTUP) {
Winch->Set(0.3);
}
else if(LIFTDOWN) {
Winch->Set(-0.3);
}
else {
Winch->Set(0.0);
}
}

if ((m_telePeriodicLoops % 4) == 0) {
/*
* Code placed in here will be called on every fourth iteration of the periodic loop.
* Assuming the default 200 Hz loop speed, code should only really be placed here for
* I/O that can respond at a 50Hz rate. (e.g. the Hitec HS-322HD servos)
*/

// put 50Hz Servo control here
{
INT32 sum=0;
float indicator_out;

#define LOW_LIMIT (SINGLE_LOW_LIMIT*BUFFERSIZE)
#define HIGH_LIMIT (SINGLE_HIGH_LIMIT*BUFFERSIZE)

for(int i=0;i<BUFFERSIZE;i++) {
sum+=gforce_array[i];
}

if(sum<LOW_LIMIT)
sum=LOW_LIMIT;
else if(sum>HIGH_LIMIT)
sum=HIGH_LIMIT;

indicator_out = (sum-LOW_LIMIT)/(float)(HIGH_LIMIT-LOW_LIMIT);

/*
printf("Analog input: %d\n",(INT32)a1.GetValue());
printf("Indicator: %f\n",indicator_out);
printf("Sum: %d\n",sum);
*/

Indicator->Set(indicator_out);
}
}

if (m_ds->GetPacketNumber() != m_priorPacketNumber) {
/*
* Code placed in here will be called only when a new packet of information
* has been received by the Driver Station. Any code which needs new information
* from the DS should go in here
*/

m_priorPacketNumber = m_ds->GetPacketNumber();
m_dsPacketsReceivedInCurrentSecond++; // increment DS packets received

// put Driver Station-dependent code here


} // if (m_ds->GetPacketNumber()...

if ((m_telePeriodicLoops % (UINT32)GetLoopsPerSec()) == 0) {
/*
* Code placed in here will be called once a second
*
* For this example code, print out the number of seconds spent in current teleop mode,
* as well as the number of DS packets received in the prior second.
*/

/* below code commented out to avoid cluttering display
printf("Teleop seconds: %d", (m_telePeriodicLoops / GetLoopsPerSec()));
printf(" DS Packets: %u", m_dsPacketsReceivedInCurrentSecond);
printf("\r\n");
*/

// reset the count of DS packets received in the current second
m_dsPacketsReceivedInCurrentSecond = 0;

if(ROLLERUP) {
printf("Roller Up\n");
}
if(ROLLERDOWN) {
printf("Roller Down\n");
}
if(LIFTUP) {
printf("Lift Up\n");
}
if(LIFTDOWN) {
printf("Lift Down\n");
}

printf("Low: %d, High: %d\n",lowVal,highVal);
}

} // TeleopPeriodic(void)


/********************************** Continuous Routines *************************************/

/*
* These routines are not used in this demonstration robot
*
*
void DisabledContinuous(void) {
}

void AutonomousContinuous(void) {
}

void TeleopContinuous(void) {
}
*/


/********************************** Miscellaneous Routines *************************************/


};

START_ROBOT_CLASS(BuiltinDefaultCode);