bool FGActuator::Run(void )
{
Input = InputNodes[0]->getDoubleValue() * InputSigns[0];
if( fcs->GetTrimStatus() ) initialized = 0;
if (fail_zero) Input = 0;
if (fail_hardover) Input = clipmax*sign(Input);
Output = Input; // Perfect actuator. At this point, if no failures are present
// and no subsequent lag, limiting, etc. is done, the output
// is simply the input. If any further processing is done
// (below) such as lag, rate limiting, hysteresis, etc., then
// the Input will be further processed and the eventual Output
// will be overwritten from this perfect value.
if (fail_stuck) {
Output = PreviousOutput;
} else {
if (lag != 0.0) Lag(); // models actuator lag
if (rate_limit_incr != 0 || rate_limit_decr != 0) RateLimit(); // limit the actuator rate
if (deadband_width != 0.0) Deadband();
if (hysteresis_width != 0.0) Hysteresis();
if (bias != 0.0) Bias(); // models a finite bias
if (delay != 0) Delay(); // Model transport latency
}
PreviousOutput = Output; // previous value needed for "stuck" malfunction
initialized = 1;
Clip();
if (clip) {
saturated = false;
if (Output >= clipmax && clipmax != 0) saturated = true;
else if (Output <= clipmin && clipmin != 0) saturated = true;
}
if (IsOutput) SetOutput();
return true;
}
task Foreground(){
//servoChangeRate[servo2] = 2;
int timeLeft;
int state=0;
while(true){
ClearTimer(T1);
hogCPU();
//--------------------Robot Code---------------------------//
long armEncoder = nMotorEncoder[blockthrower];
long robotDist = nMotorEncoder[rtWheelMotor] + nMotorEncoder[ltWheelMotor];
long robotDir = nMotorEncoder[ltWheelMotor] - nMotorEncoder[rtWheelMotor];
long distInches = robotDist/IN2CLK;
// Calculate the speed and direction commands
int speedCmd = ForwardDist(path[pathIdx][DIST_IDX], robotDist, path[pathIdx][SPD_IDX]);
int dirCmd=Direction(path[pathIdx][DIR_IDX], robotDir);
int armSpd = FlipperArm(armEncoder, armSetPos);
bool IRval;
//calculate when to increment path
if (abs(speedCmd)<3 && abs(dirCmd)<3) pathIdx++;
// State O Follow Path
if (state==0)
{
if (distInches>48)
{
state=1;
}
}
IRval = Delayatrue(1, SensorValue[IR] == 1 || SensorValue[IR] == 2);
// State 1 Look for IR Beacon
if (state==1)
{
speedCmd=10;
if ( IRval)
{
state=7;
}
else
{
state=2;
}
}
// State 2 Follow Path
if (state==2)
{
if (distInches>55)
{
state=3;
}
}
// State 3 Look for IR Beacon
if (state==3)
{
speedCmd=10;
if ( IRval==true)
{
state=7;
}
else
{
state=4;
}
}
// State 4 Follow Path
if (state==4)
{
if (distInches>74)//36
{
state=5;
}
}
// State 5 Look for IR Beacon
if (state==5)
{
speedCmd=10;
if ( IRval==true)
{
state=7;
}
else
{
state=6;
}
}
// State 6 Follow Path
if (state==6)
{
if (pathIdx == 3)//45
{
state=7;
}
}
if (state==7)// flip arm
{
speedCmd=0;
dirCmd = 0;
armSetPos = 2300;
if (abs(armSetPos - armEncoder) <10)
{
countState++;
if(countState == 3)
state=8;
}
}
if (state==8)
{
speedCmd = 0;
dirCmd = 0;
armSetPos = 0;
if (abs(armSetPos) - abs(armEncoder) < 200)
{
if(pathIdx == 3)
{
state=9;
}
}
}
if (state==9)
{
pathIdx = 3;
}
DebugInt("spd",speedCmd);
DebugInt("dir",robotDir/DEG2CLK);
DebugInt("dist",distInches);
DebugInt("path",pathIdx);
DebugInt("state",state);
DebugInt("irval",SensorValue[IR]);
// Calculate when to move to the next path index
int s=sizeof(path)/sizeof(path[0])-1;
DebugInt("siz",s);
if (pathIdx>s) pathIdx=s;
speedCmd = RateLimit(speedCmd, START_RATE,speedCmdZ1 );
motor[ltWheelMotor]=speedCmd+dirCmd;
motor[rtWheelMotor]=speedCmd-dirCmd;
motor[blockthrower]=armSpd;
//--------------------------Robot Code--------------------------//
// Wait for next itteration
timeLeft=FOREGROUND_MS-time1[T1];
releaseCPU();
wait1Msec(timeLeft);
}// While
}//Foreground
task main(){
//--------------------INIT Code---------------------------//
ForwardDistReset((tMotor)rtMotor, (tMotor)ltMotor);
DirectionReset();
nMotorEncoder[blockthrower] = 0;
speedCmdZ1=0;
pathIdx=0;
delayatruecount=0;
int state=0;
Pid_Init1();
Pid_Init2();
//--------------------End INIT Code--------------------------//
waitForStart(); // Wait for the beginning of autonomous phase.
int iFrameCnt=0;
int timeLeft;
servo[irArm]=243;
while(true){
ClearTimer(T1);
hogCPU();
//--------------------Robot Code---------------------------//
long armEncoder = nMotorEncoder[blockthrower];
long robotDist = nMotorEncoder[rtMotor] + nMotorEncoder[ltMotor];
long robotDir = nMotorEncoder[ltMotor] - nMotorEncoder[rtMotor];
long distInches = robotDist/IN2CLK;
long dirDegrees = robotDir/27;
// Calculate the speed and direction commands
int speedCmd = ForwardDist(path[pathIdx][DIST_IDX], robotDist, path[pathIdx][SPD_IDX]);
int dirCmd = Direction(path[pathIdx][DIR_IDX], robotDir);
int armSpd = FlipperArm(armEncoder, armSetPos);
bool IRval;
//calculate when to increment path
if (abs(path[pathIdx][DIR_IDX] - dirDegrees) < 4 && abs(path[pathIdx][DIST_IDX] - distInches) < 3) pathIdx++;
// Calculate the IR value
IRval = Delayatrue(1, SensorValue[IR] == 5 || SensorValue[IR] == 6);
if (state==0)// State O Follow Path
{
if (distInches>28)
{
state=1;
}
}
if(state==1)// State 1 Swing out irArm
{
servo[irArm]=150;
if (distInches>34)
{
state=2;
}
}
if (state==2)// state 2 look for ir under box 1
{
if ( IRval||SensorValue[IR] == 3||SensorValue[IR] == 2)
{
state=12;
servo[irArm]=243;
}
else
{
state=3;
}
}
if (state==12)//follows path before flipping arm
{
//speedCmd=0;
if(distInches>44)
{
state = 8;
}
}
if (state==3)//state 3 look for box 2 and follow path
{
if (distInches>46)
{
state=4;
}
}
if (state==4)//state 4 look for ir under box 2
{
if ( IRval==true||SensorValue[IR] == 3)
{
state=13;
servo[irArm]=243;
}
else
{
state=15;
}
servo[irArm]=243;
}
if (state==13) // waits for distance before flipping
{
if(distInches>57)
{
state = 8;
}
}
if (state==15) // pulls servo arm out
{
if(distInches>57)
{
servo[irArm] = 80;
state =5;
}
}
if (state==5)//state 5 look for box 3 and follow path
{
if (distInches>66)//36
{
state=6;
}
}
if (state==6)// State 6 Look for ir under box 3
{
if ( IRval||SensorValue[IR] == 4||SensorValue[IR] == 3||SensorValue[IR] == 2)
{
state=14;
}
else
{
state=7;
}
servo[irArm]=243;
}
if (state==14)// waits distance before flipping arm
{
if(distInches>69)
state = 8;
}
if (state==7)// State 7 look for box 4
{
if (pathIdx == 3)//45
{
state=8;
}
}
if (state==8)// state 8 flip arm
{
servo[irArm]=243;
speedCmd=0;
dirCmd = 0;
armSetPos = 2300;
if (abs(armSetPos - armEncoder) <10)
{
state=9;
}
}
if (state==9)//state 9 lower arm
{
speedCmd = 0;
dirCmd = 0;
armSetPos = 0;
if (abs(armSetPos - armEncoder) < 400)
{
pathIdx=3;
state=10;
}
}
if(state==10)//state 10 follow path
{
}
//DebugInt("spd",speedCmd);
//DebugInt("dir",robotDir/DEG2CLK);
//DebugInt("dist",distInches);
//DebugInt("path",pathIdx);
//DebugInt("state",state);
DebugInt("irval",SensorValue[IR]);
// Calculate when to move to the next path index
int s=sizeof(path)/sizeof(path[0])-1;
if (pathIdx>s) pathIdx=s; // Protect the path index
// Ramp the command up
speedCmd = RateLimit(speedCmd, START_RATE,speedCmdZ1);
leftmotors=Pid1(speedCmd+dirCmd);
rightmotors=Pid2(speedCmd-dirCmd);
//rightmotors=speedCmd-dirCmd;
//leftmotors=speedCmd+dirCmd;
motor[rtBack]=rightmotors;
motor[rtMotor]=rightmotors;
motor[ltMotor]=leftmotors;
motor[ltBack]=leftmotors;
motor[blockthrower]=armSpd;
//DebugInt("rightmotors",rightmotors);
//DebugInt("leftmotors",leftmotors);
//DebugInt("rightencoder",nMotorEncoder[rtMotor]);
//DebugInt("leftencoder",nMotorEncoder[ltMotor]);
if (iFrameCnt==0)
writeDebugStreamLine("i,pthIdx,rbtDist,irval,spdCmd,IR,state, rightencoder, leftencoder");
writeDebugStreamLine("%3i,%3i,%4i,%4i,%3i,%3i,%3i, %4i, %4i",iFrameCnt,pathIdx,distInches,IRval,speedCmd,SensorValue[IR],state, nMotorEncoder[rtMotor], nMotorEncoder[ltMotor]);
//--------------------------Robot Code--------------------------//
// Wait for next itteration
iFrameCnt++;
timeLeft=FOREGROUND_MS-time1[T1];
releaseCPU();
wait1Msec(timeLeft);
}// While
}//Foreground
task Foreground() {
//servoChangeRate[servo2] = 2;
Pid_Init1();
Pid_Init2();
int timeLeft;
int state=0;
int speedCmd = 0;
int dirCmd = 0;
servo[irArm]=105;
const tMUXSensor LEGOUS = msensor_S4_3;
while(true) {
ClearTimer(T1);
hogCPU();
//--------------------Robot Code---------------------------//
long armEncoder = nMotorEncoder[blockthrower];
long robotDist = nMotorEncoder[rtMotor] + nMotorEncoder[ltMotor];
long robotDir = nMotorEncoder[ltMotor] - nMotorEncoder[rtMotor];
long distInches = robotDist/IN2CLK;
long dirDegrees = robotDir/DEG2CLK;
// Calculate the speed and direction commands
if(shortPathTrue == false)
{
speedCmd = ForwardDist(path[pathIdx][DIST_IDX], robotDist, path[pathIdx][SPD_IDX]);
dirCmd=Direction(path[pathIdx][DIR_IDX], robotDir);
}
else
{
speedCmd = ForwardDist(shortPath[pathIdx][DIST_IDX], robotDist, shortPath[pathIdx][SPD_IDX]);
dirCmd=Direction(shortPath[pathIdx][DIR_IDX], robotDir);
}
int armSpd = FlipperArm(armEncoder, armSetPos);
bool IRval;
bool SonarVal;
//calculate when to increment path
if (abs(path[pathIdx][DIR_IDX] - dirDegrees) < 4 && abs(path[pathIdx][DIST_IDX] - distInches) < 3) pathIdx++;
// State O Follow Path
if (state==0)
{
if (distInches>18)
{
state=1;
}
}
IRval = Delayatrue(1, SensorValue[IR] == 4 || SensorValue[IR] == 3);
// State 1 Look for IR Beacon
if (state==1)
{
speedCmd=10;
if ( IRval)
{
state=7;
}
else
{
state=2;
}
}
// State 2 Follow Path
if (state==2)
{
if (distInches>27)
{
state=3;
}
}
// State 3 Look for IR Beacon
if (state==3)
{
speedCmd=10;
if ( IRval==true)
{
state=7;
}
else
{
state=4;
}
}
// State 4 Follow Path
if (state==4)
{
if (distInches>46)//36
{
state=5;
}
}
// State 5 Look for IR Beacon
if (state==5)
{
speedCmd=10;
if ( IRval==true)
{
state=7;
}
else
{
state=6;
}
}
// State 6 Follow Path
if (state==6)
{
if (pathIdx == 1)//45
{
state=7;
}
}
if (state==7)// flip arm
{
speedCmd=0;
dirCmd = 0;
armSetPos = 1900;
if (abs(armSetPos - armEncoder) <20)
{
state=8;
}
servo[irArm]=243;
}
if (state==8)
{
speedCmd = 0;
dirCmd = 0;
armSetPos = 0;
if (abs(armSetPos) - abs(armEncoder) < 200)
{
state=9;
}
}
SonarVal = Delayatrue2(1, USreadDist(LEGOUS) > 40);
if (state==9)
{
if ((distInches>90) && (distInches<115))
{
if(SonarVal)
{
state=10;
}
else
{
state=11;
}
}
}
if(state==10)
{
shortPathTrue=true;
}
if(state==11)
{
}
/*
DebugInt("spd",speedCmd);
DebugInt("dir",robotDir/DEG2CLK);
DebugInt("sonarval",SonarVal);
DebugInt("path",pathIdx);
DebugInt("state",state);
DebugInt("dist",distInches);
DebugInt("ir", SensorValue[IR]);
*/
writeDebugStreamLine("i,pthIdx,rbtDist,irval,spdCmd,IR,state, rightencoder, leftencoder");
writeDebugStreamLine("%3i,%3i,%4i,%4i,%3i,%3i,%3i, %4i, %4i",iFrameCnt,pathIdx,distInches,IRval,speedCmd,SensorValue[IR],state, nMotorEncoder[rtMotor], nMotorEncoder[ltMotor]);
// Calculate when to move to the next path index
int s=sizeof(path)/sizeof(path[0])-1;
DebugInt("siz",s);
if (pathIdx>s) pathIdx=s;
speedCmd = RateLimit(speedCmd, START_RATE,speedCmdZ1 );
rightmotors=speedCmd-dirCmd;
leftmotors=speedCmd+dirCmd;
motor[rtBack]=rightmotors;
motor[rtMotor]=rightmotors;
motor[ltMotor]=leftmotors;
motor[ltBack]=leftmotors;
motor[blockthrower]=armSpd;
DebugInt("rightmotors",rightmotors);
DebugInt("leftmotors",leftmotors);
//--------------------------Robot Code--------------------------//
// Wait for next itteration
timeLeft=FOREGROUND_MS-time1[T1];
releaseCPU();
wait1Msec(timeLeft);
}// While
}//Foreground
void AI::analyzeOthers(AI_Car *c, float dt, const std::list <CAR> & othercars)
{
//const float speed = std::max(1.0f,c->car->GetVelocity().Magnitude());
const float half_carlength = 1.25; //in meters
//std::cout << speed << ": " << authority << std::endl;
//const MATHVECTOR <float, 3> steer_right_axis = direction::Right;
const MATHVECTOR <float, 3> throttle_axis = direction::Forward;
#ifdef VISUALIZE_AI_DEBUG
//c->avoidancedraw->ClearLine();
#endif
for (std::list <CAR>::const_iterator i = othercars.begin(); i != othercars.end(); ++i)
{
if (&(*i) != c->car)
{
struct AI_Car::OTHERCARINFO & info = c->othercars[&(*i)];
//find direction of othercar in our frame
MATHVECTOR <float, 3> relative_position = i->GetCenterOfMassPosition() - c->car->GetCenterOfMassPosition();
(-c->car->GetOrientation()).RotateVector(relative_position);
//std::cout << relative_position.dot(throttle_axis) << ", " << relative_position.dot(steer_right_axis) << std::endl;
//only make a move if the other car is within our distance limit
float fore_position = relative_position.dot(throttle_axis);
//float speed_diff = i->GetVelocity().dot(throttle_axis) - c->car->GetVelocity().dot(throttle_axis); //positive if other car is faster
MATHVECTOR <float, 3> myvel = c->car->GetVelocity();
MATHVECTOR <float, 3> othervel = i->GetVelocity();
(-c->car->GetOrientation()).RotateVector(myvel);
(-i->GetOrientation()).RotateVector(othervel);
float speed_diff = othervel.dot(throttle_axis) - myvel.dot(throttle_axis); //positive if other car is faster
//std::cout << speed_diff << std::endl;
//float distancelimit = clamp(distancelimitcoeff*-speed_diff, distancelimitmin, distancelimitmax);
const float fore_position_offset = -half_carlength;
if (fore_position > fore_position_offset)// && fore_position < distancelimit) //only pay attention to cars roughly in front of us
{
//float horizontal_distance = relative_position.dot(steer_right_axis); //fallback method if not on a patch
//float orig_horiz = horizontal_distance;
const BEZIER * othercarpatch = GetCurrentPatch(&(*i));
const BEZIER * mycarpatch = GetCurrentPatch(c->car);
if (othercarpatch && mycarpatch)
{
float my_track_placement = GetHorizontalDistanceAlongPatch(*mycarpatch, TransformToPatchspace(c->car->GetCenterOfMassPosition()));
float their_track_placement = GetHorizontalDistanceAlongPatch(*othercarpatch, TransformToPatchspace(i->GetCenterOfMassPosition()));
float speed_diff_denom = clamp(speed_diff, -100, -0.01);
float eta = (fore_position-fore_position_offset)/-speed_diff_denom;
info.fore_distance = fore_position;
if (!info.active)
info.eta = eta;
else
info.eta = RateLimit(info.eta, eta, 10.f*dt, 10000.f*dt);
float horizontal_distance = their_track_placement - my_track_placement;
//if (!info.active)
info.horizontal_distance = horizontal_distance;
/*else
info.horizontal_distance = RateLimit(info.horizontal_distance, horizontal_distance, spacingdistance*dt, spacingdistance*dt);*/
//std::cout << info.horizontal_distance << ", " << info.eta << std::endl;
info.active = true;
}
else
info.active = false;
//std::cout << orig_horiz << ", " << horizontal_distance << ", " << fore_position << ", " << speed_diff << std::endl;
/*if (!min_horizontal_distance)
min_horizontal_distance = optional <float> (horizontal_distance);
else if (std::abs(min_horizontal_distance.get()) > std::abs(horizontal_distance))
min_horizontal_distance = optional <float> (horizontal_distance);*/
}
else
info.active = false;
/*#ifdef VISUALIZE_AI_DEBUG
if (info.active)
{
c->avoidancedraw->AddLinePoint(c->car->GetCenterOfMassPosition());
MATHVECTOR <float, 3> feeler1(speed*info.eta,0,0);
c->car->GetOrientation().RotateVector(feeler1);
MATHVECTOR <float, 3> feeler2(0,-info.horizontal_distance,0);
c->car->GetOrientation().RotateVector(feeler2);
c->avoidancedraw->AddLinePoint(c->car->GetCenterOfMassPosition()+feeler1+feeler2);
c->avoidancedraw->AddLinePoint(c->car->GetCenterOfMassPosition());
}
#endif*/
}
}
}
void AI::updateGasBrake(AI_Car *c)
{
c->brakelook.clear();
float brake_value = 0.0;
float gas_value = 0.5;
const float speed_percent = 1.0;
if (c->car->GetEngineRPM() < c->car->GetEngineStallRPM())
c->inputs[CARINPUT::START_ENGINE] = 1.0;
else
c->inputs[CARINPUT::START_ENGINE] = 0.0;
calcMu(c);
const BEZIER *curr_patch_ptr = GetCurrentPatch(c->car);
//if car is not on track, just let it roll
if (!curr_patch_ptr)
{
c->inputs[CARINPUT::THROTTLE] = 0.8;
c->inputs[CARINPUT::BRAKE] = 0.0;
return;
}
BEZIER curr_patch = RevisePatch(curr_patch_ptr, c->use_racingline);
//BEZIER curr_patch = *curr_patch_ptr;
MATHVECTOR <float, 3> patch_direction = TransformToWorldspace(GetPatchDirection(curr_patch));
//this version uses the velocity along tangent vector. it should calculate a lower current speed,
//hence higher gas value or lower brake value
//float currentspeed = c->car->chassis().cm_velocity().component(direction_vector);
float currentspeed = c->car->GetVelocity().dot(patch_direction.Normalize());
//this version just uses the velocity, do not care about the direction
//float currentspeed = c->car->chassis().cm_velocity().magnitude();
//check speed against speed limit of current patch
float speed_limit = 0;
if (!curr_patch.next_patch)
{
speed_limit = calcSpeedLimit(c, &curr_patch, NULL, c->lateral_mu, GetPatchWidthVector(*curr_patch_ptr).Magnitude())*speed_percent;
}
else
{
BEZIER next_patch = RevisePatch(curr_patch.next_patch, c->use_racingline);
speed_limit = calcSpeedLimit(c, &curr_patch, &next_patch, c->lateral_mu, GetPatchWidthVector(*curr_patch_ptr).Magnitude())*speed_percent;
}
speed_limit *= c->difficulty;
float speed_diff = speed_limit - currentspeed;
if (speed_diff < 0.0)
{
if (-speed_diff < MIN_SPEED_DIFF) //no need to brake if diff is small
{
brake_value = 0.0;
}
else
{
brake_value = -speed_diff / MAX_SPEED_DIFF;
if (brake_value > 1.0) brake_value = 1.0;
}
gas_value = 0.0;
}
else if (isnan(speed_diff) || speed_diff > MAX_SPEED_DIFF)
{
gas_value = 1.0;
brake_value = 0.0;
}
else
{
gas_value = speed_diff / MAX_SPEED_DIFF;
brake_value = 0.0;
}
//check upto maxlookahead distance
float maxlookahead = calcBrakeDist(c, currentspeed, 0.0, c->longitude_mu)+10;
//maxlookahead = 0.1;
float dist_checked = 0.0;
float brake_dist = 0.0;
BEZIER patch_to_check = curr_patch;
#ifdef VISUALIZE_AI_DEBUG
c->brakelook.push_back(patch_to_check);
#endif
while (dist_checked < maxlookahead)
{
BEZIER * unmodified_patch_to_check = patch_to_check.next_patch;
//if there is no next patch(probably a non-closed track, just let it roll
if (!patch_to_check.next_patch)
{
brake_value = 0.0;
dist_checked = maxlookahead;
break;
}
else
patch_to_check = RevisePatch(patch_to_check.next_patch, c->use_racingline);
#ifdef VISUALIZE_AI_DEBUG
c->brakelook.push_back(patch_to_check);
#endif
//speed_limit = calcSpeedLimit(c, &patch_to_check, c->lateral_mu)*speed_percent;
if (!patch_to_check.next_patch)
{
speed_limit = calcSpeedLimit(c, &patch_to_check, NULL, c->lateral_mu, GetPatchWidthVector(*unmodified_patch_to_check).Magnitude())*speed_percent;
}
else
{
BEZIER next_patch = RevisePatch(patch_to_check.next_patch, c->use_racingline);
speed_limit = calcSpeedLimit(c, &patch_to_check, &next_patch, c->lateral_mu, GetPatchWidthVector(*unmodified_patch_to_check).Magnitude())*speed_percent;
}
dist_checked += GetPatchDirection(patch_to_check).Magnitude();
brake_dist = calcBrakeDist(c, currentspeed, speed_limit, c->longitude_mu);
//if (brake_dist + CORNER_BRAKE_OFFSET > dist_checked)
if (brake_dist > dist_checked)
{
//std::cout << "brake: limit " << speed_limit << ", cur " << currentspeed << ", brake " << brake_dist << ", dist " << dist_checked << std::endl;
/*brake_value = (brake_dist + CORNER_BRAKE_OFFSET - dist_checked)*CORNER_BRAKE_GAIN;
if (brake_value > 1.0) brake_value = 1.0;*/
brake_value = 1.0;
gas_value = 0.0;
break;
}
}
//std::cout << speed_limit << std::endl;
if (c->car->GetGear() == 0)
{
c->inputs[CARINPUT::SHIFT_UP] = 1.0;
gas_value = 0.2;
}
else
{
c->inputs[CARINPUT::SHIFT_UP] = 0.0;
}
/*float trafficbrake = brakeFromOthers(c, dt, othercars, speed_diff); //consider traffic avoidance bias
if (trafficbrake > 0)
{
gas_value = 0.0;
brake_value = std::max(trafficbrake, brake_value);
}*/
c->inputs[CARINPUT::THROTTLE] = RateLimit(c->inputs[CARINPUT::THROTTLE], gas_value,
THROTTLE_RATE_LIMIT, THROTTLE_RATE_LIMIT);
c->inputs[CARINPUT::BRAKE] = RateLimit(c->inputs[CARINPUT::BRAKE], brake_value,
BRAKE_RATE_LIMIT, BRAKE_RATE_LIMIT);
//c->inputs[CARINPUT::THROTTLE] = 0.0;
//c->inputs[CARINPUT::BRAKE] = 1.0;
}
task Foreground(){
int timeLeft;
// int iFrameCnt=0;
// int out,in=0;
while(true){
ClearTimer(T1);
hogCPU();
//--------------------UNIT TEST Code---------------------------//
int spdCmdPct=40; // Percent of full motor speed command
// Calculate robot and motor information (speed, distance, direction)
mrtLtClk = nMotorEncoder[ltWheelMotor];
mtrRtClk = nMotorEncoder[rtWheelMotor];
mtrLtSpdClk = mrtLtClk - mrtLtClkOld;mrtLtClkOld = mrtLtClk;
mtrRtSpdClk = mtrRtClk - mtrRtClkOld;mtrRtClkOld = mtrRtClk;
DebugInt(" sLt",mtrLtSpdClk);
DebugInt(" sRt",mtrRtSpdClk);
rbtDistClk = mtrRtClk + mrtLtClk;
rbtDirClk = mrtLtClk - mtrRtClk;
// ------------- The path state machine --------------------//
switch (sm) {
case FWDa: //Drive Forward
dirCmdClk=DIR0;
spdCmdPct=40;
if (rbtDistClk>DST12) sm=TURNa;
break;
case TURNa: //Drive Forward
dirCmdClk=DIR90;
spdCmdPct=40;
bool fall;
fall=FallEdge(abs(dirCmdPct)>5,fallOld);
if (fall) {
sm=FWDb;
tmpDist=rbtDistClk;
}
break;
case FWDb: //Drive Forward
dirCmdClk=DIR90;
spdCmdPct=40;
if (rbtDistClk>tmpDist+DST12) sm=TURNb;
break;
case TURNb: //Drive Forward
dirCmdClk=DIRm90;
spdCmdPct=0;
fall=FallEdge(abs(dirCmdPct)>5,fallOld);
DebugInt(" fall",(int)fall);
if (fall) {
sm=STOP;
tmpDist=rbtDistClk;
}
break;
case STOP:
spdCmdPct=0;
break;
default:
spdCmdPct=40;
}
// Calculate the direction command
DebugInt(" rbtDirC",rbtDirClk);
dirCmdPct = LimitSym((dirCmdClk-rbtDirClk)/DIR_KP, TURN_SPEED);
// Calculate the motor commands
int mtrCmdLtPct;
int mtrCmdRtPct;
mtrCmdLtPct=spdCmdPct+dirCmdPct;
mtrCmdRtPct=spdCmdPct-dirCmdPct;
// Limit the rate of change of the motor commands to prevent slipping.
// DebugInt(" mlc1",mtrCmdLtPct);
// DebugInt(" mrc1",mtrCmdRtPct);
mtrCmdLtPct=RateLimit( mtrCmdLtPct, 4, mrtLtRlPct);
mtrCmdRtPct=RateLimit( mtrCmdRtPct, 4, mtrRtRlPct );
/*
// Protect against stalled motors
switch (sm2) {
case MTR_OK: //Drive Forward
#define MTR_TRP 45
if ((abs(mtrLtSpdClk)<MTR_TRP && abs(mtrCmdLtPct)>30)||(abs(mtrRtSpdClk)<MTR_TRP && abs(mtrCmdRtPct)>30)){
mtrBadTmr+=1;
if (mtrBadTmr>7) sm2=MTR_BAD;
} else {
mtrBadTmr=0;
}
break;
case MTR_BAD: //Drive Forward
mtrCmdLtPct=0;
mtrCmdRtPct=0;
break;
default:
}
*/
// Power the drive motors
motor[ltWheelMotor]=mtrCmdLtPct;
motor[rtWheelMotor]=mtrCmdRtPct;
// DebugInt(" mlc2",mtrCmdLtPct);
// DebugInt(" mrc2",mtrCmdRtPct);
// DebugInt(" SM", sm);
DebugInt(" tmr", mtrBadTmr);
DebugInt(" SM2", sm2);
//--------------------------End UNIT TEST Code--------------------------//
// Wait for next itteration
timeLeft=FOREGROUND_MS-time1[T1];
if (timeLeft<0) timeLeft=0;
DebugInt(" time", timeLeft);
releaseCPU();
wait1Msec(timeLeft);
}// While
}//Foreground
void AiCarStandard::UpdateGasBrake()
{
#ifdef VISUALIZE_AI_DEBUG
brakelook.clear();
#endif
float brake_value = 0.0;
float gas_value = 0.5;
if (car->GetEngine().GetRPM() < car->GetEngine().GetStallRPM())
inputs[CarInput::START_ENGINE] = 1.0;
else
inputs[CarInput::START_ENGINE] = 0.0;
CalcMu();
const Bezier * curr_patch_ptr = GetCurrentPatch(car);
if (!curr_patch_ptr)
{
// if car is not on track, just let it roll
inputs[CarInput::THROTTLE] = 0.8;
inputs[CarInput::BRAKE] = 0.0;
return;
}
Bezier curr_patch = RevisePatch(curr_patch_ptr, use_racingline);
const Vec3 patch_direction = GetPatchDirection(curr_patch).Normalize();
const Vec3 car_velocity = ToMathVector<float>(car->GetVelocity());
float currentspeed = car_velocity.dot(patch_direction);
// check speed against speed limit of current patch
float speed_limit = 0;
if (!curr_patch.GetNextPatch())
{
speed_limit = CalcSpeedLimit(&curr_patch, NULL, lateral_mu, GetPatchWidthVector(*curr_patch_ptr).Magnitude());
}
else
{
Bezier next_patch = RevisePatch(curr_patch.GetNextPatch(), use_racingline);
speed_limit = CalcSpeedLimit(&curr_patch, &next_patch, lateral_mu, GetPatchWidthVector(*curr_patch_ptr).Magnitude());
}
speed_limit *= difficulty;
float speed_diff = speed_limit - currentspeed;
if (speed_diff < 0.0)
{
if (-speed_diff < MIN_SPEED_DIFF) //no need to brake if diff is small
{
brake_value = 0.0;
}
else
{
brake_value = -speed_diff / MAX_SPEED_DIFF;
if (brake_value > 1.0) brake_value = 1.0;
}
gas_value = 0.0;
}
else if (std::isnan(speed_diff) || speed_diff > MAX_SPEED_DIFF)
{
gas_value = 1.0;
brake_value = 0.0;
}
else
{
gas_value = speed_diff / MAX_SPEED_DIFF;
brake_value = 0.0;
}
// check upto maxlookahead distance
float maxlookahead = CalcBrakeDist(currentspeed, 0.0, longitude_mu)+10;
float dist_checked = 0.0;
float brake_dist = 0.0;
Bezier patch_to_check = curr_patch;
#ifdef VISUALIZE_AI_DEBUG
brakelook.push_back(patch_to_check);
#endif
while (dist_checked < maxlookahead)
{
Bezier * unmodified_patch_to_check = patch_to_check.GetNextPatch();
if (!patch_to_check.GetNextPatch())
{
// if there is no next patch(probably a non-closed track, just let it roll
brake_value = 0.0;
dist_checked = maxlookahead;
break;
}
else
{
patch_to_check = RevisePatch(patch_to_check.GetNextPatch(), use_racingline);
}
#ifdef VISUALIZE_AI_DEBUG
brakelook.push_back(patch_to_check);
#endif
if (!patch_to_check.GetNextPatch())
{
speed_limit = CalcSpeedLimit(&patch_to_check, NULL, lateral_mu, GetPatchWidthVector(*unmodified_patch_to_check).Magnitude());
}
else
{
Bezier next_patch = RevisePatch(patch_to_check.GetNextPatch(), use_racingline);
speed_limit = CalcSpeedLimit(&patch_to_check, &next_patch, lateral_mu, GetPatchWidthVector(*unmodified_patch_to_check).Magnitude());
}
dist_checked += GetPatchDirection(patch_to_check).Magnitude();
brake_dist = CalcBrakeDist(currentspeed, speed_limit, longitude_mu);
if (brake_dist > dist_checked)
{
brake_value = 1.0;
gas_value = 0.0;
break;
}
}
gas_value = RateLimit(inputs[CarInput::THROTTLE], gas_value, THROTTLE_RATE_LIMIT, THROTTLE_RATE_LIMIT);
brake_value = RateLimit(inputs[CarInput::BRAKE], brake_value, BRAKE_RATE_LIMIT, BRAKE_RATE_LIMIT);
inputs[CarInput::THROTTLE] = gas_value;
inputs[CarInput::BRAKE] = brake_value;
}
