#include "WPILib.h"

#include <iostream>

#include "math.h"

#include "BTCamera.h"

#include "BTTurret.h"

BTTurret::BTTurret(ControlBoard* inputCB)

{

count = 0;

overridden = false;

camera = new BTCamera();

turretEnc = new Encoder(PORTS::TURRET_ENCODER_A, PORTS::TURRET_ENCODER_B, false);

if (PORTS::TEST_BOARD_MODE)

{

turretMotor = new Jaguar(PORTS::TEST_TURRET_MOTOR);

clLimit = new DigitalInput(PORTS::TEST_TURRET_LIMIT_CL);

ccLimit = new DigitalInput(PORTS::TEST_TURRET_LIMIT_CC);

}

else

{

turretMotor = new Jaguar(PORTS::TURRET_MOTOR);

clLimit = new DigitalInput(PORTS::TURRET_LIMIT_CL);

ccLimit = new DigitalInput(PORTS::TURRET_LIMIT_CC);

centerLimit = new DigitalInput(PORTS::TURRET_CENTER_LIMIT);

}

controlBoard = inputCB;

}

#if 0

bool BTTurret::FollowCamera(float pseudoServoValue, bool pseudoOverride)

{

float servoValue;

int targetEncValue;

int actualEncValue;

int deltaEnc;

float motorSpeed;

servoValue = 0.0;

targetEncValue = 0;

actualEncValue = 0;

deltaEnc = 0;

motorSpeed = 0.0;

foundColor = camera->IsTargetInRange();

//this code assumes the positive direction on the motor and encoder are the same

if (foundColor)

{

actualEncValue = (turretEnc->Get() + TURRET_HALF_CIRCLE);

actualEncValue = (actualEncValue % TURRET_FULL_CIRCLE);

if (actualEncValue < 0)

{

actualEncValue = (actualEncValue + TURRET_FULL_CIRCLE);

}

printf("Encoder position = %d\t", actualEncValue);

printf("Target encoder = %d\n", targetEncValue);

deltaEnc = (targetEncValue - actualEncValue);

if (deltaEnc > TURRET_ROTATION_TOLERANCE)

motorSpeed = 0.75;

if (deltaEnc < (TURRET_ROTATION_TOLERANCE * -1))

motorSpeed = -0.75;

/*if (deltaEnc > 0)

motorSpeed = 1.0;

if (deltaEnc < 0)

motorSpeed = -1.0;*/

printf("Motor Speed = %f\n", motorSpeed);

turretMotor->Set(motorSpeed);

if ((!clLimit->Get() && (motorSpeed < 0)) ||

(!ccLimit->Get() && (motorSpeed > 0)))

{

printf("LIMIT OVERRIDE!\n");

turretMotor->Set(0.0);

}

}

else

turretMotor->Set(0.0);

return foundColor;

}

#endif

bool BTTurret::Update(void)

{

double diff = camera->Track(controlBoard);

// printf("Updating turret.\n");

// foundColor = camera->IsTargetInRange();

if (!camera->foundColor)

{

if (!camera->staleImage)

{

COTPan();

// printf("Panning...\n");

}

}

else

{

float motorSpeed;

if (count++ > 10)

{

printf("Target found.\n");

printf("diff = %f\n", diff);

count = 0;

}

/* if (diff > TURRET_ROTATION_TOLERANCE_NORM)

{

motorSpeed = (0.75 * diff);

}

if (diff < (TURRET_ROTATION_TOLERANCE_NORM * -1))

{

motorSpeed = -0.75;

}

*/

motorSpeed = diff;

if ((diff < TURRET_ROTATION_TOLERANCE_NORM)

&& (diff > (TURRET_ROTATION_TOLERANCE_NORM * -1))

&& (diff != 0.0))

motorSpeed = 0.00;

if ((!clLimit->Get() && (motorSpeed < 0)) ||

(!ccLimit->Get() && (motorSpeed > 0)))

{

motorSpeed = 0.0;

printf("LIMIT OVERRIDE!");

}

turretMotor->Set(motorSpeed);

}

return camera->foundColor;

}

void BTTurret::COTPan(void)

{

double motorSpeed;

motorSpeed = turretMotor->Get();

//printf("Initial Speed = %f ccLimit = %d clLimit = %d\n", motorSpeed, ccLimit->Get(), clLimit->Get());

//limit switches are false when pressed

if (!clLimit->Get())

{

motorSpeed = 1.0;

}

if (!ccLimit->Get())

{

motorSpeed = -1.0;

}

if ((!camera->foundColor)

&&(ccLimit->Get())

&&(clLimit->Get())

&&(motorSpeed != -1.00))

{

motorSpeed = 1.0;

}

turretMotor->Set(motorSpeed);

}

void BTTurret::Override(void)

{

float stickValue;

overridden = true;

stickValue = controlBoard->GetX(ControlBoard::SHOOTER);

if (stickValue > 0)

{

if (ccLimit->Get())

{

turretMotor->Set(stickValue);

}

else

turretMotor->Set(0.00);

}

if (stickValue <= 0)

{

if (clLimit->Get())

{

turretMotor->Set(stickValue);

}

else

turretMotor->Set(0.00);

}

// printf("ccLimit = %d, clLimit = %d\n", ccLimit->Get(), clLimit->Get());

}

void BTTurret::Center(void)

{

COTPan();

if (centerLimit->Get())

{

turretMotor->Set(0);

centered = true;

}

}

int BTTurret::ServoToEncRange(float servoValue)

{

int encValue;

float normValue;

encValue = 0;

normValue = 0.0;

normValue = RangeToNormalized(servoValue, 1);

encValue = (int)(normValue * TURRET_QUARTER_CIRCLE);

if (encValue < 0)

{

encValue += TURRET_FULL_CIRCLE;

}

return encValue;

}

bool BTTurret::IsColorFound(void)

{

return camera->foundColor;

}