//Turn 90 degrees. void Turn90(string direction) { direction = beaconDirection == 1 ? "L" : "R"; motor[motorL] = direction == "R" ? DRIVE_SPEED : -DRIVE_SPEED; motor[motorR] = -motor[motorL]; wait10Msec(turnTime); StopMotors(); }
int SControllerInterrupt(void) { if(!SensorValue[RX_IO]) { // Stop the robot and wait for 100ms, then return an Interrupt code. StopMotors(); wait1Msec(100); return 1; } else { return 0; } }
void GoInches(float inches, int speed) { nMotorEncoder[motorL] = 0; nMotorEncoder[motorR] = 0; wait1Msec(200); motor[motorL] = speed; motor[motorR] = speed; while ((abs(nMotorEncoder[motorR]) + abs(nMotorEncoder[motorL])) / 2 < (convert(inches))) { } StopMotors(); }
// Move foward until the beacon is found. void MovetoIR() { int FindState = 1; bool FoundIt = false; nMotorEncoder[motorL] = 0; nMotorEncoder[motorR] = 0; wait1Msec(200); // Start moving. driveMotors(DRIVE_SPEED, DRIVE_SPEED); while(!FoundIt) { switch (FindState) { case 1: // Look for target // Get the direction. _dirAC = HTIRS2readACDir(IRseeker); // Make 0 straight ahead, all positive, no left or right worry. _dirAC = abs(_dirAC - 5); // Get the strength. nxtDisplayTextLine(1, "IR: %d", _dirAC); HTIRS2readAllACStrength(IRseeker, acS1, acS2, acS3, acS4, acS5); maxSig = (acS1 > acS2) ? acS1 : acS2; maxSig = (maxSig > acS3) ? maxSig : acS3; maxSig = (maxSig > acS4) ? maxSig : acS4; maxSig = (maxSig > acS5) ? maxSig : acS5; nxtDisplayTextLine(2, "maxSig: %d", maxSig); wait10Msec(2); if (_dirAC >= irGoal) { StopMotors(); DistanceToIR = nMotorEncoder[motorR]; FindState++; } break; case 2: // Look for strongest signal. FindState++; break; case 3: // Backup a little. FindState++; FoundIt = true; break; } } }
task main() { initializeRobot(); //waitForStart(); MovetoIR(); DumpBlock(); BackToStart(); Turn90(Left); GoInches(InchesToTape, DRIVE_SPEED); Turn90(Right); GoInches(InchesToRamp, DRIVE_SPEED); StopMotors(); // Wait for FCS to stop us. while (true) { } }
void readSerialCommand() { // Check for serial message if (SERIAL_AVAILABLE()) { queryType = SERIAL_READ(); switch (queryType) { case 'A': // Receive roll and pitch rate mode PID readSerialPID(RATE_XAXIS_PID_IDX); readSerialPID(RATE_YAXIS_PID_IDX); rotationSpeedFactor = readFloatSerial(); break; case 'B': // Receive roll/pitch attitude mode PID readSerialPID(ATTITUDE_XAXIS_PID_IDX); readSerialPID(ATTITUDE_YAXIS_PID_IDX); readSerialPID(ATTITUDE_GYRO_XAXIS_PID_IDX); readSerialPID(ATTITUDE_GYRO_YAXIS_PID_IDX); windupGuard = readFloatSerial(); // defaults found in setup() of AeroQuad.pde break; case 'C': // Receive yaw PID readSerialPID(ZAXIS_PID_IDX); readSerialPID(HEADING_HOLD_PID_IDX); headingHoldConfig = readFloatSerial(); break; case 'D': // Altitude hold PID #if defined AltitudeHoldBaro || defined AltitudeHoldRangeFinder readSerialPID(BARO_ALTITUDE_HOLD_PID_IDX); PID[BARO_ALTITUDE_HOLD_PID_IDX].windupGuard = readFloatSerial(); altitudeHoldBump = readFloatSerial(); altitudeHoldPanicStickMovement = readFloatSerial(); minThrottleAdjust = readFloatSerial(); maxThrottleAdjust = readFloatSerial(); #if defined AltitudeHoldBaro baroSmoothFactor = readFloatSerial(); #else readFloatSerial(); #endif readSerialPID(ZDAMPENING_PID_IDX); #endif break; case 'E': // Receive sensor filtering values aref = readFloatSerial(); minArmedThrottle = readFloatSerial(); break; case 'F': // Receive transmitter smoothing values receiverXmitFactor = readFloatSerial(); for(byte channel = XAXIS; channel<LASTCHANNEL; channel++) { receiverSmoothFactor[channel] = readFloatSerial(); } break; case 'G': // Receive transmitter calibration values channelCal = (int)readFloatSerial(); receiverSlope[channelCal] = readFloatSerial(); break; case 'H': // Receive transmitter calibration values channelCal = (int)readFloatSerial(); receiverOffset[channelCal] = readFloatSerial(); break; case 'I': // Initialize EEPROM with default values initializeEEPROM(); // defined in DataStorage.h writeEEPROM(); storeSensorsZeroToEEPROM(); calibrateGyro(); zeroIntegralError(); #ifdef HeadingMagHold initializeMagnetometer(); #endif #ifdef AltitudeHoldBaro initializeBaro(); #endif break; case 'J': // calibrate gyros calibrateGyro(); break; case 'K': // Write accel calibration values accelScaleFactor[XAXIS] = readFloatSerial(); readFloatSerial(); accelScaleFactor[YAXIS] = readFloatSerial(); readFloatSerial(); accelScaleFactor[ZAXIS] = readFloatSerial(); readFloatSerial(); computeAccelBias(); storeSensorsZeroToEEPROM(); break; case 'L': // generate accel bias computeAccelBias(); #if defined(AeroQuadMega_CHR6DM) || defined(APM_OP_CHR6DM) calibrateKinematics(); accelOneG = meterPerSecSec[ZAXIS]; #endif storeSensorsZeroToEEPROM(); break; case 'M': // calibrate magnetometer #ifdef HeadingMagHold magBias[XAXIS] = readFloatSerial(); magBias[YAXIS] = readFloatSerial(); magBias[ZAXIS] = readFloatSerial(); writeEEPROM(); #else skipSerialValues(3); #endif break; case 'N': // battery monitor #ifdef BattMonitor batteryMonitorAlarmVoltage = readFloatSerial(); batteryMonitorThrottleTarget = readFloatSerial(); batteryMonitorGoingDownTime = readFloatSerial(); setBatteryCellVoltageThreshold(batteryMonitorAlarmVoltage); #else skipSerialValues(3); #endif break; case 'O': // define waypoints #ifdef UseGPSNavigator missionNbPoint = readIntegerSerial(); waypoint[missionNbPoint].latitude = readIntegerSerial(); waypoint[missionNbPoint].longitude = readIntegerSerial(); waypoint[missionNbPoint].altitude = readIntegerSerial(); #else for(byte i = 0; i < 4; i++) { readFloatSerial(); } #endif break; case 'P': // read Camera values #ifdef CameraControl cameraMode = readFloatSerial(); servoCenterPitch = readFloatSerial(); servoCenterRoll = readFloatSerial(); servoCenterYaw = readFloatSerial(); mCameraPitch = readFloatSerial(); mCameraRoll = readFloatSerial(); mCameraYaw = readFloatSerial(); servoMinPitch = readFloatSerial(); servoMinRoll = readFloatSerial(); servoMinYaw = readFloatSerial(); servoMaxPitch = readFloatSerial(); servoMaxRoll = readFloatSerial(); servoMaxYaw = readFloatSerial(); #ifdef CameraTXControl servoTXChannels = readFloatSerial(); #endif #else #ifdef CameraTXControl skipSerialValues(14) #else skipSerialValues(13); #endif #endif break; case 'R': //Rotate RotateDrone( readFloatSerial()); break; case 'S': StopMotors(); break; case 'T': ThrottleDrone(readFloatSerial()); break; case 'U': // Range Finder #if defined (AltitudeHoldRangeFinder) maxRangeFinderRange = readFloatSerial(); minRangeFinderRange = readFloatSerial(); #else skipSerialValues(2); #endif break; case 'V': // GPS #if defined (UseGPSNavigator) readSerialPID(GPSROLL_PID_IDX); readSerialPID(GPSPITCH_PID_IDX); readSerialPID(GPSYAW_PID_IDX); writeEEPROM(); #else skipSerialValues(9); #endif break; case 'W': // Write all user configurable values to EEPROM writeEEPROM(); // defined in DataStorage.h zeroIntegralError(); break; case 'X': // Stop sending messages break; case '1': // Calibrate ESCS's by setting Throttle high on all channels validateCalibrateCommand(1); break; case '2': // Calibrate ESC's by setting Throttle low on all channels validateCalibrateCommand(2); break; case '3': // Test ESC calibration if (validateCalibrateCommand(3)) { testCommand = readFloatSerial(); } break; case '4': // Turn off ESC calibration if (validateCalibrateCommand(4)) { calibrateESC = 0; testCommand = 1000; } break; case '5': // Send individual motor commands (motor, command) if (validateCalibrateCommand(5)) { for (byte motor = 0; motor < LASTMOTOR; motor++) { motorConfiguratorCommand[motor] = (int)readFloatSerial(); } } break; case 'Z': // fast telemetry transfer <--- get rid if this? if (readFloatSerial() == 1.0) fastTransfer = ON; else fastTransfer = OFF; break; } } }
static ES_Event DuringStateFollowing( ES_Event Event) { ES_Event ReturnEvent = Event; // assme no re-mapping or comsumption // process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events if ( (Event.EventType == ES_ENTRY) || (Event.EventType == ES_ENTRY_HISTORY) ) { // implement any entry actions required for this state machine // after that start any lower level machines that run in this state //StartLowerLevelSM( Event ); // repeat the StartxxxSM() functions for concurrent state machines // on the lower level ES_Timer_InitTimer(DRIVE_TIMER, CONTROL_LOOP_TIMEOUT); } else if ( Event.EventType == ES_EXIT ) { // on exit, give the lower levels a chance to clean up first //RunLowerLevelSM(Event); // repeat for any concurrently running state machines // now do any local exit functionality }else // do the 'during' function for this state { // run any lower level state machine // ReturnEvent = RunLowerLevelSM(Event); // repeat for any concurrent lower level machines //Get the current line position from Camera. Error = ReturnLineCenter(); //Inversion logic says to make error opposite of last lost line #ifdef INVERSION_LOGIC if(!ReturnLineFound) { Error = -Error; } #endif //Lowpass applies a filter to previous line centers #ifdef LOWPASS //Only add the line center to the average if it was found if(ReturnLineFound()) { //Set LineLostRecovery to false, since line is found LineLostRecovery = false; //Set the past line average to 0, and calculate new running average PastLineAverage = 0; LineLostCounter = 0; //Line is found, so reset the line lost counter for (int i = 0; i < NUM_PAST_CENTER-1; i+= 1) { PastLine[i+1] = PastLine[i]; } PastLine[0] = Error; for (int i = 0; i < NUM_PAST_CENTER; i+= 1) { PastLineAverage += PastLine[i]; } PastLineAverage /= NUM_PAST_CENTER; } else { LineLostCounter += 1; } //Set the error to the previous average Error = PastLineAverage; #endif //Implement anti-windup logic and I-control. //If the accumulated error is greater than some bound, and error is positive, //don't add to the accumulated error if(AccumulatedError > WINDUP_BOUND && Error > 0) { AppendError = 0; //If the accumulated error is less than than negative of some bound, and error is negative, //don't add to the accumulated error } else if(AccumulatedError < -WINDUP_BOUND && Error < 0) { AppendError = 0; //Otherwise, add to the accumulated error } else { AppendError = (float) Error; } AccumulatedError += ((float) CONTROL_LOOP_TIMEOUT/1000)*AppendError; //Implement D-control Derivative = (Error - PreviousError)/((float) CONTROL_LOOP_TIMEOUT/1000); //Set the previous error to the current error PreviousError = Error; //Make the robot follow the line by slowing down one of the motors by a certain amount //and leaving the other motor at the nominal PWM setting Command = Kp*Error+Ki*AccumulatedError+Kd*Derivative; //Implement adaptive speed, which slows the car down based on amount of turning #ifdef ADAPTIVE_SPEED SpeedReduction = Ks*Command; if(SpeedReduction > RIGHT_NOMINAL_DUTY) { SpeedReduction = RIGHT_NOMINAL_DUTY; } #else SpeedReduction = 0; #endif //If the line has been lost for more than LINE_LOST_COUNTER_THRESHOLD cycles, //make the car go straight if(LineLostCounter > LINE_LOST_COUNTER_THRESHOLD) { LineLostRecovery = true; } //Based on the calculated command, slow down one of the wheels, saturate command if too large if(Command >= 0) { if((RIGHT_NOMINAL_DUTY+NEUTRAL_DUTY - Command) < RIGHT_UPPER_DEADBAND) { Command = RIGHT_UPPER_DEADBAND - (RIGHT_NOMINAL_DUTY+NEUTRAL_DUTY - Command); InPWMDeadband = true; } else { InPWMDeadband = false; } LeftInput = LEFT_NOMINAL_DUTY*Direction+NEUTRAL_DUTY-SpeedReduction; if(InPWMDeadband) { RightInput = RIGHT_LOWER_DEADBAND - Command; } else { RightInput = (RIGHT_NOMINAL_DUTY-Command)*Direction+NEUTRAL_DUTY-SpeedReduction; } if(LeftInput > 99) { LeftInput = 99; } else if(LeftInput < 1) { LeftInput = 1; } if(RightInput > 99) { RightInput = 99; } else if(RightInput < 1) { RightInput = 1; } UpdatePWM(LeftInput, RightInput); //May be wrong (opposite) } else if(Command < 0) { if((LEFT_NOMINAL_DUTY + PWM_SCALING*Command+NEUTRAL_DUTY) < LEFT_UPPER_DEADBAND) { Command = LEFT_UPPER_DEADBAND - (LEFT_NOMINAL_DUTY+NEUTRAL_DUTY + PWM_SCALING*Command); InPWMDeadband = true; } else { InPWMDeadband = false; } RightInput = RIGHT_NOMINAL_DUTY*Direction+NEUTRAL_DUTY-SpeedReduction; if(InPWMDeadband) { LeftInput = LEFT_LOWER_DEADBAND - Command; } else { LeftInput = (LEFT_NOMINAL_DUTY+PWM_SCALING*Command)*Direction+NEUTRAL_DUTY-SpeedReduction; } if(LeftInput > 99) { LeftInput = 99; } else if(LeftInput < 1) { LeftInput = 1; } if(RightInput > 99) { RightInput = 99; } else if(RightInput < 1) { RightInput = 1; } UpdatePWM(LeftInput, RightInput); //May be wrong (opposite) } //If the line is lost, ignore everything before and go straight if(LineLostRecovery) { UpdatePWM(50+20*PWM_SCALING, 50+20); } //Poll the ultrasonic sensor reading float pseudoDistance = querydistance(); //Add some logic to get rid of results that don't make sense from the ultrasonic if(pseudoDistance > LOWER_DISTANCE_THRESHOLD && !(pseudoDistance > 0.069 && pseudoDistance < 0.079)) { distance = pseudoDistance; } else { for (int i = 0; i < NUM_PAST_PROXIMITY-1; i+= 1) { PastProximity[i] = 0.12; } } //Implement a low pass filter for the ultrasonic results AverageProximity = 0; for (int i = 0; i < NUM_PAST_PROXIMITY-1; i+= 1) { PastProximity[i] = PastProximity[i+1]; } PastProximity[NUM_PAST_PROXIMITY-1] = distance; for (int i = 0; i < NUM_PAST_PROXIMITY; i+= 1) { AverageProximity += PastProximity[i]; } AverageProximity /= NUM_PAST_PROXIMITY; //If the distance is within a threshold (too close), rotate 180 if (AverageProximity > LOWER_DISTANCE_THRESHOLD && AverageProximity < DISTANCE_THRESHOLD && LineLostRecovery) { StopMotors(); ReturnEvent.EventType = ROTATE180; //After rotating 180 degrees flush the low pass filter for(int i = 0; i < NUM_PAST_PROXIMITY; i += 1) { PastProximity[i] = DISTANCE_THRESHOLD + 0.05; } } else { //Set a timer for the drive loop timeout. ES_Timer_InitTimer(DRIVE_TIMER, CONTROL_LOOP_TIMEOUT); } // do any activity that is repeated as long as we are in this state } // return either Event, if you don't want to allow the lower level machine // to remap the current event, or ReturnEvent if you do want to allow it. return(ReturnEvent); }
/**************************************************************************** Function RunTemplateSM Parameters ES_Event: the event to process Returns ES_Event: an event to return Description add your description here Notes uses nested switch/case to implement the machine. Author J. Edward Carryer, 2/11/05, 10:45AM ****************************************************************************/ ES_Event RunFollowLine(ES_Event CurrentEvent) { bool MakeTransition = false;/* are we making a state transition? */ FollowLine_t NextState = CurrentState; ES_Event EntryEventKind = { ES_ENTRY, 0 };// default to normal entry to new state ES_Event ReturnEvent = CurrentEvent; // assume we are not consuming event switch (CurrentState) { case FollowWaiting:// If current state is state one // Execute During function for state one. ES_ENTRY & ES_EXIT are // processed here allow the lower level state machines to re-map // or consume the event CurrentEvent = DuringStateWaiting(CurrentEvent); //process any events if (CurrentEvent.EventType != ES_NO_EVENT) {//If an event is active switch (CurrentEvent.EventType) { case START_FOLLOWING: //If event is event one //printf("\rStarted following\r\n"); // Execute action function for state one : event one NextState = Following;//Decide what the next state will be // for internal transitions, skip changing MakeTransition MakeTransition = true; //mark that we are taking a transition // if transitioning to a state with history change kind of entry EntryEventKind.EventType = ES_ENTRY; // optionally, consume or re-map this event for the upper // level state machine ReturnEvent.EventType = ES_NO_EVENT; break; // repeat cases as required for relevant events } } break; // repeat state pattern as required for other states case Following:// If current state is state one // Execute During function for state one. ES_ENTRY & ES_EXIT are // processed here allow the lower level state machines to re-map // or consume the event CurrentEvent = DuringStateFollowing(CurrentEvent); //process any events if (CurrentEvent.EventType != ES_NO_EVENT) { //If an event is active switch (CurrentEvent.EventType) { case ROTATE180: NextState = Reversing; MakeTransition = true; EntryEventKind.EventType = ES_ENTRY; ReturnEvent.EventType = ES_NO_EVENT; break; } } break; case Reversing: StopMotors(); CurrentEvent = DuringStateReversing(CurrentEvent); if(CurrentEvent.EventType == ES_TIMEOUT && CurrentEvent.EventParam == DRIVE_TIMER) { StopMotors(); NextState = Rotating; MakeTransition = true; EntryEventKind.EventType = ES_ENTRY; ReturnEvent.EventType = ES_NO_EVENT; } break; case Rotating: StopMotors(); CurrentEvent = DuringStateRotating(CurrentEvent); if(CurrentEvent.EventType == ES_TIMEOUT && CurrentEvent.EventParam == DRIVE_TIMER) { StopMotors(); NextState = Following; MakeTransition = true; EntryEventKind.EventType = ES_ENTRY; ReturnEvent.EventType = ES_NO_EVENT; } break; // repeat state pattern as required for other states } // If we are making a state transition if (MakeTransition == true) { //Execute exit function for current state CurrentEvent.EventType = ES_EXIT; RunFollowLine(CurrentEvent); CurrentState = NextState; //Modify state variable //Execute entry function for new state //This defaults to ES_ENTRY RunFollowLine(EntryEventKind); } return(ReturnEvent); }