Ejemplo n.º 1
0
	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());
	}
Ejemplo n.º 2
0
    /**
     * Runs the motors with Mecanum drive.
     */
    void OperatorControl()
    {
        robotDrive.SetSafetyEnabled(false);
        while (IsOperatorControl() && IsEnabled())
        {
            bool collisionDetected = false;

            double curr_world_linear_accel_x = ahrs->GetWorldLinearAccelX();
            double currentJerkX = curr_world_linear_accel_x - last_world_linear_accel_x;
            last_world_linear_accel_x = curr_world_linear_accel_x;
            double curr_world_linear_accel_y = ahrs->GetWorldLinearAccelY();
            double currentJerkY = curr_world_linear_accel_y - last_world_linear_accel_y;
            last_world_linear_accel_y = curr_world_linear_accel_y;

            if ( ( fabs(currentJerkX) > COLLISION_THRESHOLD_DELTA_G ) ||
                 ( fabs(currentJerkY) > COLLISION_THRESHOLD_DELTA_G) ) {
                collisionDetected = true;
            }
            SmartDashboard::PutBoolean(  "CollisionDetected", collisionDetected);

            try {
                /* Use the joystick X axis for lateral movement,            */
                /* Y axis for forward movement, and Z axis for rotation.    */
                /* Use navX MXP yaw angle to define Field-centric transform */
                robotDrive.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(),
                                                  stick.GetZ(),ahrs->GetAngle());
            } catch (std::exception ex ) {
                std::string err_string = "Error communicating with Drive System:  ";
                err_string += ex.what();
                DriverStation::ReportError(err_string.c_str());
            }
            Wait(0.005); // wait 5ms to avoid hogging CPU cycles
        }
    }
Ejemplo n.º 3
0
int main(int argc, char *argv[])
{
    QApplication a(argc, argv);
    AHRS w;
    w.show();
    
    return a.exec();
}
Ejemplo n.º 4
0
	// Start auto mode
	void AutonomousInit() override {
		autoSelected = *((int*) chooser.GetSelected()); // autonomous mode chosen in dashboard
		currentState = 1;
		ahrs->ZeroYaw();
		ahrs->GetFusedHeading();
		autoLength = SmartDashboard::GetNumber(AUTO_LENGTH, AUTO_LENGTH_DEFAULT);
		autoSpeed = SmartDashboard::GetNumber(AUTO_SPEED, AUTO_SPEED_DEFAULT);
		autoIntakeSpeed = SmartDashboard::GetNumber(AUTO_INTAKE_SPEED, AUTO_INTAKE_SPEED_DEFAULT);
		liftdown->Set(false);
	}
Ejemplo n.º 5
0
    /**
     * Drive based upon joystick inputs, and automatically control
     * motors if the robot begins tipping.
     */
    void OperatorControl()
    {
        robotDrive.SetSafetyEnabled(false);
        while (IsOperatorControl() && IsEnabled()) {

            double xAxisRate = stick.GetX();
            double yAxisRate = stick.GetY();
            double pitchAngleDegrees = ahrs->GetPitch();
            double rollAngleDegrees = ahrs->GetRoll();

            if ( !autoBalanceXMode &&
                 (fabs(pitchAngleDegrees) >=
                  fabs(kOffBalanceThresholdDegrees))) {
                autoBalanceXMode = true;
            }
            else if ( autoBalanceXMode &&
                      (fabs(pitchAngleDegrees) <=
                       fabs(kOnBalanceThresholdDegrees))) {
                autoBalanceXMode = false;
            }
            if ( !autoBalanceYMode &&
                 (fabs(pitchAngleDegrees) >=
                  fabs(kOffBalanceThresholdDegrees))) {
                autoBalanceYMode = true;
            }
            else if ( autoBalanceYMode &&
                      (fabs(pitchAngleDegrees) <=
                       fabs(kOnBalanceThresholdDegrees))) {
                autoBalanceYMode = false;
            }

            // Control drive system automatically,
            // driving in reverse direction of pitch/roll angle,
            // with a magnitude based upon the angle

            if ( autoBalanceXMode ) {
                double pitchAngleRadians = pitchAngleDegrees * (M_PI / 180.0);
                xAxisRate = sin(pitchAngleRadians) * -1;
            }
            if ( autoBalanceYMode ) {
                double rollAngleRadians = rollAngleDegrees * (M_PI / 180.0);
                yAxisRate = sin(rollAngleRadians) * -1;
            }

            try {
                // Use the joystick X axis for lateral movement, Y axis for forward movement, and Z axis for rotation.
                robotDrive.MecanumDrive_Cartesian(xAxisRate, yAxisRate,stick.GetZ());
            } catch (std::exception ex ) {
                std::string err_string = "Drive system error:  ";
                err_string += ex.what();
                DriverStation::ReportError(err_string.c_str());
            }
            Wait(0.005); // wait 5ms to avoid hogging CPU cycles
        }
    }
int main(int argc, char *argv[])
{
    //-------------------- IMU setup and main loop ----------------------------
    imuSetup();

    ros::init(argc, argv, "ros_erle_imu_euler");
    
    ros::NodeHandle n;

    ros::Publisher imu_euler_pub = n.advertise<ros_erle_imu_euler::euler>("euler", 1000);

    ros::Rate loop_rate(50);

    while (ros::ok()){

        //----------------------- Calculate delta time ----------------------------

        gettimeofday(&tv,NULL);
        previoustime = currenttime;
        currenttime = 1000000 * tv.tv_sec + tv.tv_usec;
        dt = (currenttime - previoustime) / 1000000.0;
        if(dt < 1/1300.0) 
            usleep((1/1300.0-dt)*1000000);
        gettimeofday(&tv,NULL);
        currenttime = 1000000 * tv.tv_sec + tv.tv_usec;
        dt = (currenttime - previoustime) / 1000000.0;

        //-------- Read raw measurements from the MPU and update AHRS --------------
        // Accel + gyro.
        imu.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz);
        ahrs.updateIMU(ax, ay, az, gx*0.0175, gy*0.0175, gz*0.0175, dt);

        //------------------------ Read Euler angles ------------------------------

        ahrs.getEuler(&roll, &pitch, &yaw);

        ros_erle_imu_euler::euler msg;
        msg.roll = roll;
        msg.pitch = pitch;
        msg.yaw = yaw;
        imu_euler_pub.publish(msg);

        ros::spinOnce();
        loop_rate.sleep();
    }

    return 0;

}
Ejemplo n.º 7
0
void imuSetup ( void ) {

	//MPU initialization

	imu->initialize();

	printf( "Beginning Gyro calibration...\n" );

	for( int i = 0 ; i < 100; i++ ) {

		imu->update();
		imu->read_gyroscope( &gx , &gy , &gz);

		offset[0] += ( -gx );
		offset[1] += ( -gy );
		offset[2] += ( -gz );

		usleep(10000);

	}

	offset[0] /= 100.0;
	offset[1] /= 100.0;
	offset[2] /= 100.0;

	printf( "Offsets are: %f %f %f\n" ,offset[0] ,offset[1] ,offset[2] );
	ahrs.setGyroOffset( offset[0] , offset[1] , offset[2] );
}
Ejemplo n.º 8
0
void imuSetup()
{
    //----------------------- MPU initialization ------------------------------

    imu->initialize();

    //-------------------------------------------------------------------------

	printf("Beginning Gyro calibration...\n");
	for(int i = 0; i<100; i++)
	{
		imu->update();
    		imu->read_gyroscope(&gx, &gy, &gz);

		gx *= 180 / PI;
		gy *= 180 / PI;
		gz *= 180 / PI;

		offset[0] += (-gx*0.0175);
		offset[1] += (-gy*0.0175);
		offset[2] += (-gz*0.0175);
		usleep(10000);
	}
	offset[0]/=100.0;
	offset[1]/=100.0;
	offset[2]/=100.0;

	printf("Offsets are: %f %f %f\n", offset[0], offset[1], offset[2]);
	ahrs.setGyroOffset(offset[0], offset[1], offset[2]);
}
Ejemplo n.º 9
0
 /**
  * Runs the motors with Mecanum drive.
  */
 void OperatorControl()
 {
     robotDrive.SetSafetyEnabled(false);
     while (IsOperatorControl() && IsEnabled())
     {
         bool reset_yaw_button_pressed = stick.GetRawButton(1);
         if ( reset_yaw_button_pressed ) {
             ahrs->ZeroYaw();
         }
         bool rotateToAngle = false;
         if ( stick.GetRawButton(2)) {
             turnController->SetSetpoint(0.0f);
             rotateToAngle = true;
         } else if ( stick.GetRawButton(3)) {
             turnController->SetSetpoint(90.0f);
             rotateToAngle = true;
         } else if ( stick.GetRawButton(4)) {
             turnController->SetSetpoint(179.9f);
             rotateToAngle = true;
         } else if ( stick.GetRawButton(5)) {
             turnController->SetSetpoint(-90.0f);
             rotateToAngle = true;
         }
         double currentRotationRate;
         if ( rotateToAngle ) {
             turnController->Enable();
             currentRotationRate = rotateToAngleRate;
         } else {
             turnController->Disable();
             currentRotationRate = stick.GetTwist();
         }
         try {
             /* Use the joystick X axis for lateral movement,          */
             /* Y axis for forward movement, and the current           */
             /* calculated rotation rate (or joystick Z axis),         */
             /* depending upon whether "rotate to angle" is active.    */
             robotDrive.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(),
                                               currentRotationRate ,ahrs->GetAngle());
         } catch (std::exception ex ) {
             std::string err_string = "Error communicating with Drive System:  ";
             err_string += ex.what();
             DriverStation::ReportError(err_string.c_str());
         }
         Wait(0.005); // wait 5ms to avoid hogging CPU cycles
     }
 }
Ejemplo n.º 10
0
	void AutonomousGyroTurn() {
		switch (currentState) {
		case 1:

			timer->Reset();
			timer->Start();
			//State: Stopped
			//Transition: Driving State
			currentState = 2;
			break;
		case 2:
			//State: Driving
			//Stay in State until 2 seconds have passed--`
			//Transition: Gyroturn State
			drive->TankDrive(0.5, 0.5);
			if (timer->Get() >= 1) {
				drive->TankDrive(0.0, 0.0);
				ahrs->ZeroYaw();
				currentState = 3;
				turnController->SetSetpoint(90);
				turnController->Enable();
			}
			break;
		case 3:
			//State: Gyroturn
			//Stay in state until navx yaw has reached 90 degrees
			//Transition: Driving State
			drive->TankDrive(0.5 * rotateRate, -0.5 * rotateRate);
//			if (ahrs->GetYaw() >= 90) {
			if (turnController->OnTarget()) {
				drive->TankDrive(0.0, 0.0);
				currentState = 4;
				timer->Reset();
				timer->Start();
			}
			break;
		case 4:
			//State:Driving
			//Stay in state until 2 seconds have passed
			//Transition: State Stopped
			drive->TankDrive(0.5, 0.5);
			if (timer->Get() >= 1) {
				currentState = 5;
				timer->Stop();
			}
			break;
		case 5:
			//State: Stopped
			drive->TankDrive(0.0, 0.0);
			break;

		}

	}
Ejemplo n.º 11
0
int main(int argc, char *argv[])
{
    if (argc == 3) {
        try {
            AHRS *imu = new AHRS(std::string(argv[1]),getBaudrate(argv[2]));
            char *buffer = NULL;
            size_t bufsize;
            // Connect to the AHRS.
            imu->openDevice();
            printf("Connected to %s.\n", (imu->getInfo()).c_str());
            // Prompt the user for a new baudrate.
            printf("New speed? (baud) \n");
            if (getline(&buffer, &bufsize, stdin) == -1)
            {
                fprintf(stderr, "Unable to read in new baudrate.\n");
                imu->closeDevice();
                return -1;
            }
            // Attempt to read in a baudrate and change to it.
            imu->setBaudrate(getBaudrate(buffer));
            // Clean up and close resources.
            free(buffer);
            imu->closeDevice();
            // Tell the user of the result.
            fprintf(stderr, "Changed speed successfully.\n");
            exit(0);
        } catch (std::exception& e) {
            printError(e);
        }
    }
    printUsage();
}
Ejemplo n.º 12
0
    /**
     * Runs the motors with Mecanum drive.
     */
    void OperatorControl()
    {
        robotDrive.SetSafetyEnabled(false);
        while (IsOperatorControl() && IsEnabled())
        {
            bool motionDetected = ahrs->IsMoving();
            SmartDashboard::PutBoolean("MotionDetected", motionDetected);

            try {
                /* Use the joystick X axis for lateral movement,            */
                /* Y axis for forward movement, and Z axis for rotation.    */
                /* Use navX MXP yaw angle to define Field-centric transform */
                robotDrive.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(),
                                                  stick.GetZ(),ahrs->GetAngle());
            } catch (std::exception ex ) {
                std::string err_string = "Error communicating with Drive System:  ";
                err_string += ex.what();
                DriverStation::ReportError(err_string.c_str());
            }
            Wait(0.005); // wait 5ms to avoid hogging CPU cycles
        }
    }
Ejemplo n.º 13
0
	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;
		}
	}
Ejemplo n.º 14
0
	void RobotInit() override {
		lw = LiveWindow::GetInstance();
		drive->SetExpiration(20000);
		drive->SetSafetyEnabled(false);
		//Gyroscope stuff
		try {
			/* Communicate w/navX-MXP via the MXP SPI Bus.                                       */
			/* Alternatively:  I2C::Port::kMXP, SerialPort::Port::kMXP or SerialPort::Port::kUSB */
			/* See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface/ for details.   */
			ahrs = new AHRS(SPI::Port::kMXP);
		} catch (std::exception ex) {
			std::string err_string = "Error instantiating navX-MXP:  ";
			err_string += ex.what();
			//DriverStation::ReportError(err_string.c_str());
		}

		if (ahrs) {
			LiveWindow::GetInstance()->AddSensor("IMU", "Gyro", ahrs);
			ahrs->ZeroYaw();

			// Kp	  Ki	 Kd		Kf    PIDSource PIDoutput
			turnController = new PIDController(0.015f, 0.003f, 0.100f, 0.00f,
					ahrs, this);
			turnController->SetInputRange(-180.0f, 180.0f);
			turnController->SetOutputRange(-1.0, 1.0);
			turnController->SetAbsoluteTolerance(2); //tolerance in degrees
			turnController->SetContinuous(true);
		}
		chooser.AddDefault("No Auto", new int(0));
		chooser.AddObject("GyroTest Auto", new int(1));
		chooser.AddObject("Spy Auto", new int(2));
		chooser.AddObject("Low Bar Auto", new int(3));
		chooser.AddObject("Straight Spy Auto", new int(4));
		chooser.AddObject("Adjustable Straight Auto", new int(5));
		SmartDashboard::PutNumber(AUTO_LENGTH, AUTO_LENGTH_DEFAULT);
		SmartDashboard::PutNumber(AUTO_SPEED, AUTO_SPEED_DEFAULT);
		SmartDashboard::PutNumber(AUTO_INTAKE_SPEED, AUTO_INTAKE_SPEED_DEFAULT);
		SmartDashboard::PutData("Auto Modes", &chooser);
		liftdown->Set(false);
		comp->Start();
	}
Ejemplo n.º 15
0
	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 imuSetup()
{
    //----------------------- MPU initialization ------------------------------

    imu.initialize();

    //-------------------------------------------------------------------------

	printf("Beginning Gyro calibration...\n");
	for(int i = 0; i<100; i++)
	{
		imu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
		offset[0] += (-gx*0.0175);
		offset[1] += (-gy*0.0175);
		offset[2] += (-gz*0.0175);
		usleep(10000);
	}
	offset[0]/=100.0;
	offset[1]/=100.0;
	offset[2]/=100.0;

	printf("Offsets are: %f %f %f\n", offset[0], offset[1], offset[2]);
	ahrs.setGyroOffset(offset[0], offset[1], offset[2]);
}
Ejemplo n.º 17
0
void imuLoop()
{
    //----------------------- Calculate delta time ----------------------------

	gettimeofday(&tv,NULL);
	previoustime = currenttime;
	currenttime = 1000000 * tv.tv_sec + tv.tv_usec;
	dt = (currenttime - previoustime) / 1000000.0;
	if(dt < 1/1300.0) usleep((1/1300.0-dt)*1000000);
        gettimeofday(&tv,NULL);
        currenttime = 1000000 * tv.tv_sec + tv.tv_usec;
	dt = (currenttime - previoustime) / 1000000.0;

    //-------- Read raw measurements from the MPU and update AHRS --------------

    // Accel + gyro.
	/*
    imu->update();
    imu->read_accelerometer(&ax, &ay, &az);
    imu->read_gyroscope(&gx, &gy, &gz);

    ax /= G_SI;
    ay /= G_SI;
    az /= G_SI;
    gx *= 180 / PI;
    gy *= 180 / PI;
    gz *= 180 / PI;

    ahrs.updateIMU(ax, ay, az, gx*0.0175, gy*0.0175, gz*0.0175, dt);
    	*/
    // Accel + gyro + mag.
    // Soft and hard iron calibration required for proper function.
    
    imu->update();
    imu->read_accelerometer(&ay, &ax, &az);
    imu->read_gyroscope(&gy, &gx, &gz);
    imu->read_magnetometer(&my, &mx, &mz);

    ax /= -G_SI;
    ay /= -G_SI;
    az /= G_SI;
    gx *= 180 / PI;
    gy *= 180 / PI;
    gz *= -180 / PI;

    ahrs.update(ax, ay, az, gx*0.0175, gy*0.0175, gz*0.0175, my, mx, -mz, dt);
    

    //------------------------ Read Euler angles ------------------------------

    //ahrs.getEuler(&pitch, &roll, &yaw);//roll and pitch inverted 
    ahrs.getEuler(&roll, &pitch, &yaw);

    //------------------- Discard the time of the first cycle -----------------

    if (!isFirst)
    {
    	if (dt > maxdt) maxdt = dt;
    	if (dt < mindt) mindt = dt;
    }
    isFirst = 0;
}
Ejemplo n.º 18
0
void imuLoop()
{

    float dtsum = 0.0f; //sum of delta t's

    while(dtsum < 1.0f/freq) //run this loop at 1300 Hz (Max frequency gives best results for Mahony filter)
    {
		//----------------------- Calculate delta time ----------------------------

		gettimeofday(&tv,NULL);
		previoustime = currenttime;
		currenttime = 1000000 * tv.tv_sec + tv.tv_usec;
		dt = (currenttime - previoustime) / 1000000.0;
		if(dt < 1/1300.0) usleep((1/1300.0-dt)*1000000);
		gettimeofday(&tv,NULL);
		currenttime = 1000000 * tv.tv_sec + tv.tv_usec;
		dt = (currenttime - previoustime) / 1000000.0;

	    //-------- Read raw measurements from the MPU and update AHRS --------------

	    // Accel + gyro.
	
	    imu->update();
	    imu->read_accelerometer(&ax, &ay, &az);
	    imu->read_gyroscope(&gx, &gy, &gz);

	    ax /= G_SI;
	    ay /= G_SI;
	    az /= G_SI;
	    gx *= 180 / PI;
	    gy *= 180 / PI;
	    gz *= 180 / PI;

	    ahrs.updateIMU(ax, ay, az, gx*0.0175, gy*0.0175, gz*0.0175, dt);
	    	
	    // Accel + gyro + mag.
	    // Soft and hard iron calibration required for proper function.
	    /*
	    imu->update();
	    imu->read_accelerometer(&ay, &ax, &az);
		az *= -1;
	    imu->read_gyroscope(&gy, &gx, &gz);
		gz *= -1;
	    imu->read_magnetometer(&mx, &my, &mz);

	    ax /= G_SI;
	    ay /= G_SI;
	    az /= G_SI;
	    gx *= 180 / PI;
	    gy *= 180 / PI;
	    gz *= 180 / PI;

	   ahrs.update(-ax, -ay, -az, gx*0.0175, gy*0.0175, gz*0.0175, mx, my, mz, dt);
	    */

	    //------------------------ Read Euler angles ------------------------------

	    ahrs.getEuler(&pitch, &roll, &yaw);

	    //------------------- Discard the time of the first cycle -----------------

	    if (!isFirst)
	    {
	    	if (dt > maxdt) maxdt = dt;
	    	if (dt < mindt) mindt = dt;
	    }
	    isFirst = 0;
	
	dtsum += dt;
    }


    
}
Ejemplo n.º 19
0
    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());
    }
Ejemplo n.º 20
0
void imuLoop ( void ) {

	//Calculate delta time

	gettimeofday( &tv , NULL );
	previoustime = currenttime;
	currenttime = 1000000 * tv.tv_sec + tv.tv_usec;
	dt = ( currenttime - previoustime ) / 1000000.0;
	if( dt < 1/1300.0 )  usleep( ( 1/1300.0 - dt ) * 1000000);
	gettimeofday( &tv , NULL) ;
	currenttime = 1000000 * tv.tv_sec + tv.tv_usec;
	dt = ( currenttime - previoustime ) / 1000000.0;

	//Read raw measurements from the MPU and update AHRS
	// Accel + gyro + mag.
	// Soft and hard iron calibration required for proper function.

	imu -> update();
	imu -> read_accelerometer( &ay , &ax , &az );
	imu -> read_gyroscope( &gy , &gx , &gz );
	imu -> read_magnetometer( &mx , &my , &mz );

	ax /= G_SI;
	ay /= G_SI;
	az /= G_SI;

	my = - my;

	ahrs.update( ax , ay , az , gx , gy , gz , mx , my , mz , dt );

	//Read Euler angles

	ahrs.getEuler( &pitch , &roll , &yaw );

	//Discard the time of the first cycle

	if ( !isFirst )	{

		if ( dt > maxdt ) maxdt = dt;
		if ( dt < mindt ) mindt = dt;

	}
	isFirst = 0;

	//Console and network output with a lowered rate

	dtsumm += dt;

	if( dtsumm > 0.05 ) {

		//Console output
		//printf("ROLL: %+05.2f PITCH: %+05.2f YAW: %+05.2f PERIOD %.4fs RATE %dHz \n", roll, pitch, gz/*yaw * -1*/, dt, int(1/dt));

		// Network output
		//sprintf( sendline , "%10f %10f %10f %10f %dHz\n" ,ahrs.getW() ,ahrs.getX() ,ahrs.getY() ,ahrs.getZ() ,int( 1/dt ) );
		//sendto( sockfd , sendline , strlen( sendline ) , 0 ,  ( struct sockaddr * )&servaddr , sizeof( servaddr ) );

		dtsumm = 0;

	}

}
Ejemplo n.º 21
0
	void AutonomousSpy() {
//		Strategy 1 - start as spy with a boulder, score in lower goal. Starts with intake facing low goal
//		-------------------------------------------------------------------------------------------------------------------
		switch (currentState) {
		case 1:
			//		-State: stopped
			timer->Reset();
			timer->Start();
			ahrs->ZeroYaw();
			currentState = 2;
			break;

//		--transition: state Driving Forward
		case 2:
			//		-State: Driving Forward
			//		--wait until lined up with low goal
			//		--transition: State stopped
			drive->TankDrive(0.5, 0.5);
			if (timer->Get() >= 1) { // NEEDS TO BE SET
				//		-State: stopped
				//		--wait until stopped
				drive->TankDrive(0.0, 0.0);
				currentState = 3;
				timer->Reset();
				timer->Start();
			}
			break;
			//		--transition: State Shooting
		case 3:
//		-State: Shooting
//		--wait until shooting complete
			intake->Set(-.5);
			if (timer->Get() >= .7) { //Find Out Actual Time
				intake->Set(0);
				timer->Reset();
				timer->Start();
				currentState = 4;
			}
			break;
			//		--transition: State Backing Up
		case 4:
			//		-State: Backing Up
			//		--wait until off tower ramp
			drive->TankDrive(-0.5, -0.5);
			if (timer->Get() > 1) {
				drive->TankDrive(0.0, 0.0);
				ahrs->ZeroYaw();
				ahrs->Reset();
				currentState = 5;
				turnController->SetSetpoint(-65.5);
				turnController->Enable();
			}
			break;

//		--transition: Turning
		case 5:
			//		-State: Turning Left
			//		--wait until 65 degrees has been reached to line up with low bar
			drive->TankDrive(-0.5, 0.5);
			if (turnController->OnTarget()) {
				drive->TankDrive(0.0, 0.0);
				timer->Reset();
				timer->Start();
				currentState = 6;
			}
			break;
//		--transition: Backing Up
		case 6:
			//		-State backing Up
			//		--wait until near guard wall
			drive->TankDrive(-0.5, -0.5);
			if (timer->Get() >= 1) {
				drive->TankDrive(0.0, 0.0);
				ahrs->ZeroYaw();
				ahrs->Reset();
				currentState = 7;
				turnController->SetSetpoint(-24.5);
				turnController->Enable();
			}
			break;
//		--transition: Turn Left
		case 7:
//		-State: Turn Right
//		--wait until 25 degree turn has been made to line with low bar
			drive->TankDrive(-0.5, 0.5);
			if (turnController->OnTarget()) {
				drive->TankDrive(0.0, 0.0);
				timer->Reset();
				timer->Start();
				currentState = 8;
			}
			break;
//		--transition: Back Up
		case 8:
//		-State: Backing Up
//		--wait until backed through low bar
			drive->TankDrive(-0.5, -0.5);
			if (timer->Get() >= 1) { // NeedTo Update Value
				timer->Stop();
				currentState = 9;
			}
			break;
//		--transition: Stopped
		case 9:
//		-State: Stopped
			drive->TankDrive(0.0, 0.0);
			break;
		}
	}
Ejemplo n.º 22
0
	void TeleopPeriodic() override {
		float leftPower, rightPower; // Get the values for the main drive train joystick controllers
		leftPower = -leftjoystick->GetY();
		rightPower = -rightjoystick->GetY();

		float multiplier; // TURBO mode
		if (rightjoystick->GetRawButton(1))
		{
			multiplier = 1;
		} else {
			multiplier = 0.5;
		}

		// wtf is a setpoint - it's an angle to turn to
		if (leftjoystick->GetRawButton(6)) {
			turnController->Reset();
			turnController->SetSetpoint(0);
			turnController->Enable();
			ahrs->ZeroYaw();
			//ahrs->Reset();
		}

		// Press button to auto calculate angle to rotate bot to nearest ball
//		if(leftjoystick->GetRawButton(99))
//		{
//			ahrs->ZeroYaw();
//			turnController->Reset();
//			turnController->SetSetpoint(mqServer.GetDouble("angle"));
//			turnController->Enable();
//			aimState = 1;
//		}

		switch(aimState)
		{
		default:
		case 0: // No camera assisted turning
			//Drive straight with one controller, else: drive with two controllers
			if (leftjoystick->GetRawButton(1)) {
				drive->TankDrive(leftPower * multiplier, leftPower * multiplier,
						false);
			} else if (leftjoystick->GetRawButton(2)) {
				drive->TankDrive(leftPower * multiplier + rotateRate,
						leftPower * multiplier + -rotateRate, false);
			} else {
				drive->TankDrive(leftPower * multiplier, rightPower * multiplier,
						false);
			}
			break;
		case 1: // Camera assisted turning, deny input from controllers
			drive->TankDrive(rotateRate, -rotateRate, false);
			if(turnController->OnTarget() || leftjoystick->GetRawButton(97)) {
				aimState = 0; // Finished turning, auto assist off
				turnController->Disable();
				turnController->Reset();
			}
			break;
		}

		// That little flap at the bottom of the joystick
		float scaleIntake = (1 - (controlstick->GetThrottle() + 1) / 2);
		// Depending on the button, our intake will eat or shoot the ball
		if (controlstick->GetRawButton(2)) {
			intake->Set(-scaleIntake);
			shooter->Set(scaleIntake);
		} else if (controlstick->GetRawButton(1)) {
			intake->Set(scaleIntake);
			shooter->Set(-scaleIntake);
		} else {
			intake->Set(0);
			shooter->Set(0);
		}

		// Control the motor that lifts and descends the intake bar
		float intake_lever_power = 0;
		if (controlstick->GetRawButton(6)) {
			manual = true;
			intake_lever_power = .3;
//			intakeLever->Set(.30); // close
		} else if (controlstick->GetRawButton(4)) {
			manual = true;
			intake_lever_power = -.4;
//			intakeLever->Set(-.40); // open
		} else if (controlstick->GetRawButton(3)){
			manual = true;
			intake_lever_power = -scaleIntake;
//			intakeLever->Set(-scaleIntake);
		} else if (controlstick->GetRawButton(5)) {
			manual = true;
			intake_lever_power = scaleIntake;
//			intakeLever->Set(scaleIntake);
		} else {
			if (manual) {
				manual = false;
				lastLiftPos = intakeLever->GetEncPosition();
				intakeLever->SetControlMode(CANTalon::ControlMode::kPosition);
				intakeLever->SetFeedbackDevice(CANTalon::FeedbackDevice::QuadEncoder);
				intakeLever->SetPID(1, 0.001, 0.0);
				intakeLever->EnableControl();
			}
			intake_hold = true;
			intakeLever->Set(lastLiftPos);
		}
		if (manual) {
			intake_hold = false;
			intakeLever->SetControlMode(CANTalon::ControlMode::kPercentVbus);
			intakeLever->Set(intake_lever_power);
		}
		if (controlstick->GetRawButton(11)) {
			lift->Set(true);
			liftdown->Set(false);
		} else if (controlstick->GetRawButton(12)){
			lift->Set(false);
			liftdown->Set(true);
		} else if (controlstick->GetRawButton(7)) {
			liftdown->Set(false);
		}
		if (controlstick->GetRawButton(9)) {
			winch->Set(scaleIntake);
		} else if (controlstick->GetRawButton(10)) {
			winch->Set(-scaleIntake);
		} else {
			winch->Set(0);
		}
		if (controlstick->GetPOV() == 0 && !bounce ) {
			constantLift -= 0.05;
			bounce = true;
		} else if (controlstick->GetPOV() == 180 && !bounce) {
			constantLift += 0.05;
			bounce = true;
		} else if (controlstick->GetPOV() == 270 && !bounce) {
			constantLift = 0;
			bounce = true;
		} else {
			bounce = false;
		}
		UpdateDashboard();
	}
Ejemplo n.º 23
0
int main(int argc, char *argv[])
{
   if (argc == 3) {
      try {
         AHRS *ahrs = new AHRS(std::string(argv[1]),getBaudrate(argv[2]));
         AHRS::AcqConfig config;
         AHRS::FilterData filters;
         long unsigned int i;

         ahrs->openDevice();
         printf("Connected to %s.\n", (ahrs->getInfo()).c_str());

         ahrs->sendAHRSDataFormat();

         printf("AHRS Configuration data:\n");

         config = ahrs->getAcqConfig();
         printf("Acqusition mode:   %d\n", config.poll_mode);
         printf("Flush Filter:      %d\n", config.flush_filter);
         printf("Sample delay:      %f\n", config.sample_delay);

         filters = ahrs->getFIRFilters();
         printf("Number of filters: %lu\n", filters.size());
         for (i=0; i < filters.size(); i++)
         printf("Filter #%.2lu:        %f\n",i, filters[i]);

         printf("Mag Truth Method:  %d\n", ahrs->getMagTruthMethod());
         printf("Functional mode:   %d\n", ahrs->getAHRSMode());
         printf("Declination:       %f\n", ahrs->getDeclination());
         printf("UserCalNumPoints:  %d\n", ahrs->getCalPoints());
         printf("Mag Setting:       %d\n", ahrs->getMagCalID());
         printf("Accel setting:     %d\n", ahrs->getAccelCalID());
         printf("Mounting mode:     %d\n", ahrs->getMounting());
         printf("Baudrate:          %d\n", ahrs->getBaudrate().baud);
         printf("True north?        %d\n", ahrs->getTrueNorth());
         printf("Big endian?        %d\n", ahrs->getBigEndian());
         printf("Auto sampling?     %d\n", ahrs->getAutoCalibration());
         printf("Mils/Degrees?      %d\n", ahrs->getMils());
         printf("HPR During Cal?    %d\n", ahrs->getHPRCal());
         ahrs->closeDevice();
         exit(0);
      }
      catch (std::exception& e)
      {
         printError(e);
      }
   } 
   printUsage();
}