Hello. I am trying to use VCS to program a PD control loop for turning using the average of the rotations of two gyro, but something has gone wrong as the robot will do nothing but spin counterclockwise endlessly. The code is below. Thanks for any help.
float error=0.0;
int integral=0;
int derivative=0;
int speed;
int previous_error=0;
//these three variables below need to be adjusted for each robot to make the motors move effiecently
float kp=1.795;//the proportional term
float kd=0.0;//how fast is error changing.
double Angle;
bool Continue = true;
double theta;
void TurnRobot()
{
while(Continue)
{
theta = (Gyro1.value(vex::rotationUnits::deg)+Gyro2.value(vex::rotationUnits::deg))/2;
//the gyro reads upto 1/10th of a degree so its max value is 3600 so any angle measure given must be multiplied by 10 or you will turn a very small amount
error=(Angle) - theta;//the difference between the actual angle and the desired angle is the error
integral = integral+error;//the integral is an accumulator that keps the sum of all the errors
derivative=error-previous_error;//the derivative is the change in error since the last recorded error
previous_error=error;//sets the previous error as the one calculated in line 60
speed=(kp*error)+(kd*derivative);//this is the final product. It uses the sum of the calcukated terms times the coefficents to generate a speed
if(speed>200)//200 is the maximum velocity for the motors being used, so if the value calculated is greater than 200, it will be set to the max
{
speed=200;
}
if(speed<-200)//-200 is the maximum velocity in the reversed direction, so if it less than -200, it is set to -200
{
speed=-200;
}
LeftDrive.spin(vex::directionType(vex::rev), speed, vex::rpm);//one motor moves forward and the other moves backwards to make the robot spinv
RightDrive.spin(vex::directionType(vex::fwd), speed, vex::rpm);
vex::task::sleep(30);
}
}