//rotates(point turn) the robot to reach a certain degree
void rotate(int goal, int cutOff)
{
//more variables for later
float error = 1800;
proportion = 0;
float Kp = 5;
//prepares goal for use with gyro since gyro measures 10 increments for each degree
goal *= 10;
//carrier for the sensor value
int gyroValue;
//buffer in gyro units
int buffer = 175;
//main loop. will continue adjusting until set at right angle
while(!((error<buffer) && (error> -buffer)))
{
//preparing error...mostly magic
gyroValue = SensorValue[gyro];
gyroValue = adjustGyro(gyroValue);
//error = centerGyro(goal, gyroValue);
error = goal - gyroValue;
if(gyroValue < goal)
{
setRightDrive(-127, cutOff);
setLeftDrive(127, cutOff);
}
else if(gyroValue >goal)
{
setRightDrive(127, cutOff);
setLeftDrive(-127, cutOff);
}
else
{
stopDrive();
}
/*
proportion = 127//(int)(abs(error) * Kp);
if(error > 0)
{
//when error is positive robot needs to rotate counter clockwise and if it is negative it is reversed
setRightDrive(-proportion, cutOff);
setLeftDrive(proportion, cutOff);
}
else
{
setRightDrive(proportion, cutOff);
setLeftDrive(-proportion, cutOff);
}
*/
//if the motors arn't moving due to low power input then check the timer else clear it
}
stopDrive();
}
void updateDrive()
{
if(abs(vexRT[Ch3]) > 10)
{
setLeftDrive(vexRT[Ch3]);
}
else
{
setLeftDrive(0);
}
if(abs(vexRT[Ch2]) > 10)
{
setRightDrive(vexRT[Ch2]);
}
else
{
setRightDrive(0);
}
}
//go forward for distance(in degrees) in a straight line at heading goal
//distance is always a positive integer = the distance in inches
//goal should be 0 if comming out of a turn or it should be the current gyro heading
//base power is positive or negative depending on desired direction
void forward(int distance, int goal, int basePower, int cutOff)
{
nMotorEncoder[BL] = 0;
nMotorEncoder[BR] = 0;
int direction;
//sets direction for use later
if(distance <= 0)
{
direction = -1;
}
else
{
direction = 1;
}
basePower*=direction;
//adjusts distance
distance = (int)(51 * distance -(277*direction));
//error variables for use later
float error;
float oldError = 0;
int change = 0;
//resets variables for use later
proportion = 0;
integral = 0;
derivative = 0;
//set constants for PID
float Kp = 0.5;
float Ki = 0.075;
float Kd = 6;
//prepares goal for use with gyro
goal *= 10;
//holds gyro value for future use
int gyroValue;
bool endLoop = false;
int encoderLeftVal = nMotorEncoder[BL];
int encoderRightVal = nMotorencoder[BR];
//will loop while the average of the sides is less than the direction
while(!endLoop)
{
encoderLeftVal = nMotorEncoder[BL];
encoderRightVal = nMotorencoder[BR];
if(direction > 0)
{
if(!(((nMotorEncoder[BL] + (nMotorEncoder[BR]*-1))/2) < distance))
{
endLoop = true;
}
}
else
{
if(!((nMotorEncoder[BL] + (nMotorEncoder[BR]*-1))/2 > distance))
{
endLoop = true;
}
}
//preparing error...mostly magic
gyroValue = SensorValue[gyro];
gyroValue = adjustGyro(gyroValue);
error = centerGyro(goal, gyroValue);
change = PID(Kp, Ki, Kd, error, oldError);
//subtracts(or adds a negative since change will be negaive in that case/..) to the side that is ahead
//multiplies by direction so that change is reversed
//when both signs are the same left is affected, when signs are different right is effected
if((change * direction) > 0)//if change and direction are both positive or negative
{
setRightDrive(basePower, cutOff);
setLeftDrive((basePower - change), cutOff);
}
else if((change * direction) < 0)//if either change or direction but not both are negative
{
setRightDrive((basePower + change), cutOff);
setLeftDrive(basePower, cutOff);
}
else
{
setRightDrive(basePower, cutOff);
setLeftDrive(basePower, cutOff);
}
//sets the current error to be the old error for the next iteration
oldError = error;
//waits so that calculations are at even intervals
wait1Msec(50);
}
stopDrive();
}
/////////////////////////////////////////////autonomous functions////////////////////////////////////////////////////////////////////////
void setDrive(int magnitude, int cutOff)
{
setRightDrive(magnitude, cutOff);
setLeftDrive(magnitude, cutOff);
}
void setDrive(int power)
{
setLeftDrive(power);
setRightDrive(power);
}
