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