コード例 #1
0
ファイル: Robot.cpp プロジェクト: RoboLoCo-5338/BasicDrive
	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());
	}
コード例 #2
0
ファイル: Robot.cpp プロジェクト: RoboLoCo-5338/BasicDrive
	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;
		}
	}
コード例 #3
0
ファイル: Robot.cpp プロジェクト: RoboLoCo-5338/BasicDrive
	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;
		}
	}
コード例 #4
0
ファイル: AHRS.cpp プロジェクト: CRRobotics/2016Robot
    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());
    }