void UpdateDashboard() {
float r = 0.00001 * i;
SmartDashboard::PutNumber("State", currentState + r);
SmartDashboard::PutNumber("PID Turn Error",
turnController->GetError() + r);
SmartDashboard::PutNumber("PID Target",
turnController->GetSetpoint() + r);
// SmartDashboard::PutBoolean("Straight", straight);
SmartDashboard::PutData("test", turnController);
SmartDashboard::PutNumber("Yaw:", ahrs->GetYaw() + r);
SmartDashboard::PutNumber("Roll:", ahrs->GetRoll() + r);
SmartDashboard::PutNumber("Pitch", ahrs->GetPitch() + r);
SmartDashboard::PutNumber("Scissor 1", pdp->GetCurrent(1) + r);
SmartDashboard::PutNumber("Scissor 2", pdp->GetCurrent(2) + r);
SmartDashboard::PutNumber("Left Drive 1", pdp->GetCurrent(12) + r);
SmartDashboard::PutNumber("Left Drive 2", pdp->GetCurrent(13) + r);
SmartDashboard::PutNumber("Right Drive 1", pdp->GetCurrent(14) + r);
SmartDashboard::PutNumber("Right Drive 2", pdp->GetCurrent(15) + r);
SmartDashboard::PutNumber("Constant Lift", constantLift);
SmartDashboard::PutNumber("Rotate Rate", rotateRate + r);
i = (i + 1) % 2;
printf("2.1");
// .PutLong("test1.2", 1337);
printf("2.2");
// mqServer.PutDouble("test",DriverStation::GetInstance().GetMatchTime());
printf("2.3");
// mqServer.PutString("test1.1","YOLO_SWAGINS");
printf("2.4");
// SmartDashboard::PutString("test1.2", mqServer.GetString("test1.1"));
// SmartDashboard::PutNumber("test1", mqServer.GetDouble("test"));
// SmartDashboard::PutNumber("test1.3", mqServer.GetLong("test1.2"));
// SmartDashboard::PutNumber("test2", DriverStation::GetInstance().GetMatchTime());
}
void AutonomousAdjustableStraight() {
switch (currentState) {
case 1:
timer->Reset();
timer->Start();
turnController->Reset();
turnController->SetSetpoint(ahrs->GetYaw());
turnController->Enable();
currentState = 2;
break;
case 2:
intakeLever->Set(autoIntakeSpeed);
if (timer->Get() >= 1) {
intakeLever->Set(0);
currentState = 3;
timer->Reset();
timer->Start();
}
break;
case 3:
drive->TankDrive(autoSpeed, autoSpeed);
intakeLever->Set(-0.1);
if (timer->Get() >= autoLength) {
intakeLever->Set(0.0);
drive->TankDrive(0.0, 0.0);
currentState = 4;
timer->Reset();
timer->Start();
}
break;
case 4:
intake->Set(0.5);
shooter->Set(-0.5);
if (timer->Get() >= 2) {
currentState = 5;
}
break;
case 5:
intake->Set(0.0);
shooter->Set(0.0);
drive->TankDrive(0.0, 0.0);
break;
}
}
void AutonomousStraightSpy() {
switch (currentState) {
case 1:
timer->Reset();
timer->Start();
turnController->Reset();
turnController->SetSetpoint(ahrs->GetYaw());
turnController->Enable();
currentState = 2;
break;
case 2:
intakeLever->Set(0.25);
if (timer->Get() >= 1) {
intakeLever->Set(0);
currentState = 3;
timer->Reset();
timer->Start();
}
break;
case 3:
drive->TankDrive(0.5, 0.5);
if (timer->Get() >= 5) {
drive->TankDrive(0.0, 0.0);
currentState = 4;
timer->Reset();
timer->Start();
}
break;
case 4:
intake->Set(0.5);
if (timer->Get() >= 2) {
currentState = 5;
}
break;
case 5:
intake->Set(0.0);
drive->TankDrive(0.0, 0.0);
break;
}
}
void SetAHRSPosData(AHRSProtocol::AHRSPosUpdate& ahrs_update) {
/* Update base IMU class variables */
ahrs->yaw = ahrs_update.yaw;
ahrs->pitch = ahrs_update.pitch;
ahrs->roll = ahrs_update.roll;
ahrs->compass_heading = ahrs_update.compass_heading;
ahrs->yaw_offset_tracker->UpdateHistory(ahrs_update.yaw);
/* Update AHRS class variables */
// 9-axis data
ahrs->fused_heading = ahrs_update.fused_heading;
// Gravity-corrected linear acceleration (world-frame)
ahrs->world_linear_accel_x = ahrs_update.linear_accel_x;
ahrs->world_linear_accel_y = ahrs_update.linear_accel_y;
ahrs->world_linear_accel_z = ahrs_update.linear_accel_z;
// Gyro/Accelerometer Die Temperature
ahrs->mpu_temp_c = ahrs_update.mpu_temp;
// Barometric Pressure/Altitude
ahrs->altitude = ahrs_update.altitude;
ahrs->baro_pressure = ahrs_update.barometric_pressure;
// Status/Motion Detection
ahrs->is_moving =
(((ahrs_update.sensor_status &
NAVX_SENSOR_STATUS_MOVING) != 0)
? true : false);
ahrs->is_rotating =
(((ahrs_update.sensor_status &
NAVX_SENSOR_STATUS_YAW_STABLE) != 0)
? false : true);
ahrs->altitude_valid =
(((ahrs_update.sensor_status &
NAVX_SENSOR_STATUS_ALTITUDE_VALID) != 0)
? true : false);
ahrs->is_magnetometer_calibrated =
(((ahrs_update.cal_status &
NAVX_CAL_STATUS_MAG_CAL_COMPLETE) != 0)
? true : false);
ahrs->magnetic_disturbance =
(((ahrs_update.sensor_status &
NAVX_SENSOR_STATUS_MAG_DISTURBANCE) != 0)
? true : false);
ahrs->quaternionW = ahrs_update.quat_w;
ahrs->quaternionX = ahrs_update.quat_x;
ahrs->quaternionY = ahrs_update.quat_y;
ahrs->quaternionZ = ahrs_update.quat_z;
ahrs->velocity[0] = ahrs_update.vel_x;
ahrs->velocity[1] = ahrs_update.vel_y;
ahrs->velocity[2] = ahrs_update.vel_z;
ahrs->displacement[0] = ahrs_update.disp_x;
ahrs->displacement[1] = ahrs_update.disp_y;
ahrs->displacement[2] = ahrs_update.disp_z;
ahrs->yaw_angle_tracker->NextAngle(ahrs->GetYaw());
}