// Drive during race.
void Driver::drive(tSituation *s)
{
memset(&car->ctrl, 0, sizeof(tCarCtrl));
update(s);
//pit->setPitstop(true);
if (isStuck()) {
car->_steerCmd = -mycardata->getCarAngle() / car->_steerLock;
car->_gearCmd = -1; // Reverse gear.
car->_accelCmd = 1.0f; // 100% accelerator pedal.
car->_brakeCmd = 0.0f; // No brakes.
car->_clutchCmd = 0.0f; // Full clutch (gearbox connected with engine).
} else {
car->_steerCmd = filterSColl(getSteer());
car->_gearCmd = getGear();
car->_brakeCmd = filterABS(filterBrakeSpeed(filterBColl(filterBPit(getBrake()))));
if (car->_brakeCmd == 0.0f) {
car->_accelCmd = filterTCL(filterTrk(filterOverlap(getAccel())));
} else {
car->_accelCmd = 0.0f;
}
car->_clutchCmd = getClutch();
}
}
CarControl ApproachingCurve::drive(FSMDriver5 *FSMDriver5, CarState &cs) {
if(!sensorsAreUpdated) /*@todo Só atualiza na 1a vez mesmo? */
updateSensors(cs);
const int focus = 0, meta = 0;
const float clutch = 0;
return CarControl(getAccel(cs), getBrake(cs), getGear(cs), cs.getAngle(), clutch, focus, meta);
//Use the line below if the behavior of adjusting the car to the curve ahead is desired (not fully functional):
//return CarControl(getAccel(cs), getBrake(cs), getGear(cs), getSteering(cs), clutch, focus, meta);
}
/* Drive during race. */
void Driver::drive(tSituation *s)
{
memset(&car->ctrl, 0, sizeof(tCarCtrl));
update(s);
if (isStuck()) {
car->ctrl.steer = -angle / car->_steerLock;
car->ctrl.gear = -1; // reverse gear
car->ctrl.accelCmd = 0.5; // 50% accelerator pedal
car->ctrl.brakeCmd = 0.0; // no brakes
} else {
car->ctrl.steer = filterSColl(getSteer());
car->ctrl.gear = getGear();
car->ctrl.brakeCmd = filterABS(filterBrakeSpeed(filterBColl(filterBPit(getBrake()))));
if (car->ctrl.brakeCmd == 0.0) {
car->ctrl.accelCmd = filterTCL(filterTrk(getAccel()));
} else {
car->ctrl.accelCmd = 0.0;
}
}
}
static void drive(int index, tCarElt* car, tSituation *s)
{
memset(&car->ctrl, 0, sizeof(tCarCtrl));
if (isStuck(car)) {
float angle = -RtTrackSideTgAngleL(&(car->_trkPos)) + car->_yaw;
NORM_PI_PI(angle); // put the angle back in the range from -PI to PI
car->ctrl.steer = angle / car->_steerLock;
car->ctrl.gear = -1; // reverse gear
car->ctrl.accelCmd = 0.3; // 30% accelerator pedal
car->ctrl.brakeCmd = 0.0; // no brakes
}
else {
float angle;
const float SC = 1.0;
angle = RtTrackSideTgAngleL(&(car->_trkPos)) - car->_yaw;
NORM_PI_PI(angle); // put the angle back in the range from -PI to PI
angle -= SC*(car->_trkPos.toMiddle+keepLR)/car->_trkPos.seg->width;
// set up the values to return
car->ctrl.steer = angle / car->_steerLock;
car->ctrl.gear = getGear(car);
if (car->_speed_x>desired_speed) {
car->ctrl.brakeCmd=0.5;
car->ctrl.accelCmd=0.0;
}
else if (car->_speed_x<desired_speed) {
car->ctrl.accelCmd=0.5;
car->ctrl.brakeCmd=0.0;
}
}
}
CarControl
SimpleDriver::wDrive(CarState cs)
{
// check if car is currently stuck
if ( fabs(cs.getAngle()) > stuckAngle )
{
// update stuck counter
stuck++;
}
else
{
// if not stuck reset stuck counter
stuck = 0;
}
// after car is stuck for a while apply recovering policy
if (stuck > stuckTime)
{
/* set gear and sterring command assuming car is
* pointing in a direction out of track */
// to bring car parallel to track axis
float steer = - cs.getAngle() / steerLock;
int gear=-1; // gear R
// if car is pointing in the correct direction revert gear and steer
if (cs.getAngle()*cs.getTrackPos()>0)
{
gear = 1;
steer = -steer;
}
// Calculate clutching
clutching(cs,clutch);
// build a CarControl variable and return it
CarControl cc (1.0,0.0,gear,steer,clutch);
return cc;
}
else // car is not stuck
{
// compute accel/brake command
float accel_and_brake = getAccel(cs);
// compute gear
int gear = getGear(cs);
// compute steering
float steer = getSteer(cs);
// normalize steering
if (steer < -1)
steer = -1;
if (steer > 1)
steer = 1;
// set accel and brake from the joint accel/brake command
float accel,brake;
if (accel_and_brake>0)
{
accel = accel_and_brake;
brake = 0;
}
else
{
accel = 0;
// apply ABS to brake
brake = filterABS(cs,-accel_and_brake);
}
// Calculate clutching
clutching(cs,clutch);
cout << "Steer: "<< steer << endl;
cout << "Accel: :"<< accel << endl;
// build a CarControl variable and return it
CarControl cc(accel,brake,gear,steer,clutch);
return cc;
}
}
void DriveTrain::drive( float throttle, float turn, bool isQuickTurn ) {
// Modified Cheesy Drive; base code courtesy of FRC Team 254
if (m_isDefencive == true){
throttle = throttle * -1;
turn = turn *-1;
}
// Limit values to [-1 .. 1]
throttle = limit( throttle , 1.f );
turn = limit( turn , 1.f );
/* Apply joystick deadband
* (Negate turn since joystick X-axis is reversed)
*/
throttle = -applyDeadband( throttle );
turn = applyDeadband( turn );
double negInertia = turn - m_oldTurn;
m_oldTurn = turn;
float turnNonLinearity = m_settings.getFloat( "TURN_NON_LINEARITY" );
/* Apply a sine function that's scaled to make turning sensitivity feel better.
* turnNonLinearity should never be zero, but can be close
*/
turn = sin( M_PI / 2.0 * turnNonLinearity * turn ) /
sin( M_PI / 2.0 * turnNonLinearity );
double angularPower = 0.f;
double linearPower = throttle;
double leftPwm = linearPower, rightPwm = linearPower;
// Negative inertia!
double negInertiaScalar;
if ( getGear() ) {
negInertiaScalar = m_settings.getFloat( "INERTIA_HIGH_GEAR" );
}
else {
if ( turn * negInertia > 0 ) {
negInertiaScalar = m_settings.getFloat( "INERTIA_LOW_DAMPEN" );
}
else {
if ( fabs(turn) > 0.65 ) {
negInertiaScalar = m_settings.getFloat( "INERTIA_LOW_HIGH_TURN" );
}
else {
negInertiaScalar = m_settings.getFloat( "INERTIA_LOW_LOW_TURN" );
}
}
}
m_negInertiaAccumulator += negInertia * negInertiaScalar; // adds negInertiaPower
// Apply negative inertia
turn += m_negInertiaAccumulator;
if ( m_negInertiaAccumulator > 1 ) {
m_negInertiaAccumulator -= 1;
}
else if ( m_negInertiaAccumulator < -1 ) {
m_negInertiaAccumulator += 1;
}
else {
m_negInertiaAccumulator = 0;
}
// QuickTurn!
if ( isQuickTurn ) {
if ( fabs(linearPower) < 0.2 ) {
double alpha = 0.1;
m_quickStopAccumulator = (1 - alpha) * m_quickStopAccumulator +
alpha * limit( turn, 1.f ) * 5;
}
angularPower = turn;
}
else {
angularPower = fabs(throttle) * turn * m_sensitivity - m_quickStopAccumulator;
if ( m_quickStopAccumulator > 1 ) {
m_quickStopAccumulator -= 1;
}
else if ( m_quickStopAccumulator < -1 ) {
m_quickStopAccumulator += 1;
}
else {
m_quickStopAccumulator = 0.0;
}
}
// Adjust straight path for turn
leftPwm += angularPower;
rightPwm -= angularPower;
// Limit PWM bounds to [-1..1]
if ( leftPwm > 1.0 ) {
// If overpowered turning enabled
if ( isQuickTurn ) {
rightPwm -= (leftPwm - 1.f);
}
leftPwm = 1.0;
}
else if ( rightPwm > 1.0 ) {
// If overpowered turning enabled
if ( isQuickTurn ) {
leftPwm -= (rightPwm - 1.f);
}
rightPwm = 1.0;
}
else if ( leftPwm < -1.0 ) {
// If overpowered turning enabled
if ( isQuickTurn ) {
rightPwm += (-leftPwm - 1.f);
}
leftPwm = -1.0;
}
else if ( rightPwm < -1.0 ) {
// If overpowered turning enabled
if ( isQuickTurn ) {
leftPwm += (-rightPwm - 1.f);
}
rightPwm = -1.0;
}
m_leftGrbx->setManual( leftPwm );
m_rightGrbx->setManual( rightPwm );
//std::cout << "left PWM:" << leftPwm << std::endl;
//std::cout << "right PWM:" << rightPwm << std::endl;
}
