Mandelbrot
21-02-2011, 19:32
Our build team has been working on the robot for quite a while and we haven't had time to check to see if our code will work. I don't think it's likely that we will have time soon, so I wanted to see if any of you on Chief Delphi would be able to spot any problems with it right off the bat.
This code is supposed to make the robot drive 100 inches and then stop, slowing down after every sixth of the target distance it has traveled.
void OperatorControl(void)
{
float X, Y, T, angle;
float armPosition;
int sensors;
double motorX = 0, motorY = 0;
double targetDistance = 100; //The total distance that the robot will drive
double drivePeriod = 0.05; //Time between motor update
double printPeriod = 0.25; //How long in between prints
double driveExpiration, printExpiration;
double powerProfile[] = {0.3, 0.25, 0.2, 0.15, 0.1, 0.09}; //To reduce the robot speed gradually
double distanceTraveled = 0;
int speed = 0; //The element of the powerProfile matrix
double currentSpeed = 0; //The current speed the robot is traveling at
int goneSixth = 1; //How many sixths of the total distance traveled
double distancePerPulse = .075398223686; //How many inches the robot travels after one pulse is sensed
int pulsePerRevolution = 250; //The pulses on one revolution of the wheel
int secPerMin = 60;
// dashboard.init();
// dashboard.log("Initializing...", "System State")
printf("Initializing...\n");
myRobot.SetSafetyEnabled(true);
gyro.Reset(); // remove this for actual matches where gyro is reset before autonomous
timer.Reset();
timer.Start();
driveExpiration = drivePeriod;
printExpiration = printPeriod;
motorX = 0;
motorY = -0.1;
while (IsOperatorControl())
{
else if (stick1.GetRawButton(6) == true)
{
timer.Reset();
timer.Start();
distanceTraveled = 0;
while (stick1.GetRawButton(6) == true && distanceTraveled <= targetDistance && timer.Get() < 4.0 )
{
if (timer.Get() > driveExpiration)
{
driveExpiration += drivePeriod; //Adds drivePeriod to the driveExpiration
//Calculates the current speed
currentSpeed = ((motorFrontLeft.GetSpeed() + motorFrontRight.GetSpeed() +
motorRearLeft.GetSpeed() + motorRearRight.GetSpeed())/4);
//Calculates the distance traveled
distanceTraveled += distancePerPulse * (pulsePerRevolution/secPerMin) * currentSpeed * drivePeriod;
//Decreases the speed after every sixth of the target distance the robot has driven.
if(distanceTraveled > (targetDistance * (goneSixth/7)))
{
speed++;
goneSixth++;
}
myRobot.MecanumDrive_Cartesian(0, -powerProfile[speed], 0, angle);
//printf("Period: %e Distance Traveled: %e Current Speed: %e\n" , drivePeriod, distanceTraveled, currentSpeed);
}
//Wait(waitPeriod);
}
myRobot.MecanumDrive_Cartesian(0, 0, 0, angle); //Stops the robot
//printf("Finished\n"); //Displays when the while loop ends
while( stick1.GetRawButton(6) == true)
{
}
}
Thanks in advance!
This code is supposed to make the robot drive 100 inches and then stop, slowing down after every sixth of the target distance it has traveled.
void OperatorControl(void)
{
float X, Y, T, angle;
float armPosition;
int sensors;
double motorX = 0, motorY = 0;
double targetDistance = 100; //The total distance that the robot will drive
double drivePeriod = 0.05; //Time between motor update
double printPeriod = 0.25; //How long in between prints
double driveExpiration, printExpiration;
double powerProfile[] = {0.3, 0.25, 0.2, 0.15, 0.1, 0.09}; //To reduce the robot speed gradually
double distanceTraveled = 0;
int speed = 0; //The element of the powerProfile matrix
double currentSpeed = 0; //The current speed the robot is traveling at
int goneSixth = 1; //How many sixths of the total distance traveled
double distancePerPulse = .075398223686; //How many inches the robot travels after one pulse is sensed
int pulsePerRevolution = 250; //The pulses on one revolution of the wheel
int secPerMin = 60;
// dashboard.init();
// dashboard.log("Initializing...", "System State")
printf("Initializing...\n");
myRobot.SetSafetyEnabled(true);
gyro.Reset(); // remove this for actual matches where gyro is reset before autonomous
timer.Reset();
timer.Start();
driveExpiration = drivePeriod;
printExpiration = printPeriod;
motorX = 0;
motorY = -0.1;
while (IsOperatorControl())
{
else if (stick1.GetRawButton(6) == true)
{
timer.Reset();
timer.Start();
distanceTraveled = 0;
while (stick1.GetRawButton(6) == true && distanceTraveled <= targetDistance && timer.Get() < 4.0 )
{
if (timer.Get() > driveExpiration)
{
driveExpiration += drivePeriod; //Adds drivePeriod to the driveExpiration
//Calculates the current speed
currentSpeed = ((motorFrontLeft.GetSpeed() + motorFrontRight.GetSpeed() +
motorRearLeft.GetSpeed() + motorRearRight.GetSpeed())/4);
//Calculates the distance traveled
distanceTraveled += distancePerPulse * (pulsePerRevolution/secPerMin) * currentSpeed * drivePeriod;
//Decreases the speed after every sixth of the target distance the robot has driven.
if(distanceTraveled > (targetDistance * (goneSixth/7)))
{
speed++;
goneSixth++;
}
myRobot.MecanumDrive_Cartesian(0, -powerProfile[speed], 0, angle);
//printf("Period: %e Distance Traveled: %e Current Speed: %e\n" , drivePeriod, distanceTraveled, currentSpeed);
}
//Wait(waitPeriod);
}
myRobot.MecanumDrive_Cartesian(0, 0, 0, angle); //Stops the robot
//printf("Finished\n"); //Displays when the while loop ends
while( stick1.GetRawButton(6) == true)
{
}
}
Thanks in advance!