Collision Detection is commonly used in automobiles to trigger airbag deployment, which can reduce the force of an impact and save lives during an accident. A similar technique can be used on a robot to detect when it has collided with another object.
The principle used within the Collision Detection example is the calculation of Jerk (which is defined as the change in acceleration). As shown in the graph below (taken from navX-sensor data recorded in LabVIEW of a small collision), whenever the jerk (in units of G) exceeds a threshold, a collision has occurred.
In the sample code shown below, both the X axis and the Y axis jerk are calculated, and if either exceeds a threshold, then a collision has occurred.
The “collision threshold” used in these samples will likely need to be tuned for your robot, since the amount of jerk which constitutes a collision will be dependent upon the robot mass and expected maximum velocity of either the robot, or any object which may strike the robot.
FRC C++ Example
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| #include "WPILib.h" | |
| #include "AHRS.h" | |
| /** | |
| * This is a demo program showing the use of the navX-MXP to implement | |
| * a collision detection feature, which can be used to detect events | |
| * while driving a robot, such as bumping into a wall or being hit | |
| * by another robot. | |
| * | |
| * The basic principle used within the Collision Detection example | |
| * is the calculation of Jerk (which is defined as the change in | |
| * acceleration). In the sample code shown below, both the X axis and | |
| * the Y axis jerk are calculated, and if either exceeds a threshold, | |
| * then a collision has occurred. | |
| * | |
| * The 'collision threshold' used in these samples will likely need to | |
| * be tuned for your robot, since the amount of jerk which constitutes | |
| * a collision will be dependent upon the robot mass and expected | |
| * maximum velocity of either the robot, or any object which may strike | |
| * the robot. | |
| */ | |
| #define COLLISION_THRESHOLD_DELTA_G 0.5f | |
| class Robot: public SampleRobot | |
| { | |
| // Channels for the wheels | |
| const static int frontLeftChannel = 2; | |
| const static int rearLeftChannel = 3; | |
| const static int frontRightChannel = 1; | |
| const static int rearRightChannel = 0; | |
| const static int joystickChannel = 0; | |
| RobotDrive robotDrive; // Robot drive system | |
| Joystick stick; // Driver Joystick | |
| AHRS *ahrs; // navX-MXP | |
| double last_world_linear_accel_x; | |
| double last_world_linear_accel_y; | |
| public: | |
| Robot() : | |
| robotDrive(frontLeftChannel, rearLeftChannel, | |
| frontRightChannel, rearRightChannel), // initialize variables in | |
| stick(joystickChannel) // same order declared above | |
| { | |
| last_world_linear_accel_x = 0.0f; | |
| last_world_linear_accel_y = 0.0f; | |
| robotDrive.SetExpiration(0.1); | |
| robotDrive.SetInvertedMotor(RobotDrive::kFrontLeftMotor, true); // invert left side motors | |
| robotDrive.SetInvertedMotor(RobotDrive::kRearLeftMotor, true); // change to match your robot | |
| 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); | |
| } | |
| } | |
| /** | |
| * 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 | |
| } | |
| } | |
| }; | |
| START_ROBOT_CLASS(Robot); |
by FRC Java Example
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| package org.usfirst.frc.team2465.robot; | |
| import com.kauailabs.navx.frc.AHRS; | |
| import edu.wpi.first.wpilibj.DriverStation; | |
| import edu.wpi.first.wpilibj.SPI; | |
| import edu.wpi.first.wpilibj.SampleRobot; | |
| import edu.wpi.first.wpilibj.RobotDrive; | |
| import edu.wpi.first.wpilibj.Joystick; | |
| import edu.wpi.first.wpilibj.Timer; | |
| import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; | |
| /** | |
| * This is a demo program showing the use of the navX-MXP to implement | |
| * a collision detection feature, which can be used to detect events | |
| * while driving a robot, such as bumping into a wall or being hit | |
| * by another robot. | |
| * | |
| * The basic principle used within the Collision Detection example | |
| * is the calculation of Jerk (which is defined as the change in | |
| * acceleration). In the sample code shown below, both the X axis and | |
| * the Y axis jerk are calculated, and if either exceeds a threshold, | |
| * then a collision has occurred. | |
| * | |
| * The “collision threshold” used in these samples will likely need to | |
| * be tuned for your robot, since the amount of jerk which constitutes | |
| * a collision will be dependent upon the robot mass and expected | |
| * maximum velocity of either the robot, or any object which may strike | |
| * the robot. | |
| */ | |
| public class Robot extends SampleRobot { | |
| AHRS ahrs; | |
| RobotDrive myRobot; | |
| Joystick stick; | |
| double last_world_linear_accel_x; | |
| double last_world_linear_accel_y; | |
| final static double kCollisionThreshold_DeltaG = 0.5f; | |
| public Robot() { | |
| myRobot = new RobotDrive(0, 1); | |
| myRobot.setExpiration(0.1); | |
| stick = new Joystick(0); | |
| 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 (RuntimeException ex ) { | |
| DriverStation.reportError("Error instantiating navX-MXP: " + ex.getMessage(), true); | |
| } | |
| } | |
| /** | |
| * Drive left & right motors for 2 seconds then stop | |
| */ | |
| public void autonomous() { | |
| myRobot.setSafetyEnabled(false); | |
| myRobot.drive(-0.5, 0.0); // drive forwards half speed | |
| Timer.delay(2.0); // for 2 seconds | |
| myRobot.drive(0.0, 0.0); // stop robot | |
| } | |
| /** | |
| * Runs the motors with arcade steering. | |
| */ | |
| public void operatorControl() { | |
| myRobot.setSafetyEnabled(true); | |
| while (isOperatorControl() && isEnabled()) { | |
| boolean 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 ( ( Math.abs(currentJerkX) > kCollisionThreshold_DeltaG ) || | |
| ( Math.abs(currentJerkY) > kCollisionThreshold_DeltaG) ) { | |
| collisionDetected = true; | |
| } | |
| SmartDashboard.putBoolean( "CollisionDetected", collisionDetected); | |
| try { | |
| myRobot.mecanumDrive_Cartesian(stick.getX(), stick.getY(), stick.getTwist(),0); | |
| } catch( RuntimeException ex ) { | |
| String err_string = "Drive system error: " + ex.getMessage(); | |
| DriverStation.reportError(err_string, true); | |
| } | |
| Timer.delay(0.005); // wait for a motor update time | |
| } | |
| } | |
| /** | |
| * Runs during test mode | |
| */ | |
| public void test() { | |
| } | |
| } |
Full Java Source Code
FRC LabView Example
The navX-sensor AutoBalance LabView example shows how to make small modifications to the LabView “FRC RoboRIO Robot Project” using the “Mecanum Robot” configuration to implement collision detection.
RobotMain.vi
Place the NavX main vi on the block diagram and set it up to your needs. The default sample rate is 50Hz. You may need to process faster for your situation. For the SPI, I2C and USB connections the max sample rate is 200Hz.
Teleop.vi
The Teleop.vi is modified to feed the Linear Acceleration to a threshold detector to determine if a collision has occurred.
