Merge "Add a spit button"
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 66a3dcd..4b36714 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -28,6 +28,11 @@
namespace control_loops {
namespace drivetrain {
+namespace {
+// Maximum variation to allow in the gyro when zeroing.
+constexpr double kMaxYawGyroZeroingRange = 0.05;
+}
+
DrivetrainFilters::DrivetrainFilters(const DrivetrainConfig<double> &dt_config,
::aos::EventLoop *event_loop,
LocalizerInterface *localizer)
@@ -204,9 +209,27 @@
break;
}
- ready_ = dt_config_.gyro_type == GyroType::SPARTAN_GYRO ||
- dt_config_.gyro_type == GyroType::FLIPPED_SPARTAN_GYRO ||
- imu_zeroer_.Zeroed();
+ switch (dt_config_.gyro_type) {
+ case GyroType::SPARTAN_GYRO:
+ case GyroType::FLIPPED_SPARTAN_GYRO:
+ if (!yaw_gyro_zero_.has_value()) {
+ yaw_gyro_zeroer_.AddData(last_gyro_rate_);
+ if (yaw_gyro_zeroer_.GetRange() < kMaxYawGyroZeroingRange) {
+ yaw_gyro_zero_ = yaw_gyro_zeroer_.GetAverage()(0);
+ }
+ }
+ ready_ = yaw_gyro_zero_.has_value();
+ if (ready_) {
+ last_gyro_rate_ = last_gyro_rate_ - yaw_gyro_zero_.value();
+ }
+ break;
+ case GyroType::IMU_X_GYRO:
+ case GyroType::IMU_Y_GYRO:
+ case GyroType::IMU_Z_GYRO:
+ case GyroType::FLIPPED_IMU_Z_GYRO:
+ ready_ = imu_zeroer_.Zeroed();
+ break;
+ }
// TODO(james): How aggressively can we fault here? If we fault to
// aggressively, we might have issues during startup if wpilib_interface takes
diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index 52ae605..a06b965 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -138,6 +138,9 @@
// Last applied voltage.
Eigen::Matrix<double, 2, 1> last_voltage_;
Eigen::Matrix<double, 2, 1> last_last_voltage_;
+
+ std::optional<double> yaw_gyro_zero_;
+ zeroing::Averager<double, 200> yaw_gyro_zeroer_;
};
class DrivetrainLoop
diff --git a/frc971/control_loops/drivetrain/drivetrain_plotter.ts b/frc971/control_loops/drivetrain/drivetrain_plotter.ts
index deb300f..4b59dc0 100644
--- a/frc971/control_loops/drivetrain/drivetrain_plotter.ts
+++ b/frc971/control_loops/drivetrain/drivetrain_plotter.ts
@@ -20,6 +20,8 @@
'/drivetrain', 'frc971.control_loops.drivetrain.Status');
const output = aosPlotter.addMessageSource(
'/drivetrain', 'frc971.control_loops.drivetrain.Output');
+ const gyroReading = aosPlotter.addMessageSource(
+ '/drivetrain', 'frc971.sensors.GyroReading');
const imu = aosPlotter.addRawMessageSource(
'/drivetrain', 'frc971.IMUValuesBatch',
new ImuMessageHandler(conn.getSchema('frc971.IMUValuesBatch')));
@@ -323,6 +325,7 @@
gyroY.setColor(GREEN);
const gyroZ = gyroPlot.addMessageLine(imu, ['gyro_z']);
gyroZ.setColor(BLUE);
+ gyroPlot.addMessageLine(gyroReading, ['velocity']).setColor(BLUE);
// IMU States
const imuStatePlot =
diff --git a/scouting/scouting_test.ts b/scouting/scouting_test.ts
index c3a1c6e..f400430 100644
--- a/scouting/scouting_test.ts
+++ b/scouting/scouting_test.ts
@@ -71,8 +71,8 @@
await expectReviewFieldToBe('Attempted to Climb', 'No');
// Validate Defense.
- await expectReviewFieldToBe('Defense Played On Rating', '3');
- await expectReviewFieldToBe('Defense Played Rating', '3');
+ await expectReviewFieldToBe('Defense Played On Rating', '0');
+ await expectReviewFieldToBe('Defense Played Rating', '0');
// TODO(phil): Submit data and make sure it made its way to the database
// correctly. Right now the /requests/submit/data_scouting endpoint is not
diff --git a/scouting/www/app.ts b/scouting/www/app.ts
index 285d306..79c1094 100644
--- a/scouting/www/app.ts
+++ b/scouting/www/app.ts
@@ -10,11 +10,26 @@
export class App {
tab: Tab = 'Entry';
+ constructor() {
+ window.addEventListener('beforeunload', (e) => {
+ // Based on https://developer.mozilla.org/en-US/docs/Web/API/WindowEventHandlers/onbeforeunload#example
+ // This combination ensures a dialog will be shown on most browsers.
+ e.preventDefault();
+ e.returnValue = '';
+ });
+ }
+
tabIs(tab: Tab) {
return this.tab == tab;
}
switchTabTo(tab: Tab) {
- this.tab = tab;
+ let shouldSwitch = true;
+ if (tab === 'ImportMatchList') {
+ shouldSwitch = window.confirm('Leave data scouting page?');
+ }
+ if (shouldSwitch) {
+ this.tab = tab;
+ }
}
}
diff --git a/scouting/www/entry/entry.component.ts b/scouting/www/entry/entry.component.ts
index af0ce97..3db5f72 100644
--- a/scouting/www/entry/entry.component.ts
+++ b/scouting/www/entry/entry.component.ts
@@ -10,7 +10,7 @@
import ErrorResponse = error_response.scouting.webserver.requests.ErrorResponse;
type Section = 'Team Selection'|'Auto'|'TeleOp'|'Climb'|'Defense'|'Review and Submit'|'Home'
-type Level = 'Low'|'Medium'|'High'|'Transversal'
+type Level = 'Failed'|'Low'|'Medium'|'High'|'Transversal'
@Component({
selector: 'app-entry',
@@ -27,8 +27,8 @@
teleUpperShotsMade: number = 0;
teleLowerShotsMade: number = 0;
teleShotsMissed: number = 0;
- defensePlayedOnScore: number = 3;
- defensePlayedScore: number = 3;
+ defensePlayedOnScore: number = 0;
+ defensePlayedScore: number = 0;
level: Level;
proper: boolean = false;
climbed: boolean = false;
@@ -38,6 +38,10 @@
this.proper = !this.proper;
}
+ setFailed() {
+ this.level = 'Failed';
+ }
+
setLow() {
this.level = 'Low';
}
@@ -54,22 +58,6 @@
this.level = 'Transversal';
}
- defensePlayedOnSlider(event) {
- this.defensePlayedOnScore = event.target.value;
- }
-
- defensePlayedSlider(event) {
- this.defensePlayedScore = event.target.value;
- }
-
- setClimbedTrue() {
- this.climbed = true;
- }
-
- setClimbedFalse() {
- this.climbed = false;
- }
-
nextSection() {
if (this.section === 'Team Selection') {
this.section = 'Auto';
diff --git a/scouting/www/entry/entry.ng.html b/scouting/www/entry/entry.ng.html
index ceaaac2..c5cf233 100644
--- a/scouting/www/entry/entry.ng.html
+++ b/scouting/www/entry/entry.ng.html
@@ -25,10 +25,12 @@
<h4>Image</h4>
<form>
<!--Choice for each ball location-->
- <input type="radio" name="balls" value="1" id="ball-1"><label for="ball-1">Ball 1</label>
- <input type="radio" name="balls" value="2" id="ball-2"><label for="ball-2">Ball 2</label><br>
- <input type="radio" name="balls" value="3" id="ball-3"><label for="ball-3">Ball 3</label>
- <input type="radio" name="balls" value="4" id="ball-4"><label for="ball-4">Ball 4</label>
+ <input type="checkbox" name="balls" value="1" id="ball-1"><label for="ball-1">Ball 1</label>
+ <input type="checkbox" name="balls" value="2" id="ball-2"><label for="ball-2">Ball 2</label><br>
+ <input type="checkbox" name="balls" value="3" id="ball-3"><label for="ball-3">Ball 3</label>
+ <input type="checkbox" name="balls" value="4" id="ball-4"><label for="ball-4">Ball 4</label><br>
+ <input type="checkbox" name="balls" value="5" id="ball-5"><label for="ball-5">Ball 5</label>
+ <input type="checkbox" name="balls" value="6" id="ball-6"><label for="ball-6">Ball 6</label>
</form>
</div>
<div class="row">
@@ -71,18 +73,28 @@
<div *ngSwitchCase="'Climb'" id="climb" class="container-fluid">
<div class="row">
<form>
- <input (click)="setClimbedFalse()" type="radio" name="climbing" id="continue"><label for="continue">Kept Shooting</label><br>
- <input (click)="setClimbedTrue()" type="radio" name="climbing" id="climbed"><label for="climbed">Attempted to Climb</label><br>
+ <input [(ngModel)]="climbed" type="radio" name="climbing" id="continue" [value]="false">
+ <label for="continue">Kept Shooting</label><br>
+ <input [(ngModel)]="climbed" type="radio" name="climbing" id="climbed" [value]="true">
+ <label for="climbed">Attempted to Climb</label><br>
</form>
</div>
<div *ngIf="climbed">
<h4>Bar Made</h4>
<form>
- <input (click)="setLow()" type="radio" name="level" id="low"><label for="low">Low</label><br>
- <input (click)="setMedium()" type="radio" name="level" id="medium"><label for="medium">Medium</label><br>
- <input (click)="setHigh()" type="radio" name="level" id="high"><label for="high">High</label><br>
- <input (click)="setTransversal()" type="radio" name="level" id="transversal"><label for="transversal">Transversal</label><br>
- <input (click)="toggleProper()" type="checkbox" id="proper"><label for="proper">~10 seconds to attempt next level?</label>
+ <input [ngModel]="level" (click)="setLow()" type="radio" name="level" id="low" value="Low">
+ <label for="low">Low</label><br>
+ <input [ngModel]="level" (click)="setMedium()" type="radio" name="level" id="medium" value="Medium">
+ <label for="medium">Medium</label><br>
+ <input [ngModel]="level" (click)="setHigh()" type="radio" name="level" id="high" value="High">
+ <label for="high">High</label><br>
+ <input [ngModel]="level" (click)="setTransversal()" type="radio" name="level" id="transversal" value="Transversal">
+ <label for="transversal">Transversal</label><br>
+ <input [ngModel]="level" (click)="setFailed()" type="radio" name="level" id="failed" value="Failed">
+ <label for="failed">Failed</label><br>
+
+ <input (click)="toggleProper()" type="checkbox" id="proper">
+ <label for="proper">~10 seconds to attempt next level?</label>
</form>
</div>
<div class="row">
@@ -104,7 +116,7 @@
</div>
<div class="col">
- <input type="range" min="1" max="5" value="3" (input)="defensePlayedOnSlider($event)">
+ <input type="range" min="0" max="5" value="0" [(ngModel)]="defensePlayedOnScore">
</div>
<div class="col">
@@ -123,7 +135,7 @@
</div>
<div class="col">
- <input type="range" min="1" max="5" value="3" (input)="defensePlayedSlider($event)">
+ <input type="range" min="0" max="5" value="0" [(ngModel)]="defensePlayedScore">
</div>
<div class="col">
diff --git a/scouting/www/import_match_list/import_match_list.component.ts b/scouting/www/import_match_list/import_match_list.component.ts
index b2b15e5..7ad4ab6 100644
--- a/scouting/www/import_match_list/import_match_list.component.ts
+++ b/scouting/www/import_match_list/import_match_list.component.ts
@@ -10,44 +10,48 @@
import ErrorResponse = error_response.scouting.webserver.requests.ErrorResponse;
@Component({
- selector: 'app-import-match-list',
- templateUrl: './import_match_list.ng.html',
- styleUrls: ['../common.css', './import_match_list.component.css']
+ selector: 'app-import-match-list',
+ templateUrl: './import_match_list.ng.html',
+ styleUrls: ['../common.css', './import_match_list.component.css']
})
export class ImportMatchListComponent {
- year: number = new Date().getFullYear();
- eventCode: string = '';
- progressMessage: string = '';
- errorMessage: string = '';
+ year: number = new Date().getFullYear();
+ eventCode: string = '';
+ progressMessage: string = '';
+ errorMessage: string = '';
- async importMatchList() {
- this.errorMessage = '';
-
- const builder = new flatbuffer_builder.Builder() as unknown as flatbuffers.Builder;
- const eventCode = builder.createString(this.eventCode);
- RefreshMatchList.startRefreshMatchList(builder);
- RefreshMatchList.addYear(builder, this.year);
- RefreshMatchList.addEventCode(builder, eventCode);
- builder.finish(RefreshMatchList.endRefreshMatchList(builder));
-
- this.progressMessage = 'Importing match list. Please be patient.';
-
- const buffer = builder.asUint8Array();
- const res = await fetch(
- '/requests/refresh_match_list', {method: 'POST', body: buffer});
-
- if (res.ok) {
- // We successfully submitted the data.
- this.progressMessage = 'Successfully imported match list.';
- } else {
- this.progressMessage = '';
- const resBuffer = await res.arrayBuffer();
- const fbBuffer = new ByteBuffer(new Uint8Array(resBuffer));
- const parsedResponse = ErrorResponse.getRootAsErrorResponse(
- fbBuffer as unknown as flatbuffers.ByteBuffer);
-
- const errorMessage = parsedResponse.errorMessage();
- this.errorMessage = `Received ${res.status} ${res.statusText}: "${errorMessage}"`;
- }
+ async importMatchList() {
+ if (!window.confirm('Actually import new matches?')) {
+ return;
}
+
+ this.errorMessage = '';
+
+ const builder = new flatbuffer_builder.Builder() as unknown as flatbuffers.Builder;
+ const eventCode = builder.createString(this.eventCode);
+ RefreshMatchList.startRefreshMatchList(builder);
+ RefreshMatchList.addYear(builder, this.year);
+ RefreshMatchList.addEventCode(builder, eventCode);
+ builder.finish(RefreshMatchList.endRefreshMatchList(builder));
+
+ this.progressMessage = 'Importing match list. Please be patient.';
+
+ const buffer = builder.asUint8Array();
+ const res = await fetch(
+ '/requests/refresh_match_list', {method: 'POST', body: buffer});
+
+ if (res.ok) {
+ // We successfully submitted the data.
+ this.progressMessage = 'Successfully imported match list.';
+ } else {
+ this.progressMessage = '';
+ const resBuffer = await res.arrayBuffer();
+ const fbBuffer = new ByteBuffer(new Uint8Array(resBuffer));
+ const parsedResponse = ErrorResponse.getRootAsErrorResponse(
+ fbBuffer as unknown as flatbuffers.ByteBuffer);
+
+ const errorMessage = parsedResponse.errorMessage();
+ this.errorMessage = `Received ${res.status} ${res.statusText}: "${errorMessage}"`;
+ }
+ }
}
diff --git a/y2022/BUILD b/y2022/BUILD
index 488026e..b16067d 100644
--- a/y2022/BUILD
+++ b/y2022/BUILD
@@ -39,6 +39,7 @@
"//y2022/vision:viewer",
"//y2022/localizer:imu_main",
"//y2022/localizer:localizer_main",
+ "//y2022/vision:image_decimator",
],
data = [
":aos_config",
diff --git a/y2022/constants.cc b/y2022/constants.cc
index 34ffae2..980735f 100644
--- a/y2022/constants.cc
+++ b/y2022/constants.cc
@@ -161,10 +161,10 @@
intake_back->subsystem_params.zeroing_constants
.measured_absolute_position = 0.280099007470002;
- turret->potentiometer_offset =
- -9.99970387166721 + 0.06415943 + 0.073290115367682;
+ turret->potentiometer_offset = -9.99970387166721 + 0.06415943 +
+ 0.073290115367682 - 0.0634440443622909;
turret->subsystem_params.zeroing_constants.measured_absolute_position =
- 0.511895084051468;
+ 0.568649928102931;
flipper_arm_left->potentiometer_offset = -6.4;
flipper_arm_right->potentiometer_offset = 5.56;
diff --git a/y2022/constants.h b/y2022/constants.h
index e3bc93f..ffe1283 100644
--- a/y2022/constants.h
+++ b/y2022/constants.h
@@ -113,7 +113,7 @@
.lower_hard = -6.0, // Back Hard
.upper_hard = 4.0, // Front Hard
.lower = -5.0, // Back Soft
- .upper = 3.7 // Front Soft
+ .upper = 3.3 // Front Soft
};
}
diff --git a/y2022/control_loops/superstructure/catapult/catapult.cc b/y2022/control_loops/superstructure/catapult/catapult.cc
index fda62ad..4a644e2 100644
--- a/y2022/control_loops/superstructure/catapult/catapult.cc
+++ b/y2022/control_loops/superstructure/catapult/catapult.cc
@@ -313,7 +313,7 @@
const flatbuffers::Offset<
frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>
Catapult::Iterate(const Goal *unsafe_goal, const Position *position,
- double *catapult_voltage, bool fire,
+ double battery_voltage, double *catapult_voltage, bool fire,
flatbuffers::FlatBufferBuilder *fbb) {
const frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal
*catapult_goal = unsafe_goal != nullptr && unsafe_goal->has_catapult()
@@ -373,8 +373,9 @@
} else {
// TODO(austin): Voltage error?
CHECK_NOTNULL(catapult_voltage);
- *catapult_voltage =
- std::max(0.0, std::min(12.0, *solution - 0.0 * next_X(2, 0)));
+ *catapult_voltage = std::max(
+ 0.0, std::min(12.0, (*solution - 0.0 * next_X(2, 0)) * 12.0 /
+ std::max(battery_voltage, 8.0)));
use_profile_ = false;
}
} else {
@@ -396,8 +397,10 @@
}
case CatapultState::RESETTING:
- if (catapult_.controller().R(1, 0) > 0.0) {
- catapult_.AdjustProfile(7.0, 1400.0);
+ if (catapult_.controller().R(1, 0) > 7.0) {
+ catapult_.AdjustProfile(7.0, 2000.0);
+ } else if (catapult_.controller().R(1, 0) > 0.0) {
+ catapult_.AdjustProfile(7.0, 1000.0);
} else {
catapult_state_ = CatapultState::PROFILE;
}
@@ -420,8 +423,9 @@
}
}
- catapult_.UpdateObserver(catapult_voltage != nullptr ? *catapult_voltage
- : 0.0);
+ catapult_.UpdateObserver(catapult_voltage != nullptr
+ ? (*catapult_voltage * battery_voltage / 12.0)
+ : 0.0);
return catapult_.MakeStatus(fbb);
}
diff --git a/y2022/control_loops/superstructure/catapult/catapult.h b/y2022/control_loops/superstructure/catapult/catapult.h
index ccd4b06..0606c0d 100644
--- a/y2022/control_loops/superstructure/catapult/catapult.h
+++ b/y2022/control_loops/superstructure/catapult/catapult.h
@@ -211,7 +211,7 @@
const flatbuffers::Offset<
frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>
Iterate(const Goal *unsafe_goal, const Position *position,
- double *catapult_voltage, bool fire,
+ double battery_voltage, double *catapult_voltage, bool fire,
flatbuffers::FlatBufferBuilder *fbb);
private:
diff --git a/y2022/control_loops/superstructure/superstructure.cc b/y2022/control_loops/superstructure/superstructure.cc
index e45917a..0b693ef 100644
--- a/y2022/control_loops/superstructure/superstructure.cc
+++ b/y2022/control_loops/superstructure/superstructure.cc
@@ -179,6 +179,44 @@
}
}
+ // Send the turret to the loading position if we have a ball in the intake, or
+ // are trying to intake one
+ double turret_loading_position = std::numeric_limits<double>::quiet_NaN();
+ if (state_ == SuperstructureState::TRANSFERRING &&
+ intake_state_ != IntakeState::NO_BALL) {
+ turret_loading_position = (intake_state_ == IntakeState::INTAKE_FRONT_BALL
+ ? constants::Values::kTurretFrontIntakePos()
+ : constants::Values::kTurretBackIntakePos());
+ } else if (state_ == SuperstructureState::IDLE && unsafe_goal != nullptr) {
+ if (unsafe_goal->roller_speed_front() > 0) {
+ turret_loading_position = constants::Values::kTurretFrontIntakePos();
+ } else if (unsafe_goal->roller_speed_back() > 0) {
+ turret_loading_position = constants::Values::kTurretBackIntakePos();
+ }
+ }
+
+ if (!std::isnan(turret_loading_position)) {
+ // Turn to the loading position as close to the current position as
+ // possible
+ // Strategy is copied from frc971/control_loops/aiming/aiming.cc
+ turret_loading_position =
+ turret_.estimated_position() +
+ aos::math::NormalizeAngle(turret_loading_position -
+ turret_.estimated_position());
+ // if out of range, reset back to within +/- pi of zero.
+ if (turret_loading_position > constants::Values::kTurretRange().upper ||
+ turret_loading_position < constants::Values::kTurretRange().lower) {
+ turret_loading_position =
+ aos::math::NormalizeAngle(turret_loading_position);
+ }
+
+ turret_goal_buffer.Finish(
+ frc971::control_loops::
+ CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *turret_goal_buffer.fbb(), turret_loading_position));
+ turret_goal = &turret_goal_buffer.message();
+ }
+
switch (state_) {
case SuperstructureState::IDLE: {
if (is_spitting) {
@@ -203,31 +241,6 @@
break;
}
- double turret_loading_position =
- (intake_state_ == IntakeState::INTAKE_FRONT_BALL
- ? constants::Values::kTurretFrontIntakePos()
- : constants::Values::kTurretBackIntakePos());
-
- // Turn to the loading position as close to the current position as
- // possible
- // Strategy is copied from frc971/control_loops/aiming/aiming.cc
- turret_loading_position =
- turret_.estimated_position() +
- aos::math::NormalizeAngle(turret_loading_position -
- turret_.estimated_position());
- // if out of range, reset back to within +/- pi of zero.
- if (turret_loading_position > constants::Values::kTurretRange().upper ||
- turret_loading_position < constants::Values::kTurretRange().lower) {
- turret_loading_position =
- aos::math::NormalizeAngle(turret_loading_position);
- }
-
- turret_goal_buffer.Finish(
- frc971::control_loops::
- CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
- *turret_goal_buffer.fbb(), turret_loading_position));
- turret_goal = &turret_goal_buffer.message();
-
const bool turret_near_goal =
std::abs(turret_.estimated_position() - turret_loading_position) <
kTurretGoalThreshold;
@@ -381,12 +394,12 @@
// Disable the catapult if we want to restart to prevent damage with
// flippers
const flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
- catapult_status_offset =
- catapult_.Iterate(unsafe_goal, position,
- output != nullptr && !catapult_.estopped()
- ? &(output_struct.catapult_voltage)
- : nullptr,
- fire_, status->fbb());
+ catapult_status_offset = catapult_.Iterate(
+ unsafe_goal, position, robot_state().voltage_battery(),
+ output != nullptr && !catapult_.estopped()
+ ? &(output_struct.catapult_voltage)
+ : nullptr,
+ fire_, status->fbb());
const flatbuffers::Offset<RelativeEncoderProfiledJointStatus>
climber_status_offset = climber_.Iterate(
diff --git a/y2022/control_loops/superstructure/superstructure_goal.fbs b/y2022/control_loops/superstructure/superstructure_goal.fbs
index 8862d22..7816563 100644
--- a/y2022/control_loops/superstructure/superstructure_goal.fbs
+++ b/y2022/control_loops/superstructure/superstructure_goal.fbs
@@ -26,8 +26,11 @@
intake_back:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal (id: 2);
// Positive is rollers intaking.
+ // When spinning the rollers, the turret will be moved to that side,
+ // so both shouldn't be positive at the same time.
roller_speed_front:double (id: 3);
roller_speed_back:double (id: 4);
+
transfer_roller_speed_front:double (id: 5);
transfer_roller_speed_back:double (id: 12);
diff --git a/y2022/control_loops/superstructure/superstructure_lib_test.cc b/y2022/control_loops/superstructure/superstructure_lib_test.cc
index 66a615e..73ceee7 100644
--- a/y2022/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2022/control_loops/superstructure/superstructure_lib_test.cc
@@ -776,6 +776,7 @@
ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_front(), 12.0);
EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_back(), 12.0);
+
EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage_front(),
0.0);
EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage_back(),
@@ -783,8 +784,10 @@
EXPECT_EQ(superstructure_status_fetcher_->state(), SuperstructureState::IDLE);
EXPECT_EQ(superstructure_status_fetcher_->intake_state(),
IntakeState::NO_BALL);
- EXPECT_NEAR(superstructure_status_fetcher_->turret()->position(), kTurretGoal,
- 0.001);
+ // Since we spun the front rollers, the turret should be trying to intake from
+ // there
+ EXPECT_NEAR(superstructure_status_fetcher_->turret()->position(),
+ constants::Values::kTurretFrontIntakePos(), 0.001);
EXPECT_EQ(superstructure_status_fetcher_->shot_count(), 0);
superstructure_plant_.set_intake_beambreak_front(true);
diff --git a/y2022/control_loops/superstructure/turret/aiming.cc b/y2022/control_loops/superstructure/turret/aiming.cc
index 4c0309c..2a600d8 100644
--- a/y2022/control_loops/superstructure/turret/aiming.cc
+++ b/y2022/control_loops/superstructure/turret/aiming.cc
@@ -22,13 +22,22 @@
// straight out the back of the robot.
constexpr double kTurretZeroOffset = M_PI;
+constexpr double kMaxProfiledVelocity = 10.0;
+constexpr double kMaxProfiledAccel = 20.0;
+
flatbuffers::DetachedBuffer MakePrefilledGoal() {
flatbuffers::FlatBufferBuilder fbb;
fbb.ForceDefaults(true);
+ frc971::ProfileParameters::Builder profile_builder(fbb);
+ profile_builder.add_max_velocity(kMaxProfiledVelocity);
+ profile_builder.add_max_acceleration(kMaxProfiledAccel);
+ const flatbuffers::Offset<frc971::ProfileParameters> profile_offset =
+ profile_builder.Finish();
Aimer::Goal::Builder builder(fbb);
builder.add_unsafe_goal(0);
builder.add_goal_velocity(0);
- builder.add_ignore_profile(true);
+ builder.add_ignore_profile(false);
+ builder.add_profile_params(profile_offset);
fbb.Finish(builder.Finish());
return fbb.Release();
}
diff --git a/y2022/control_loops/superstructure/turret_plotter.ts b/y2022/control_loops/superstructure/turret_plotter.ts
index 10fc10e..6753880 100644
--- a/y2022/control_loops/superstructure/turret_plotter.ts
+++ b/y2022/control_loops/superstructure/turret_plotter.ts
@@ -29,6 +29,12 @@
positionPlot.addMessageLine(status, ['turret', 'goal_position']).setColor(RED).setPointSize(4.0);
positionPlot.addMessageLine(status, ['turret', 'goal_velocity']).setColor(ORANGE).setPointSize(4.0);
positionPlot.addMessageLine(status, ['turret', 'estimator_state', 'position']).setColor(CYAN).setPointSize(1.0);
+ positionPlot.addMessageLine(status, ['aimer', 'turret_velocity'])
+ .setColor(WHITE)
+ .setPointSize(1.0);
+ positionPlot.addMessageLine(status, ['aimer', 'turret_position'])
+ .setColor(BROWN)
+ .setPointSize(1.0);
const voltagePlot =
aosPlotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
diff --git a/y2022/joystick_reader.cc b/y2022/joystick_reader.cc
index 64048cf..8423f47 100644
--- a/y2022/joystick_reader.cc
+++ b/y2022/joystick_reader.cc
@@ -61,8 +61,8 @@
const ButtonLocation kIntakeBackOut(4, 9);
const ButtonLocation kSpit(3, 3);
-const ButtonLocation kRedLocalizerReset(4, 13);
-const ButtonLocation kBlueLocalizerReset(4, 14);
+const ButtonLocation kRedLocalizerReset(4, 14);
+const ButtonLocation kBlueLocalizerReset(4, 13);
const ButtonLocation kLocalizerReset(3, 8);
#endif
@@ -84,16 +84,18 @@
setpoint_fetcher_(
event_loop->MakeFetcher<Setpoint>("/superstructure")) {}
+ // Localizer reset positions are with front of robot pressed up against driver
+ // station in the middle of the field side-to-side.
void BlueResetLocalizer() {
auto builder = localizer_control_sender_.MakeBuilder();
frc971::control_loops::drivetrain::LocalizerControl::Builder
localizer_control_builder = builder.MakeBuilder<
frc971::control_loops::drivetrain::LocalizerControl>();
- localizer_control_builder.add_x(7.4);
- localizer_control_builder.add_y(-1.7);
+ localizer_control_builder.add_x(-7.9);
+ localizer_control_builder.add_y(0.0);
localizer_control_builder.add_theta_uncertainty(10.0);
- localizer_control_builder.add_theta(0.0);
+ localizer_control_builder.add_theta(M_PI);
localizer_control_builder.add_keep_current_theta(false);
if (builder.Send(localizer_control_builder.Finish()) !=
aos::RawSender::Error::kOk) {
@@ -107,10 +109,10 @@
frc971::control_loops::drivetrain::LocalizerControl::Builder
localizer_control_builder = builder.MakeBuilder<
frc971::control_loops::drivetrain::LocalizerControl>();
- localizer_control_builder.add_x(-7.9);
- localizer_control_builder.add_y(0.7);
+ localizer_control_builder.add_x(7.9);
+ localizer_control_builder.add_y(0.0);
localizer_control_builder.add_theta_uncertainty(10.0);
- localizer_control_builder.add_theta(M_PI);
+ localizer_control_builder.add_theta(0.0);
localizer_control_builder.add_keep_current_theta(false);
if (builder.Send(localizer_control_builder.Finish()) !=
aos::RawSender::Error::kOk) {
diff --git a/y2022/localizer/imu.cc b/y2022/localizer/imu.cc
index fed9ceb..2310e99 100644
--- a/y2022/localizer/imu.cc
+++ b/y2022/localizer/imu.cc
@@ -124,9 +124,9 @@
// extra data from the pico
imu_builder.add_pico_timestamp_us(pico_timestamp);
imu_builder.add_left_encoder(
- constants::Values::DrivetrainEncoderToMeters(encoder1_count));
+ -constants::Values::DrivetrainEncoderToMeters(encoder2_count));
imu_builder.add_right_encoder(
- constants::Values::DrivetrainEncoderToMeters(encoder2_count));
+ constants::Values::DrivetrainEncoderToMeters(encoder1_count));
imu_builder.add_previous_reading_diag_stat(diag_stat_offset);
}
diff --git a/y2022/localizer/localizer.cc b/y2022/localizer/localizer.cc
index e7b6421..8f9ffa1 100644
--- a/y2022/localizer/localizer.cc
+++ b/y2022/localizer/localizer.cc
@@ -1,9 +1,9 @@
#include "y2022/localizer/localizer.h"
+#include "aos/json_to_flatbuffer.h"
#include "frc971/control_loops/c2d.h"
#include "frc971/wpilib/imu_batch_generated.h"
#include "y2022/constants.h"
-#include "aos/json_to_flatbuffer.h"
namespace frc971::controls {
@@ -17,7 +17,7 @@
constexpr double kVisionTargetY = 0.0;
// Minimum confidence to require to use a target match.
-constexpr double kMinTargetEstimateConfidence = 0.2;
+constexpr double kMinTargetEstimateConfidence = 0.75;
template <int N>
Eigen::Matrix<double, N, 1> MakeState(std::vector<double> values) {
@@ -39,8 +39,9 @@
.coefficients()),
down_estimator_(dt_config) {
statistics_.rejection_counts.fill(0);
- CHECK_EQ(branches_.capacity(), static_cast<size_t>(std::chrono::seconds(1) /
- kNominalDt / kBranchPeriod));
+ CHECK_EQ(branches_.capacity(),
+ static_cast<size_t>(std::chrono::seconds(1) / kNominalDt /
+ kBranchPeriod));
if (dt_config_.is_simulated) {
down_estimator_.assume_perfect_gravity();
}
@@ -170,7 +171,7 @@
const ModelBasedLocalizer::ModelState &state) const {
const double robot_speed =
(state(kLeftVelocity) + state(kRightVelocity)) / 2.0;
- const double lat_speed = (AModel(state) * state)(kTheta) * long_offset_;
+ const double lat_speed = (AModel(state) * state)(kTheta)*long_offset_;
const double velocity_x = std::cos(state(kTheta)) * robot_speed -
std::sin(state(kTheta)) * lat_speed;
const double velocity_y = std::sin(state(kTheta)) * robot_speed +
@@ -438,7 +439,7 @@
VLOG(2) << "Model state " << current_state_.model_state.transpose();
VLOG(2) << "Accel state " << current_state_.accel_state.transpose();
VLOG(2) << "Accel state for model "
- << AccelStateForModelState(current_state_.model_state).transpose();
+ << AccelStateForModelState(current_state_.model_state).transpose();
VLOG(2) << "Input acce " << accel.transpose();
VLOG(2) << "Input gyro " << gyro.transpose();
VLOG(2) << "Input voltage " << voltage.transpose();
@@ -487,7 +488,7 @@
// Node names of the pis to listen for cameras from.
const std::array<std::string_view, 4> kPisToUse{"pi1", "pi2", "pi3", "pi4"};
-}
+} // namespace
const Eigen::Matrix<double, 4, 4> ModelBasedLocalizer::CameraTransform(
const OldPosition &state,
@@ -647,9 +648,9 @@
// And now we have to correct *everything* on all the branches:
for (CombinedState &state : branches_) {
state.model_state += K_model * (measured_robot_position.value() -
- H_model * state.model_state);
+ H_model * state.model_state);
state.accel_state += K_accel * (measured_robot_position.value() -
- H_accel * state.accel_state);
+ H_accel * state.accel_state);
}
current_state_.model_state +=
K_model *
@@ -678,6 +679,7 @@
aos::math::NormalizeAngle(camera_yaw + target->angle_to_target());
debug.accepted = true;
debug.image_age_sec = aos::time::DurationInSeconds(t_ - sample_time);
+ CHECK_LT(image_debugs_.size(), kDebugBufferSize);
image_debugs_.push_back(debug);
}
@@ -692,7 +694,7 @@
void ModelBasedLocalizer::HandleReset(aos::monotonic_clock::time_point now,
const Eigen::Vector3d &xytheta) {
branches_.Reset();
- t_ = now;
+ t_ = now;
using_model_ = true;
current_state_.model_state << xytheta(0), xytheta(1), xytheta(2),
current_state_.model_state(kLeftEncoder), 0.0, 0.0,
@@ -754,7 +756,7 @@
const CombinedState &state = current_state_;
const flatbuffers::Offset<ModelBasedState> model_state_offset =
- BuildModelState(fbb, state.model_state);
+ BuildModelState(fbb, state.model_state);
const flatbuffers::Offset<AccelBasedState> accel_state_offset =
BuildAccelState(fbb, state.accel_state);
@@ -799,7 +801,7 @@
aos::SizedArray<flatbuffers::Offset<TargetEstimateDebug>, kDebugBufferSize>
debug_offsets;
- for (const TargetEstimateDebugT& debug : image_debugs_) {
+ for (const TargetEstimateDebugT &debug : image_debugs_) {
debug_offsets.push_back(PackTargetEstimateDebug(debug, fbb));
}
@@ -822,6 +824,7 @@
TargetEstimateDebugT debug;
debug.accepted = false;
debug.rejection_reason = reason;
+ CHECK_LT(image_debugs_.size(), kDebugBufferSize);
image_debugs_.push_back(debug);
}
@@ -847,7 +850,7 @@
static double DrivetrainWrapPeriod() {
return y2022::constants::Values::DrivetrainEncoderToMeters(1 << 16);
}
-}
+} // namespace
EventLoopLocalizer::EventLoopLocalizer(
aos::EventLoop *event_loop,
@@ -864,6 +867,10 @@
clock_offset_fetcher_(
event_loop_->MakeFetcher<aos::message_bridge::ServerStatistics>(
"/aos")),
+ superstructure_fetcher_(
+ event_loop_
+ ->MakeFetcher<y2022::control_loops::superstructure::Status>(
+ "/superstructure")),
left_encoder_(-DrivetrainWrapPeriod() / 2.0, DrivetrainWrapPeriod()),
right_encoder_(-DrivetrainWrapPeriod() / 2.0, DrivetrainWrapPeriod()) {
event_loop_->MakeWatcher(
@@ -876,45 +883,70 @@
model_based_.HandleReset(event_loop_->monotonic_now(),
{control.x(), control.y(), theta});
});
- event_loop_->MakeWatcher(
- "/superstructure",
- [this](const y2022::control_loops::superstructure::Status &status) {
- if (!status.has_turret()) {
- return;
- }
- CHECK(status.has_turret());
- model_based_.HandleTurret(event_loop_->context().monotonic_event_time,
- status.turret()->position(),
- status.turret()->velocity());
- });
+ aos::TimerHandler *superstructure_timer = event_loop_->AddTimer([this]() {
+ if (superstructure_fetcher_.Fetch()) {
+ const y2022::control_loops::superstructure::Status &status =
+ *superstructure_fetcher_.get();
+ if (!status.has_turret()) {
+ return;
+ }
+ CHECK(status.has_turret());
+ model_based_.HandleTurret(
+ superstructure_fetcher_.context().monotonic_event_time,
+ status.turret()->position(), status.turret()->velocity());
+ }
+ });
+ event_loop_->OnRun([this, superstructure_timer]() {
+ superstructure_timer->Setup(event_loop_->monotonic_now(),
+ std::chrono::milliseconds(20));
+ });
- for (size_t camera_index = 0; camera_index < kPisToUse.size(); ++camera_index) {
- event_loop_->MakeWatcher(
- absl::StrCat("/", kPisToUse[camera_index], "/camera"),
- [this, camera_index](const y2022::vision::TargetEstimate &target) {
- const std::optional<aos::monotonic_clock::duration> monotonic_offset =
- ClockOffset(kPisToUse[camera_index]);
- if (!monotonic_offset.has_value()) {
- return;
- }
- // TODO(james): Get timestamp from message contents.
- aos::monotonic_clock::time_point capture_time(
- event_loop_->context().monotonic_remote_time - monotonic_offset.value());
- if (capture_time > event_loop_->context().monotonic_event_time) {
- model_based_.TallyRejection(RejectionReason::IMAGE_FROM_FUTURE);
- return;
- }
- model_based_.HandleImageMatch(capture_time, &target, camera_index);
- if (model_based_.NumQueuedImageDebugs() ==
- ModelBasedLocalizer::kDebugBufferSize ||
- (last_visualization_send_ + kMinVisualizationPeriod <
- event_loop_->monotonic_now())) {
- auto builder = visualization_sender_.MakeBuilder();
- visualization_sender_.CheckOk(
- builder.Send(model_based_.PopulateVisualization(builder.fbb())));
- }
- });
+ for (size_t camera_index = 0; camera_index < kPisToUse.size();
+ ++camera_index) {
+ CHECK_LT(camera_index, target_estimate_fetchers_.size());
+ target_estimate_fetchers_[camera_index] =
+ event_loop_->MakeFetcher<y2022::vision::TargetEstimate>(
+ absl::StrCat("/", kPisToUse[camera_index], "/camera"));
}
+ aos::TimerHandler *estimate_timer = event_loop_->AddTimer([this]() {
+ for (size_t camera_index = 0; camera_index < kPisToUse.size();
+ ++camera_index) {
+ if (model_based_.NumQueuedImageDebugs() ==
+ ModelBasedLocalizer::kDebugBufferSize ||
+ (last_visualization_send_ + kMinVisualizationPeriod <
+ event_loop_->monotonic_now())) {
+ auto builder = visualization_sender_.MakeBuilder();
+ visualization_sender_.CheckOk(
+ builder.Send(model_based_.PopulateVisualization(builder.fbb())));
+ }
+ if (target_estimate_fetchers_[camera_index].Fetch()) {
+ const std::optional<aos::monotonic_clock::duration> monotonic_offset =
+ ClockOffset(kPisToUse[camera_index]);
+ if (!monotonic_offset.has_value()) {
+ continue;
+ }
+ // TODO(james): Get timestamp from message contents.
+ aos::monotonic_clock::time_point capture_time(
+ target_estimate_fetchers_[camera_index]
+ .context()
+ .monotonic_remote_time -
+ monotonic_offset.value());
+ if (capture_time > target_estimate_fetchers_[camera_index]
+ .context()
+ .monotonic_event_time) {
+ model_based_.TallyRejection(RejectionReason::IMAGE_FROM_FUTURE);
+ continue;
+ }
+ model_based_.HandleImageMatch(
+ capture_time, target_estimate_fetchers_[camera_index].get(),
+ camera_index);
+ }
+ }
+ });
+ event_loop_->OnRun([this, estimate_timer]() {
+ estimate_timer->Setup(event_loop_->monotonic_now(),
+ std::chrono::milliseconds(100));
+ });
event_loop_->MakeWatcher(
"/localizer", [this](const frc971::IMUValuesBatch &values) {
CHECK(values.has_readings());
@@ -947,8 +979,8 @@
std::chrono::milliseconds(10) <
event_loop_->context().monotonic_event_time);
model_based_.HandleImu(
- sample_timestamp,
- zeroer_.ZeroedGyro(), zeroer_.ZeroedAccel(), encoders,
+ sample_timestamp, zeroer_.ZeroedGyro(), zeroer_.ZeroedAccel(),
+ encoders,
disabled ? Eigen::Vector2d::Zero()
: Eigen::Vector2d{output_fetcher_->left_voltage(),
output_fetcher_->right_voltage()});
diff --git a/y2022/localizer/localizer.h b/y2022/localizer/localizer.h
index 4b7eff0..c19bf05 100644
--- a/y2022/localizer/localizer.h
+++ b/y2022/localizer/localizer.h
@@ -322,6 +322,10 @@
aos::Sender<LocalizerVisualization> visualization_sender_;
aos::Fetcher<frc971::control_loops::drivetrain::Output> output_fetcher_;
aos::Fetcher<aos::message_bridge::ServerStatistics> clock_offset_fetcher_;
+ std::array<aos::Fetcher<y2022::vision::TargetEstimate>, 4>
+ target_estimate_fetchers_;
+ aos::Fetcher<y2022::control_loops::superstructure::Status>
+ superstructure_fetcher_;
zeroing::ImuZeroer zeroer_;
aos::monotonic_clock::time_point last_output_send_ =
aos::monotonic_clock::min_time;
diff --git a/y2022/localizer/localizer_test.cc b/y2022/localizer/localizer_test.cc
index 791adf3..bf75446 100644
--- a/y2022/localizer/localizer_test.cc
+++ b/y2022/localizer/localizer_test.cc
@@ -849,19 +849,19 @@
drivetrain_plant_.set_accel_sin_magnitude(0.01);
drivetrain_plant_.mutable_state()->x() = 2.0;
drivetrain_plant_.mutable_state()->y() = 2.0;
- SendLocalizerControl(5.0, 3.0, 0.0);
+ SendLocalizerControl(6.0, 3.0, 0.0);
event_loop_factory_.RunFor(std::chrono::seconds(1));
CHECK(output_fetcher_.Fetch());
CHECK(status_fetcher_.Fetch());
ASSERT_FALSE(status_fetcher_->model_based()->using_model());
- EXPECT_FALSE(VerifyEstimatorAccurate(0.5));
+ EXPECT_FALSE(VerifyEstimatorAccurate(3.0));
send_targets_ = true;
event_loop_factory_.RunFor(std::chrono::seconds(4));
CHECK(status_fetcher_.Fetch());
ASSERT_FALSE(status_fetcher_->model_based()->using_model());
- EXPECT_TRUE(VerifyEstimatorAccurate(0.5));
+ EXPECT_TRUE(VerifyEstimatorAccurate(3.0));
// y should be noticeably more accurate than x, since we are just driving
// straight.
ASSERT_NEAR(drivetrain_plant_.state()(1), status_fetcher_->model_based()->y(), 0.1);
diff --git a/y2022/setpoint_setter.cc b/y2022/setpoint_setter.cc
index 7dbd4b8..b588636 100644
--- a/y2022/setpoint_setter.cc
+++ b/y2022/setpoint_setter.cc
@@ -8,13 +8,15 @@
DEFINE_double(catapult_velocity, 18.0, "Catapult shot velocity");
DEFINE_double(turret, 0.0, "Turret setpoint");
+DEFINE_string(config, "aos_config.json", "Path to the config file to use.");
+
using y2022::input::joysticks::Setpoint;
int main(int argc, char **argv) {
aos::InitGoogle(&argc, &argv);
aos::FlatbufferDetachedBuffer<aos::Configuration> config =
- aos::configuration::ReadConfig("config.json");
+ aos::configuration::ReadConfig(FLAGS_config);
aos::ShmEventLoop event_loop(&config.message());
diff --git a/y2022/vision/BUILD b/y2022/vision/BUILD
index 4ca8561..bede03a 100644
--- a/y2022/vision/BUILD
+++ b/y2022/vision/BUILD
@@ -252,3 +252,15 @@
"//y2022/control_loops/superstructure:superstructure_status_fbs",
],
)
+
+cc_binary(
+ name = "image_decimator",
+ srcs = ["image_decimator.cc"],
+ visibility = ["//y2022:__subpackages__"],
+ deps = [
+ "//aos:flatbuffers",
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ "//frc971/vision:vision_fbs",
+ ],
+)
diff --git a/y2022/vision/image_decimator.cc b/y2022/vision/image_decimator.cc
new file mode 100644
index 0000000..5fda423
--- /dev/null
+++ b/y2022/vision/image_decimator.cc
@@ -0,0 +1,52 @@
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "aos/flatbuffers.h"
+#include "frc971/vision/vision_generated.h"
+
+DEFINE_string(config, "aos_config.json", "Path to the config file to use.");
+
+namespace frc971::vision {
+// Reads images from /camera and resends them in /camera/decimated at a fixed
+// rate (1 Hz, in this case).
+class ImageDecimator {
+ public:
+ ImageDecimator(aos::EventLoop *event_loop)
+ : slow_image_sender_(
+ event_loop->MakeSender<CameraImage>("/camera/decimated")),
+ image_fetcher_(event_loop->MakeFetcher<CameraImage>("/camera")) {
+ aos::TimerHandler *timer =
+ event_loop->AddTimer(
+ [this]() {
+ if (image_fetcher_.Fetch()) {
+ const aos::FlatbufferSpan<CameraImage> image(
+ {reinterpret_cast<const uint8_t *>(
+ image_fetcher_.context().data),
+ image_fetcher_.context().size});
+ slow_image_sender_.CheckOk(slow_image_sender_.Send(image));
+ }
+ });
+ event_loop->OnRun([event_loop, timer]() {
+ timer->Setup(event_loop->monotonic_now(),
+ std::chrono::milliseconds(1000));
+ });
+ }
+
+ private:
+ aos::Sender<CameraImage> slow_image_sender_;
+ aos::Fetcher<CameraImage> image_fetcher_;
+};
+}
+
+int main(int argc, char *argv[]) {
+ aos::InitGoogle(&argc, &argv);
+
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig(FLAGS_config);
+
+ aos::ShmEventLoop event_loop(&config.message());
+ frc971::vision::ImageDecimator decimator(&event_loop);
+
+ event_loop.Run();
+
+ return 0;
+}
diff --git a/y2022/wpilib_interface.cc b/y2022/wpilib_interface.cc
index a1b1202..69883b4 100644
--- a/y2022/wpilib_interface.cc
+++ b/y2022/wpilib_interface.cc
@@ -701,13 +701,6 @@
AddLoop(&output_event_loop);
- // Thread 5
- ::aos::ShmEventLoop can_sensor_reader_event_loop(&config.message());
- CANSensorReader can_sensor_reader(&can_sensor_reader_event_loop);
- can_sensor_reader.set_flipper_arms_falcon(
- superstructure_writer.flipper_arms_falcon());
- AddLoop(&can_sensor_reader_event_loop);
-
RunLoops();
}
};
diff --git a/y2022/y2022_imu.json b/y2022/y2022_imu.json
index 3d97c3c..c5c7904 100644
--- a/y2022/y2022_imu.json
+++ b/y2022/y2022_imu.json
@@ -223,22 +223,7 @@
"type": "frc971.controls.LocalizerStatus",
"source_node": "imu",
"frequency": 2200,
- "max_size": 2000,
- "logger": "LOCAL_AND_REMOTE_LOGGER",
- "logger_nodes": [
- "logger"
- ],
- "destination_nodes": [
- {
- "name": "logger",
- "priority": 5,
- "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
- "timestamp_logger_nodes": [
- "imu"
- ],
- "time_to_live": 5000000
- }
- ]
+ "max_size": 2000
},
{
"name": "/imu/aos/remote_timestamps/logger/localizer/frc971-controls-LocalizerStatus",
@@ -395,6 +380,7 @@
{
"name": "localizer_logger",
"executable_name": "logger_main",
+ "autostart": false,
"args": ["--snappy_compress"],
"nodes": [
"imu"
diff --git a/y2022/y2022_logger.json b/y2022/y2022_logger.json
index d6fa4a4..b72abeb 100644
--- a/y2022/y2022_logger.json
+++ b/y2022/y2022_logger.json
@@ -322,7 +322,7 @@
"max_size": 200
},
{
- "name": "/pi1/camera",
+ "name": "/pi1/camera/decimated",
"type": "frc971.vision.CameraImage",
"source_node": "pi1",
"logger": "LOCAL_AND_REMOTE_LOGGER",
@@ -338,7 +338,7 @@
]
},
{
- "name": "/pi2/camera",
+ "name": "/pi2/camera/decimated",
"type": "frc971.vision.CameraImage",
"source_node": "pi2",
"logger": "LOCAL_AND_REMOTE_LOGGER",
@@ -349,12 +349,12 @@
{
"name": "logger",
"priority": 3,
- "time_to_live": 5000000
+ "time_to_live": 500000000
}
]
},
{
- "name": "/pi3/camera",
+ "name": "/pi3/camera/decimated",
"type": "frc971.vision.CameraImage",
"source_node": "pi3",
"logger": "LOCAL_AND_REMOTE_LOGGER",
@@ -370,7 +370,7 @@
]
},
{
- "name": "/pi4/camera",
+ "name": "/pi4/camera/decimated",
"type": "frc971.vision.CameraImage",
"source_node": "pi4",
"logger": "LOCAL_AND_REMOTE_LOGGER",
@@ -399,16 +399,17 @@
],
"applications": [
{
- "name": "message_bridge_client",
+ "name": "logger_message_bridge_client",
"executable_name": "message_bridge_client",
- "args": ["--rmem=8388608"],
+ "args": ["--rmem=8388608", "--rt_priority=16"],
"nodes": [
"logger"
]
},
{
- "name": "message_bridge_server",
+ "name": "logger_message_bridge_server",
"executable_name": "message_bridge_server",
+ "args": ["--rt_priority=16"],
"nodes": [
"logger"
]
diff --git a/y2022/y2022_pi_template.json b/y2022/y2022_pi_template.json
index 7f49e41..27bc251 100644
--- a/y2022/y2022_pi_template.json
+++ b/y2022/y2022_pi_template.json
@@ -146,6 +146,14 @@
"num_senders": 18
},
{
+ "name": "/pi{{ NUM }}/camera/decimated",
+ "type": "frc971.vision.CameraImage",
+ "source_node": "pi{{ NUM }}",
+ "frequency": 2,
+ "max_size": 620000,
+ "num_senders": 18
+ },
+ {
"name": "/pi{{ NUM }}/camera",
"type": "frc971.vision.calibration.CalibrationData",
"source_node": "pi{{ NUM }}",
@@ -328,6 +336,13 @@
"nodes": [
"pi{{ NUM }}"
]
+ },
+ {
+ "name": "image_decimator",
+ "executable_name": "image_decimator",
+ "nodes": [
+ "pi{{ NUM }}"
+ ]
}
],
"maps": [
diff --git a/y2022/y2022_roborio.json b/y2022/y2022_roborio.json
index b16f0cb..e342094 100644
--- a/y2022/y2022_roborio.json
+++ b/y2022/y2022_roborio.json
@@ -211,13 +211,19 @@
"num_senders": 2,
"logger": "LOCAL_AND_REMOTE_LOGGER",
"logger_nodes": [
- "imu"
+ "imu",
+ "logger"
],
"destination_nodes": [
{
"name": "imu",
"priority": 5,
- "time_to_live": 5000000
+ "time_to_live": 50000000
+ },
+ {
+ "name": "logger",
+ "priority": 5,
+ "time_to_live": 50000000
}
]
},