#include "WPILib.h"

#include <iostream.h>

#include "math.h"

#include "BTCollector.h"

#include "BTRobot.h"

BTCollector::BTCollector(void)

{

collMotor = new Jaguar(PORTS::COLLECTOR_MOTOR);

jamLimit = new DigitalInput(PORTS::COLLECTOR_JAM_LIMIT);

collMotor->Set(0.0);

lastLimitChangeTime = GetClock();

lastLimitPosition = false;

}

void BTCollector::Enable(BTAutonomous *btauto)

{

enabled = true;

float actualCollSpeed = 0.0;

int ramp = 0;

collectorJammed = false;

while(ramp <= 100)

{

actualCollSpeed += 0.01;

collMotor->Set(actualCollSpeed);

ramp += 1;

}

}

void BTCollector::Reverse(void)

{

enabled = true;

collMotor->Set(COLLECTOR_FAST_REVERSE);

}

void BTCollector::Disable(void)

{

enabled = false;

collMotor->Set(0.0);

}

void BTCollector::DetectJam(void)

{

if (jamLimit->Get() != lastLimitPosition)

{

lastLimitChangeTime = GetClock();

}

lastLimitPosition = jamLimit->Get();

printf("Limit Position = %d\n", jamLimit->Get());

if (((GetClock() - lastLimitChangeTime) > COLL_LIMIT_TIMEOUT) && (enabled))

{

Disable();

enabled = true;

printf("COLLECTOR JAMMED!\n");

collectorJammed = true;

}

else

collectorJammed = false;

}