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; }