Hi everyone, I am trying to stabilize my X-Drive through a Gyroscope. I have done it be resetting the gyroscope when the rotational value changes, but I added a field centric driving control, which does not allow for this. I used trig to calculate direction and was wondering if there was code to stabilize it from these values. Here’s the code. #pragma config(Sensor, in1, Gyro, sensorGyro) #pragma config(Sensor, in2, Potchain, sensorPotentiometer) #pragma config(Sensor, in8, Engage, sensorPotentiometer) #pragma config(Motor, port2, LeftBack, tmotorVex393_MC29, openLoop) #pragma config(Motor, port3, RightBack, tmotorVex393_MC29, openLoop, reversed) #pragma config(Motor, port8, RightFront, tmotorVex393_MC29, openLoop, reversed) #pragma config(Motor, port9, LeftFront, tmotorVex393_MC29, openLoop, reversed)
task main()
{
enableCompetitionMode();
if (getCompetitionAutonomous() == true) {
}
if (getCompetitionDriverControl() == true) {
repeat (forever) {
setJoystickScale(50);
A = vexRT[Ch1] / 4;
B = vexRT[Ch3];
C = vexRT[Ch4];
D = SensorValue[Potchain] / 1;
G = SensorValue[Gyro] / 600;
P = sqrt(BB+CC);
theta = atan2(B,C);
thetagyro = theta - G;
X = Psin(thetagyro);
Y = Pcos(thetagyro);
if (SensorValue[Engage] > 1500) {
X = Psin(theta);
Y = Pcos(theta);
G = SensorValue[Gyro] / 6;
A = vexRT[Ch1] + G;
if (vexRT[Ch1] < -15) {
resetSensor(Gyro);
}
if (vexRT[Ch1] > 15) {
resetSensor(Gyro);
}
}
setMotor(LeftBack, X-Y+A);
setMotor(RightBack, X+Y-A);
setMotor(RightFront, X-Y-A);
setMotor(LeftFront, X+Y+A);
}
}
}
The Pot in the code allows for me to switch from Robot centric to field centric.
I’m a bit confused about what you mean by ‘stabilize.’ If you mean you want to get rid of all accumulated drift, there isn’t really an easy way to do this. Using multiple gyros and some sort of filter (e.g. a Kalman Filter), you can help keep them from drifting too much. Even then, though, it will eventually drift too far to be usable. My best advice would be some sort of reset button. If the driver hits a button the the joystick, reset the gyro to zero. Then if the driver notices the gyro starts to get off, they can line it up against a wall or something and reset it. Of course this would not be ideal, as it would take up a substantial amount of time in an already short game. If you have encoders on your wheels, they can be used to approximate orientation as well. You should be able to take the difference between two adjacent wheels (e.g. front left and back right) and multiply that by some factor to figure out orientation. You can average that the other pair (back left and front right) to obtain better results. This alone would not give very good performance, however, as if a motor skids or is raised off the ground while spinning, they would not necessarily give a good approximation of the angle. If you were to take into account both the gyro and encoders, your angle may be more accurate.
If I was wrong on what you meant by ‘stabilize,’ then this will probably not help much, but you will probably experience a lot of drift anyway with a field-centric control.
EDIT: the Kalman filter tutorial I linked refers to a one-dimensional Kalman filter, which would not work if you’re trying to make sense of multiple sensors. There’s a really good free Udacity course called “Artificial Intelligence for Robotics” that discusses Kalman Filters very early on, and I’d suggest it to anybody who wants an in-depth walkthrough on creating and using Kalman filters.
That’s not what i’m referring to. Sorry if I was not clear. X-drives tend to not drive straight as some wheels will jump and rotate more or less than the others, causing the yaw to change, even if you don’t control the rotation with the joystick. I prevent drift by 5 different mechanical isolators + 4 shaft encoders, which makes it practically perfect.
Ah, that makes more sense, thanks for clarifying. An easy way to ‘soft reset’ gyros (reset it to zero in some of your code while retaining the orientation for the rest) is with a variable. Anytime you would reset the gyro in the code, instead set a variable to the current gyro value. Then, to access the ‘reset’ gyro, you would do something like this:
SensorValue[gyro] - reset_variable;
assuming
reset_variable
is the variable I mentioned above and that you are using RobotC (if not, translating it shouldn’t be too hard).
The gyro still retains its orientation data, but if you need the difference between when you ‘reset’ it and where it is now, just subtract them.