DriveTrain::DriveTrain() :
TrapezoidProfile( maxWheelSpeed , 3.f ),
m_settings( "RobotSettings.txt" ) {
m_settings.update();
m_deadband = 0.02f;
m_sensitivity = m_settings.getFloat( "LOW_GEAR_SENSITIVE" );
// TODO Does robot start in low gear?
m_oldTurn = 0.f;
m_quickStopAccumulator = 0.f;
m_negInertiaAccumulator = 0.f;
m_leftGrbx = new GearBox<Talon>( 7 , 5, 6 , 1 , 2, 3 );
m_rightGrbx = new GearBox<Talon>( 0 , 3 , 4 , 4 , 5, 6 );
m_rightGrbx->setReversed( true );
m_isDefencive = ( false );
// c = PI * 10.16cm [wheel diameter]
// dPerP = c / pulses
m_leftGrbx->setDistancePerPulse( ((3.14159265 * 10.16 )/ 360.0 )* 1.0/3.0);
m_rightGrbx->setDistancePerPulse( ((3.14159265 * 10.16 )/ 360.0 )* 1.0/3.0);
reloadPID();
}
DriveTrain::DriveTrain() : BezierTrapezoidProfile(maxWheelSpeed, 2) {
m_sensitivity = m_settings.GetDouble("LOW_GEAR_SENSITIVE");
m_leftGrbx.setMotorReversed(true);
m_leftGrbx.setEncoderReversed(true);
m_rightGrbx.setEncoderReversed(true);
m_leftGrbx.setDistancePerPulse(72.0 / 2800.0);
m_rightGrbx.setDistancePerPulse(72.0 / 2800.0);
m_leftGrbx.setManual(0.0);
m_rightGrbx.setManual(0.0);
setWidth(27.0);
reloadPID();
}
