Gyroscopes - Partie logicielle
Note
Cette section couvre le logiciel associé aux gyroscopes. Pour un guide sur le branchement électrique des gyroscopes, consulter Gyroscopes.
Un gyroscope, ou «gyro», est un capteur de vitesse angulaire généralement utilisé en robotique pour mesurer et/ou stabiliser le cap (direction angulaire souhaitée) d’un robot. WPILib fournit un support spécifique pour le gyroscope ADXRS450 disponible dans le kit de pièces, ainsi qu’un support plus général pour une plus grande variété de gyroscopes analogiques via la classe AnalogGyro.
Il existe des accesseurs (getters) de la vitesse angulaire et du cap actuels ainsi que des fonctions permettant de remettre à zéro le cap actuel et de calibrer le gyroscope.
Note
Il est primordial que le robot reste immobile pendant l’étalonnage d’un gyroscope.
ADIS16448
The ADIS16448 uses the ADIS16448_IMU
class (Java, C++, Python
). See the Analog Devices ADIS16448 documentation for additional information and examples.
Avertissement
La documentation d’Analog Devices liée ci-dessus contient des instructions obsolètes pour l’installation du logiciel, car l’ADIS16448 est désormais intégré à WPILib.
// ADIS16448 plugged into the MXP port
ADIS16448_IMU gyro = new ADIS16448_IMU();
// ADIS16448 plugged into the MXP port
ADIS16448_IMU gyro;
from wpilib import ADIS16448_IMU
# ADIS16448 plugged into the MXP port
self.gyro = ADIS16448_IMU()
ADIS16470
The ADIS16470 uses the ADIS16470_IMU
class (Java, C++, Python
). See the Analog Devices ADIS16470 documentation for additional information and examples.
Avertissement
La documentation d’Analog Devices liée ci-dessus contient des instructions obsolètes pour l’installation du logiciel, car l’ADIS16470 est désormais intégré à WPILib.
// ADIS16470 plugged into the SPI port
ADIS16470_IMU gyro = new ADIS16470_IMU();
// ADIS16470 plugged into the SPI port
ADIS16470_IMU gyro;
# ADIS16470 plugged into the SPI port
self.gyro = ADIS16470_IMU()
ADXRS450_Gyro
The ADXRS450_Gyro
class (Java, C++, Python
) provides support for the Analog Devices ADXRS450 gyro available in the kit of parts, which connects over the SPI bus.
Note
L’accumulation des données du gyroscope ADXRS450 est gérée par des circuits spéciaux dans le FPGA; en conséquence, une seule instance de ADXRS450_Gyro
peut être utilisée.
// Creates an ADXRS450_Gyro object on the onboard SPI port
ADXRS450_Gyro gyro = new ADXRS450_Gyro();
// Creates an ADXRS450_Gyro object on the onboard SPI port
frc::ADXRS450_Gyro gyro;
# Creates an ADXRS450_Gyro object on the onboard SPI port
self.gyro = ADXRS450_Gyro()
AnalogGyro
The AnalogGyro
class (Java, C++, Python
) provides support for any single-axis gyro with an analog output.
Note
L’accumulation des données du gyroscope est gérée par des circuits spéciaux dans le FPGA; en conséquence, AnalogGyro
`s ne peut être utilisé que sur les ports analogiques 0 et 1.
// Creates an AnalogGyro object on port 0
AnalogGyro gyro = new AnalogGyro(0);
// Creates an AnalogGyro object on port 0
frc::AnalogGyro gyro{0};
# Creates an AnalogGyro object on port 0
self.gyro = AnalogGyro(0)
Pigeon
Le Pigeon doit utiliser la classe WPI_PigeonIMU
. Le Pigeon peut être connecté avec le réseau CAN ou par câble de données à un TalonSRX. Le Guide de l’utilisateur de Pigeon IMU contient tous les détails sur l’utilisation du Pigeon.
WPI_PigeonIMU gyro = new WPI_PigeonIMU(0); // Pigeon is on CAN Bus with device ID 0
// OR (choose one or the other based on your connection)
TalonSRX talon = new TalonSRX(0); // TalonSRX is on CAN Bus with device ID 0
WPI_PigeonIMU gyro = new WPI_PigeonIMU(talon); // Pigeon uses the talon created above
WPI_PigeonIMU gyro{0}; // Pigeon is on CAN Bus with device ID 0
// OR (choose one or the other based on your connection)
TalonSRX talon{0}; // TalonSRX is on CAN Bus with device ID 0
WPI_PigeonIMU gyro{talon}; // Pigeon uses the talon created above
import phoenix5
import ctre.sensors
self.gyro = ctre.WPI_PigeonIMU(0); # Pigeon is on CAN Bus with device ID 0
# OR (choose one or the other based on your connection)
talon = ctre.TalonSRX(0); # TalonSRX is on CAN Bus with device ID 0
self.gyro = ctre.WPI_PigeonIMU(talon) # Pigeon uses the talon created above
Utilisation de gyroscopes dans le code
Note
Comme les gyroscopes mesurent le taux (variation) plutôt que la position angulaire, la position est calculée en intégrant (additionnant) les taux pour obtenir la valeur d’angle actuel. Par conséquant, les mesures d’angle gyroscopique sont toujours relatives à un angle zéro arbitraire (déterminé par l’angle du gyroscope lorsque le robot est mis en marche ou lorsqu’une méthode de mise à zéro a été appelée). Ces mesures sont également sujettes à des erreurs accumulées (appelées « dérives ») qui vont en s’augmentant, au fur et à mesure que le gyroscope est utilisé. La quantité de dérive varie selon le type de gyroscope.
Les gyroscopes sont extrêmement utiles en FRC pour mesurer et contrôler le cap du robot. Étant donné que les jeux FRC sont de courte durée (moins de 3 minutes), la dérive totale du gyroscope au cours d’un jeu FRC a tendance à être gérable (de l’ordre de quelques degrés pour un gyroscope de bonne qualité). De plus, ce ne sont pas toutes les applications gyroscopiques qui nécessitent que la mesure du cap reste précise tout au long du jeu.
Affichage du cap du robot sur le tableau de bord
Shuffleboard inclut un widget pour afficher les données de cap d’un gyro sous la forme d’une boussole. Cela peut être utile pour visualiser le cap du robot lorsque les lignes de vue vers le robot sont obscurcies:
// Use gyro declaration from above here
public void robotInit() {
// Places a compass indicator for the gyro heading on the dashboard
Shuffleboard.getTab("Example tab").add(gyro);
}
// Use gyro declaration from above here
void Robot::RobotInit() {
// Places a compass indicator for the gyro heading on the dashboard
frc::Shuffleboard.GetTab("Example tab").Add(gyro);
}
from wpilib.shuffleboard import Shuffleboard
def robotInit(self):
# Use gyro declaration from above here
# Places a compass indicator for the gyro heading on the dashboard
Shuffleboard.getTab("Example tab").add(self.gyro)
Stabilisation du cap pendant la conduite
Une utilisation très courante d’un gyroscope est de stabiliser le cap du robot pendant la conduite, de sorte que le robot roule en ligne droite. Ceci est particulièrement important pour les entraînements holonomiques tels que Mécanum et Swerve, mais aussi utile pour les entraînements différentiels, style « char d’assaut ».
Ceci est généralement réalisé en utilisant une boucle de contrôle PID axée sur la vitesse de rotation angulaire ou le cap du robot. La sortie de cette boucle est dirigée vers les contrôleurs de moteurs, et le code génère alors un léger différentiel de vitesse entre les deux côtés de l’entraînement (gauche et droite) afin de corriger le cap du robot. Pour les robots avec Mécanum ou Swerve, cette correction de cap est gérée de façon différente.
Avertissement
Comme pour toutes les boucles de contrôle, les utilisateurs doivent veiller à ce que la direction du capteur et la direction de rotation soient cohérentes. Si ce n’est pas le cas, la boucle sera instable et le robot pivotera autour de son axe central de manière incontrôlée.
Exemple: stabilisation de l’entraînement de type différentiel à l’aide du taux de rotation
L’exemple suivant montre comment stabiliser le cap à l’aide d’une simple boucle P axée sur le taux de rotation angulaire. Étant donné qu’un robot qui ne tourne pas devrait avoir un taux de rotation égal à zéro, le point de consigne pour la boucle est implicitement nul, ce qui rend cette méthode très simple.
// Use gyro declaration from above here
// The gain for a simple P loop
double kP = 1;
// Initialize motor controllers and drive
Spark leftLeader = new Spark(0);
Spark leftFollower = new Spark(1);
Spark rightLeader = new Spark(2);
Spark rightFollower = new Spark(3);
DifferentialDrive drive = new DifferentialDrive(leftLeader::set, rightLeader::set);
@Override
public void robotInit() {
// Configures the encoder's distance-per-pulse
// The robot moves forward 1 foot per encoder rotation
// There are 256 pulses per encoder rotation
encoder.setDistancePerPulse(1./256.);
// Invert the right side of the drivetrain. You might have to invert the other side
rightLeader.setInverted(true);
// Configure the followers to follow the leaders
leftLeader.addFollower(leftFollower);
rightLeader.addFollower(rightFollower);
}
@Override
public void autonomousPeriodic() {
// Setpoint is implicitly 0, since we don't want the heading to change
double error = -gyro.getRate();
// Drives forward continuously at half speed, using the gyro to stabilize the heading
drive.tankDrive(.5 + kP * error, .5 - kP * error);
}
// Use gyro declaration from above here
// The gain for a simple P loop
double kP = 1;
// Initialize motor controllers and drive
frc::Spark leftLeader{0};
frc::Spark leftFollower{1};
frc::Spark rightLeader{2};
frc::Spark rightFollower{3};
frc::DifferentialDrive drive{[&](double output) { leftLeader.Set(output); },
& <double output>__ { rightLeader.Set(output); }};
void Robot::RobotInit() {
// Invert the right side of the drivetrain. You might have to invert the other side
rightLeader.SetInverted(true);
// Configure the followers to follow the leaders
leftLeader.AddFollower(leftFollower);
rightLeader.AddFollower(rightFollower);
}
void Robot::AutonomousPeriodic() {
// Setpoint is implicitly 0, since we don't want the heading to change
double error = -gyro.GetRate();
// Drives forward continuously at half speed, using the gyro to stabilize the heading
drive.TankDrive(.5 + kP * error, .5 - kP * error);
}
from wpilib import Spark
from wpilib import MotorControllerGroup
from wpilib.drive import DifferentialDrive
def robotInit(self):
# Use gyro declaration from above here
# The gain for a simple P loop
self.kP = 1
# Initialize motor controllers and drive
left1 = Spark(0)
left2 = Spark(1)
right1 = Spark(2)
right2 = Spark(3)
leftMotors = MotorControllerGroup(left1, left2)
rightMotors = MotorControllerGroup(right1, right2)
self.drive = DifferentialDrive(leftMotors, rightMotors)
rightMotors.setInverted(true)
def autonomousPeriodic(self):
# Setpoint is implicitly 0, since we don't want the heading to change
error = -self.gyro.getRate()
# Drives forward continuously at half speed, using the gyro to stabilize the heading
self.drive.tankDrive(.5 + self.kP * error, .5 - self.kP * error)
Note
MotorControllerGroup is deprecated in 2024. Can you help update the Python example?
D’autres implémentations plus avancées peuvent utiliser une boucle de contrôle plus complexe. Les boucles PI sont particulièrement efficaces pour l’autorégulation de la direction basée sur le taux de rotation angulaire.
Exemple: stabilisation de l’entraînement du réservoir à l’aide du cap
L’exemple suivant montre comment stabiliser le cap à l’aide d’une simple boucle P axée sur le cap. Contrairement à l’exemple précédent, nous devrons régler le point de consigne sur le cap actuel du robot avant de commencer le mouvement, ce qui rend cette méthode légèrement plus compliquée.
// Use gyro declaration from above here
// The gain for a simple P loop
double kP = 1;
// The heading of the robot when starting the motion
double heading;
// Initialize motor controllers and drive
Spark left1 = new Spark(0);
Spark left2 = new Spark(1);
Spark right1 = new Spark(2);
Spark right2 = new Spark(3);
MotorControllerGroup leftMotors = new MotorControllerGroup(left1, left2);
MotorControllerGroup rightMotors = new MotorControllerGroup(right1, right2);
DifferentialDrive drive = new DifferentialDrive(leftMotors, rightMotors);
@Override
public void robotInit() {
rightMotors.setInverted(true);
}
@Override
public void autonomousInit() {
// Set setpoint to current heading at start of auto
heading = gyro.getAngle();
}
@Override
public void autonomousPeriodic() {
double error = heading - gyro.getAngle();
// Drives forward continuously at half speed, using the gyro to stabilize the heading
drive.tankDrive(.5 + kP * error, .5 - kP * error);
}
// Use gyro declaration from above here
// The gain for a simple P loop
double kP = 1;
// The heading of the robot when starting the motion
double heading;
// Initialize motor controllers and drive
frc::Spark left1{0};
frc::Spark left2{1};
frc::Spark right1{2};
frc::Spark right2{3};
frc::MotorControllerGroup leftMotors{left1, left2};
frc::MotorControllerGroup rightMotors{right1, right2};
frc::DifferentialDrive drive{leftMotors, rightMotors};
void Robot::RobotInit() {
rightMotors.SetInverted(true);
}
void Robot::AutonomousInit() {
// Set setpoint to current heading at start of auto
heading = gyro.GetAngle();
}
void Robot::AutonomousPeriodic() {
double error = heading - gyro.GetAngle();
// Drives forward continuously at half speed, using the gyro to stabilize the heading
drive.TankDrive(.5 + kP * error, .5 - kP * error);
}
from wpilib import Spark
from wpilib import MotorControllerGroup
from wpilib.drive import DifferentialDrive
def robotInit(self):
# Use gyro declaration from above here
# The gain for a simple P loop
self.kP = 1
# Initialize motor controllers and drive
left1 = Spark(0)
left2 = Spark(1)
right1 = Spark(2)
right2 = Spark(3)
leftMotors = MotorControllerGroup(left1, left2)
rightMotors = MotorControllerGroup(right1, right2)
self.drive = DifferentialDrive(leftMotors, rightMotors)
rightMotors.setInverted(true)
def autonomousInit(self):
# Set setpoint to current heading at start of auto
self.heading = self.gyro.getAngle()
def autonomousPeriodic(self):
error = self.heading - self.gyro.getAngle()
# Drives forward continuously at half speed, using the gyro to stabilize the heading
self.drive.tankDrive(.5 + self.kP * error, .5 - self.kP * error)
Les implémentations plus avancées peuvent utiliser une boucle de contrôle plus complexe. Les boucles PD sont particulièrement efficaces pour l’autorégulation de la direction basée sur le cap.
Rotation vers un cap défini
Une autre application courante et très utile pour un gyroscope consiste à faire tourner le robot pour qu’il pointe vers une direction spécifiée. Cela peut être utile lors de la période de conduite autonome, ou encore, pendant le contrôle téléopéré pour aider à aligner le robot avec un élément du terrain de jeu.
Tout comme avec la stabilisation de cap, ceci est accompli avec une boucle PID - contrairement à la stabilisation, la boucle doit être axée sur le cap uniquement. L’exemple de code suivant fera tourner le robot de 90 degrés avec une simple boucle P:
// Use gyro declaration from above here
// The gain for a simple P loop
double kP = 0.05;
// Initialize motor controllers and drive
Spark left1 = new Spark(0);
Spark left2 = new Spark(1);
Spark right1 = new Spark(2);
Spark right2 = new Spark(3);
MotorControllerGroup leftMotors = new MotorControllerGroup(left1, left2);
MotorControllerGroup rightMotors = new MotorControllerGroup(right1, right2);
DifferentialDrive drive = new DifferentialDrive(leftMotors, rightMotors);
@Override
public void robotInit() {
rightMotors.setInverted(true);
}
@Override
public void autonomousPeriodic() {
// Find the heading error; setpoint is 90
double error = 90 - gyro.getAngle();
// Turns the robot to face the desired direction
drive.tankDrive(kP * error, -kP * error);
}
// Use gyro declaration from above here
// The gain for a simple P loop
double kP = 0.05;
// Initialize motor controllers and drive
frc::Spark left1{0};
frc::Spark left2{1};
frc::Spark right1{2};
frc::Spark right2{3};
frc::MotorControllerGroup leftMotors{left1, left2};
frc::MotorControllerGroup rightMotors{right1, right2};
frc::DifferentialDrive drive{leftMotors, rightMotors};
void Robot::RobotInit() {
rightMotors.SetInverted(true);
}
void Robot::AutonomousPeriodic() {
// Find the heading error; setpoint is 90
double error = 90 - gyro.GetAngle();
// Turns the robot to face the desired direction
drive.TankDrive(kP * error, -kP * error);
}
from wpilib import Spark
from wpilib import MotorControllerGroup
from wpilib.drive import DifferentialDrive
def robotInit(self):
# Use gyro declaration from above here
# The gain for a simple P loop
self.kP = 0.05
# Initialize motor controllers and drive
left1 = Spark(0)
left2 = Spark(1)
right1 = Spark(2)
right2 = Spark(3)
leftMotors = MotorControllerGroup(left1, left2)
rightMotors = MotorControllerGroup(right1, right2)
self.drive = DifferentialDrive(leftMotors, rightMotors)
rightMotors.setInverted(true)
def autonomousPeriodic(self):
# Find the heading error; setpoint is 90
error = 90 - self.gyro.getAngle()
# Drives forward continuously at half speed, using the gyro to stabilize the heading
self.drive.tankDrive(self.kP * error, -self.kP * error)
Comme précédemment, les implémentations plus avancées peuvent utiliser des boucles de contrôle plus compliquées.
Note
Les boucles de rotation à angle peuvent être difficiles à régler correctement en raison de la force de friction existante dans la transmission, surtout si une simple boucle P est utilisée. Il existe plusieurs façons de contrebalancer cela; l’une des plus courantes/efficaces consiste à ajouter une valeur de « sortie minimale » à la sortie de la boucle de contrôle. Une autre stratégie efficace consiste à pré-régler les contrôleurs de vitesse pour chaque moteur, sur chaque côté du robot.