C1983_Lift(Encoder *liftSensorChannel, Relay *brake)
{
m_liftSensor = liftSensorChannel;
m_brake = brake;
m_bIsBraking = true;
m_brake->SetDirection(m_brake->kBothDirections);
m_liftSensor->SetDistancePerPulse((float)LIFTDISTPERPULSE);
}
SixWheelBot(void):
//myRobot(1, 2, 3, 4), // these must be initialized in the same order
stickL(1), stickR(2), camera()
#if !OI
, stickOp(3)
#endif// as they are declared above.
{
//myRobot.SetExpiration(0.1);
frontLeftMotor = new Jaguar(FRONTLEFTMOTORPORT);
rearLeftMotor = new Jaguar(REARLEFTMOTORPORT);
frontRightMotor = new Jaguar(FRONTRIGHTMOTORPORT);
rearRightMotor = new Jaguar(REARRIGHTMOTORPORT);
liftMotor1 = new Jaguar(LIFTMOTORPORT1);
liftMotor2 = new Jaguar(LIFTMOTORPORT2);
gripMotor1 = new Victor(GRIPMOTORPORT1);
//gripMotor2 = new Victor(GRIPMOTORPORT2);
fakeEncoderA = new Encoder(6,1,6,2,false, fakeEncoderA->k4X);
leftEncoder = new Encoder(4, LEFTMOTORA, 4, LEFTMOTORB, true, leftEncoder->k4X);
//fakeEncoderB = new Encoder(6,3,6,4,false, fakeEncoderB->k1X);
liftEncoder = new Encoder(4, LIFTMOTORA, 4, LIFTMOTORB, false, liftEncoder->k4X);
rightEncoder = new Encoder(4, RIGHTMOTORA, 4, RIGHTMOTORB, false, rightEncoder->k4X);
//fakeEncoderC = new Encoder(6,5,6,6,false, fakeEncoderC->k1X);
//liftEncoder = new Encoder(4, LIFTMOTORA, 4, LIFTMOTORB, false, liftEncoder->k4X);
gripPot = new AnalogChannel(1, GRIPPOTPORT);
compSwitch = new DigitalInput(4, COMPRESSORSWITCH);
lsRight = new DigitalInput(3); //light sensors
lsMiddle = new DigitalInput(2);
lsLeft = new DigitalInput(1);
light = new Relay(4, LIGHTPORT, light->kForwardOnly);
//light = new Relay(6, 8);
brake = new Relay(4, BRAKEPORT, brake->kBothDirections);
shifter = new Relay(4, SHIFTERPORT, shifter->kBothDirections);
miniDep = new Relay(4, MINIBOTPORT);
miniDep->SetDirection(miniDep->kBothDirections);
gripOpen1 = new Solenoid(8, GRIPOPENPORT1);
gripOpen2 = new Solenoid(8, GRIPOPENPORT2);
gripPop1 = new Solenoid(8, GRIPPOPPORT1);
gripPop2 = new Solenoid(8, GRIPPOPPORT2);
gripLatch1 = new Solenoid(8, GRIPLATCH1);
gripLatch2 = new Solenoid(8, GRIPLATCH2);
compressor = new Relay(4, COMPRESSORPORT, compressor->kBothDirections);
driverStation = DriverStation::GetInstance();
lift = new C1983_Lift(liftEncoder, brake);
ultrasonic = new AnalogChannel(1, ULTRASOUNDPORT);
camGimble = new Servo(CAMERAYSERVOPORT);
maxSpeed = 500;
leftPIDSource = new EncoderPIDSource(leftEncoder, &maxSpeed);
rightPIDSource = new EncoderPIDSource(rightEncoder, &maxSpeed);
liftPIDSource = new LiftPIDSource(lift);
gripPIDSource = new GripPIDSource(gripPot);
leftPIDOutput = new TankPIDOutput(frontLeftMotor, rearLeftMotor, false);
rightPIDOutput = new TankPIDOutput(frontRightMotor, rearRightMotor, true);
liftPIDOutput = new LiftPIDOutput(liftMotor1, liftMotor2);
//gripPIDOutput = new GripPIDOutput(gripMotor1, gripMotor2);
gripPIDOutput = new GripPIDOutput(gripMotor1);
PIDDriveLeft = new PIDController(DRIVEPROPGAIN, DRIVEINTGAIN, DRIVEDERIVGAIN,
leftPIDSource, leftPIDOutput);
PIDDriveRight = new PIDController(DRIVEPROPGAIN, DRIVEINTGAIN, DRIVEDERIVGAIN,
rightPIDSource, rightPIDOutput);
PIDLift = new PIDController(LIFTPROPGAIN, LIFTINTGAIN, LIFTDERIVGAIN,
liftPIDSource, liftPIDOutput);
PIDGrip = new PIDController(GRIPPROPGAIN, GRIPINTGAIN, GRIPDERIVGAIN,
gripPIDSource, gripPIDOutput);
/*bcd1 = new DigitalInput(4);
bcd2 = new DigitalInput(5);
bcd3 = new DigitalInput(6);*/
}
RobotDemo(void)
{
///constructs
game = false;//set to true or false before use- true for auton & tele-op, false for just 1
cd = new CustomDrive(NUM_JAGS);
closed = new Solenoid(8, 1);//true if closed
open = new Solenoid(8, 2);
stick = new Joystick(2);// arm controll
controller = new Joystick(1);// base
manEnc = new Encoder(4,5);
reset = false;//if button
manJag = new Jaguar(6);//cons manip Jag
ShoulderJag = new Jaguar(5);
minibot = new DoubleSolenoid(3, 4);cmp = new Compressor(DOC7_RELAY_PORT, DOC7_COMPRESSOR_PORT);
track = new Tracking(DOC7_LEFT_LINE_PORT, DOC7_CENTER_LINE_PORT, DOC7_RIGHT_LINE_PORT);
red_white = new Relay(DOC7_RED_WHITE_SPIKE);
red_white->SetDirection(Relay::kBothDirections);
red_white->Set(Relay::kOff);
blue = new Relay(DOC7_BLUE_SPIKE);
blue->SetDirection(Relay::kBothDirections);
blue->Set(Relay::kOff);
GetWatchdog().Kill();
};