int moveStraight(int distance, int inchesOrEncoders = true, int speed = defaultMovementSpeed)
{
resetEncoders();
int encoderDistance = 0;
if(inchesOrEncoders){
encoderDistance = inchesToEncoder(distance);
}
else {
encoderDistance = distance;
}
if (distance > 0){
SetMotors(speed,speed);
while (averageEncoders() < encoderDistance){
wait1Msec(3);
}
}
else{
SetMotors(-speed,-speed);
while (averageEncoders() > encoderDistance){
wait1Msec(3);
}
}
SetMotors(0,0);
return averageEncoders();
}
void driveInches (float inches, int l, int r)
{
if(inches < 0)
{
l *= -1;
r *= -1;
inches *= -1;
}
//drive a certain amount of inches, order: # of inches, power left, power right
nMotorEncoder[left] = 0;
while(abs(nMotorEncoder[motorD]) <= inchesToEncoder(inches))
{
motor[left] = l;
motor[right] = r;
}
stopMotors();
}
task main()
{
eraseDisplay();
bool i = inputWaitForStart();//check for wait for start
wait1Msec(500);
if(i)
{
waitForStart();
}
initializeRobot();
/*
nMotorEncoder[left] = 0;
while(abs(nMotorEncoder[left]) < inchesToEncoder(32))
{
motor[right]=100;
motor[left] = 0;
PlaySound(soundBeepBeep);
}
stopMotors();
return;
*/
driveInches(88,35,35); //drive to tube
wait1Msec(500);
servoDown();
wait1Msec(500);
driveInches(-5);
raiseLift();
wait1Msec(500);
dump();
wait1Msec(500);
dumpUp();
wait1Msec(500);
// turn around and drive to ramp
nMotorEncoder[left] = 0;
//start of floor code
//turn around and drive to ramp
nMotorEncoder[right] = 0;
//start of floor code
//go flat against wall
while(abs(nMotorEncoder[right]) < inchesToEncoder(20))
{
motor[left]= 50;
motor[right] = -50;
print(nMotorEncoder[left], 4);
}
//turn towards floor parking zone
motor[left]= 100;
motor[right] = 0;
wait1Msec(200);
motor[left] = 0;
//drive to parking zone
driveInches(120,50,50);
//release thingy
servoUp();
return;
//start of ramp code
while(abs(nMotorEncoder[left]) < inchesToEncoder(64))
{
motor[right]=100;
motor[left] = 0;
}
stopMotors();
//driveInches(4);
//driveInches(40,50,0);
turn(LEFT, 40);
driveInches(257,100,100);
turn(RIGHT, 30);
wait1Msec(5000);
PlaySound(soundFastUpwardTones);
driveInches(30, 50, 50);
wait1Msec(500);
turn(LEFT, 30);
wait1Msec(500);
driveInches(30, 50, 50);
servoUp();
//drive back off of ramp
driveInches(-90);
}
float bhs_Autonomous::encoderToInches(int a_encoders) {
return a_encoders / inchesToEncoder(1);
}
//Maintains the given distance from the wall, while also maintaining a parallel alignment along the wall
//Calculates PID correction on both alignment and distance by using relative distances from front and back sensors to calcualte alignment, and the avg distance reading
//from back and front sensor to calculate distance
void wallFollowDrive1(float inches, int leftPow, int rightPow, float wallDistance)
{
/*
the correction is calculated by the formula:
C = P + I + D
P = error * kp
I = I * ki (I = integral sum of all errors)
D = D * kd (where D = the derivative of the error d/dt(e))
*/
//constants that vary the effect of the P, I, and D terms
//TODO: tune values
float kp = 2.0;//propotional constant
float ki = 0.1;//integral constant
float kd = 1;//derivative constant
//declare variables used in PID
int i = 0;//integral sum
float p=0;//error used to calcualate PID correction
int d;//derivative of error (p)
//used for avg distance
float avgDistkp = 2.0;//propotional constant
float avgDistki = 0.1;//integral constant
float avgDistkd = 1;//derivative constant
int avgDistI = 0;
float avgDistP=0;
int avgDistD;
int previousAvgDistP = 0;
int previousP = 0;//previous error used to calculate derivative
int correction = 0;
int encoderCounts = inchesToEncoder(inches);
nMotorEncoder[left] = 0;
while(nMotorEncoder[left] < encoderCounts)
{
int sonarValue = SensorValue[sonarFront] - SensorValue[sonarBack];//used to correct for alignment with wall
float avgDist = (SensorValue[sonarFront] + SensorValue[sonarBack])/2;//used to correct for distance from the wall
previousP = p;
p = sonarValue - wallDistance;
i += p;
d = p- previousP;//derivative disregards time (dt) as it should be time between loops should be constant - it will be compensated for in kd)
previousAvgDistP = avgDistP;
avgDistP = avgDist - wallDistance;
avgDistI += avgDistP;
avgDistD = avgDistP - previousAvgDistP;
correction = (int)round((kp*p + ki * i + kd*d)+(avgDistkp*avgDistP + avgDistki * avgDistI + avgDistkd*avgDistD));
//if sonarDiff > 0, sonarFront is farther away, meaning the robot is skewed left, there for add power to the left motor and subtract from
//therefore a positive correciton should be added to the left motor power and subtracted from right motor power to turn the robot closer to the wall
//and vice versa for sonarDiff < 0
int newLeft = leftPow + correction;
int newRight = rightPow - correction;
if(newLeft < 20)
newLeft = 20;
if(newRight < 20)
newRight =20;
if(newLeft > 80)
newLeft = 80;
if(newRight > 80)
newRight =80;
motor[left] = (newLeft*1.3);
motor[right] = newRight;
print(newLeft,2);
print(newRight,4);
print(sonarValue, 6);
print(p,8);
wait10Msec(10);
}
}
