void Robot::calibrateGrid()
{
int i = 0;
for(i = 0; i < 25; i++)
{
gridSensors.calibrate();
delay(20);
}
turnTo(NWEST);
delay(500);
moveTicks(AFEW, APPROACHSPEED);
stopMoving(APPROACHSPEED);
for(i = 0; i < 25; i++)
{
gridSensors.calibrate();
delay(20);
}
moveTicks(-AFEW, APPROACHSPEED);
stopMoving(APPROACHSPEED, true);
delay(500);
turnTo(NORTH);
delay(500);
}
// TODO: This function may need to be re-coded because it is too specific for
// the code we're on. It might be best to have something like this in the AI part.
// It might be best to just have openClaws and closeClaws.
void Robot::dropBlock()
{
unsigned int sensors[8];
bool atPoint = false;
bool delivered = false;
int ticks_moved=0;
turnTo(SOUTH);
movePoints(1);
moveTicks(60, 25);
stopMoving(25);
openClaws();
delay(300);
moveTicks(-50, 25);
stopMoving(25, true);
delay(300);
turnTo(NORTH);
delay(300);
movePoints(1);
delay(500);
/*while (!delivered)
{
ticks_moved = 0;
while ((ticks_moved < 70) && !atPoint)
{
//Go Forward
moveTicks(2, 25);
ticks_moved = ticks_moved + wheelEnc.getCountsM1();
gridSensors.readCalibrated(sensors, QTR_EMITTERS_ON);
if ((sensors[0] < SENSORBLACK) && (sensors[7] < SENSORBLACK) && (ticks_moved >10))
{
motorStop();
delay(2000);
atPoint = true;
servo(SERVO_RIGHT,30); //Check required values.
servo(SERVO_LEFT,80);
moveTicks(-ticks_moved, 20);
delivered = true;
}
}
motorStop();
delay(2000);
atPoint = false;
moveTicks(-ticks_moved, 20);
}*/
}
// stopMoving moves a short distance of 10 ticks to slow down, then stops.
// It is to be called whenever we want to stop.
// The movement errors used for adjustment are reset here.
// Use the motorSpeed that we would be travelling before stopping.
// This should be called after moveTicks() and moveTillPoint()
void Robot::stopMoving(int motorSpeed, bool backwards)
{
// Just move 10 ticks to slow down first.
if (backwards)
{
moveTicks(-5, motorSpeed - 20);
}
else
{
moveTicks(5, motorSpeed - 20);
}
motorStop();
lastError = 0;
errorSum = 0;
}
// TODO at the moment, this only works well for NORTH, SOUTH, EAST, WEST
// We'll try to sort it out for NEAST, SEAST, NWEST, SWEST later.
void Robot::moveTillPoint(int motorSpeed, bool forward, bool useBothSensors)
{
wheelEnc.getCountsAndResetM1();
wheelEnc.getCountsAndResetM2();
//Moves forward or backward based on the boolean 'forward' until a line intersection.
//unsigned int sensors[8];
//gridSensors.readLine(sensors, QTR_EMITTERS_ON, true);
int error;
//bool atPoint = false;
while(!reachedPoint(useBothSensors)/*!atPoint*/)
{
/* gridSensors.readLine(sensors, QTR_EMITTERS_ON, true);
if(reachedPoint(sensors, useBothSensors))
{
delay(5);
gridSensors.readLine(sensors, QTR_EMITTERS_ON, true);
if(reachedPoint(sensors, useBothSensors))
atPoint = true;
}*/
error = errorAdjustment();
if (forward)
motorForward(motorSpeed, error);
else
motorBackward(motorSpeed, error);
}
if(!useBothSensors)
moveTicks(5,STRAIGHTSPEED);
}
//Converts length into ticks and calls moveTicks function
void Movement::moveLength(int length, int speed)
{
//ticks = length*48/(D_w*pi) = length*48/(42*pi) ~= length*(48/132)
//Where D_w is the wheel diameter
moveTicks( round((float)(length * 48)/132.0), speed);
}
// movePoints has the robot move the number of points told.
// Positive is forward, negative is backwards.
// Slowing down does not happen until we have moved the number of points told.
// If you want to be safe, move one point. Todd suggests you take a risk.
void Robot::movePoints(int numberOfPoints, bool useBothSensors)
{
bool forward = true;
// Move the number of points. We're not going to slow down here.
if (numberOfPoints < 0)
{
numberOfPoints = -numberOfPoints;
forward = false;
}
for (int i = 0; i < numberOfPoints; i++)
{
switch (myDirection)
{
case NEAST:
case SEAST:
case NWEST:
case SWEST:
if (forward)
{
moveTicks(DIAGONALTICKS, STRAIGHTSPEED);
}
else
{
moveTicks(-DIAGONALTICKS, STRAIGHTSPEED);
}
break;
default:
if (forward)
{
moveTicks(ORTHOGONALTICKS, STRAIGHTSPEED);
}
else
{
moveTicks(-ORTHOGONALTICKS, STRAIGHTSPEED);
}
break;
}
moveTillPoint(STRAIGHTSPEED, forward, useBothSensors);
}
stopMoving(STRAIGHTSPEED);
unsigned int sensors[8];
gridSensors.readCalibrated(sensors, QTR_EMITTERS_ON);
delay(20);
// TODO WE NEED TO LOOK INTO THIS WAY MORE
// This code is being written assuming that our gridSensors have the right most sensor
// at sensor[0] and the left most sensor at sensor[8]
// Turn the error adjust off.
//errorAdjust = false;
/* switch (myDirection)
{
// If we're facing north east
case NEAST:
// If our right sensor is on the line, but left is not.
if ((sensors[0] < SENSORBLACK) && (sensors[7] > SENSORBLACK))
{
// Then we turn north.
// turnTo(NORTH);
// delay(10);
// Move till we hit the line.
moveTillPoint(APPROACHSPEED, forward, useBothSensors);
// Stop Moving
stopMoving(APPROACHSPEED);
// delay(10);
// Turn east.
// turnTo(EAST);
// delay(10);
// Move till we hit the line.
moveTillPoint(APPROACHSPEED, forward, useBothSensors);
// Stop Moving.
stopMoving(APPROACHSPEED);
delay(10);
}
// If our left sensor is on the line, but right is not.
else if ((sensors[0] > SENSORBLACK) && (sensors[7] < SENSORBLACK))
{
// Then we turn east.
turnTo(EAST);
delay(10);
// Move till we hit the line.
moveTillPoint(APPROACHSPEED, forward, useBothSensors);
// Stop Moving
stopMoving(APPROACHSPEED);
delay(10);
// Turn north.
turnTo(NORTH);
delay(10);
// Move till we hit the line.
moveTillPoint(APPROACHSPEED, forward, useBothSensors);
// Stop Moving.
stopMoving(APPROACHSPEED);
delay(10);
}
// If both sensors are on the line.
else
{
// Move forward a few ticks
moveTicks(AFEW, APPROACHSPEED);
stopMoving(APPROACHSPEED);
}
break;
case SEAST:
// If our right sensor is on the line, but left is not.
if ((sensors[0] < SENSORBLACK) && (sensors[7] > SENSORBLACK))
{
// Then we turn south.
turnTo(SOUTH);
// Move till we hit the line.
moveTillPoint(APPROACHSPEED, forward, useBothSensors);
// Stop Moving
stopMoving(APPROACHSPEED);
// Turn east.
turnTo(EAST);
// Move till we hit the line.
moveTillPoint(APPROACHSPEED, forward, useBothSensors);
// Stop Moving.
stopMoving(APPROACHSPEED);
}
// If our left sensor is on the line, but right
// is not.
else if ((sensors[0] > SENSORBLACK) && (sensors[7] < SENSORBLACK))
{
// Then we turn east.
turnTo(EAST);
// Move till we hit the line.
moveTillPoint(APPROACHSPEED, forward, useBothSensors);
// Stop Moving
stopMoving(APPROACHSPEED);
// Turn south.
turnTo(SOUTH);
// Move till we hit the line.
moveTillPoint(APPROACHSPEED, forward, useBothSensors);
// Stop Moving.
stopMoving(APPROACHSPEED);
}
// If both sensors are on the line.
else
{
// Move forward a few ticks
moveTicks(AFEW, APPROACHSPEED);
stopMoving(APPROACHSPEED);
}
break;
case NWEST:
// If our right sensor is on the line, but left is not.
if ((sensors[0] < SENSORBLACK) && (sensors[7] > SENSORBLACK))
{
// Then we turn north.
turnTo(NORTH);
// Move till we hit the line.
moveTillPoint(APPROACHSPEED, forward, useBothSensors);
// Stop Moving
stopMoving(APPROACHSPEED);
// Turn west.
turnTo(WEST);
// Move till we hit the line.
moveTillPoint(APPROACHSPEED, forward, useBothSensors);
// Stop Moving.
stopMoving(APPROACHSPEED);
}
// If our left sensor is on the line, but right is not.
else if ((sensors[0] > SENSORBLACK) && (sensors[7] < SENSORBLACK))
{
// Then we turn west.
turnTo(WEST);
// Move till we hit the line.
moveTillPoint(APPROACHSPEED, forward, useBothSensors);
// Stop Moving
stopMoving(APPROACHSPEED);
// Turn north.
turnTo(NORTH);
// Move till we hit the line.
moveTillPoint(APPROACHSPEED, forward, useBothSensors);
// Stop Moving.
stopMoving(APPROACHSPEED);
}
// If both sensors are on the line.
else
{
// Move forward a few ticks
moveTicks(AFEW, APPROACHSPEED);
stopMoving(APPROACHSPEED);
}
break;
case SWEST:
// If our right sensor is on the line, but left is not.
if ((sensors[0] < SENSORBLACK) && (sensors[7] > SENSORBLACK))
{
// Then we turn south.
turnTo(SOUTH);
// Move till we hit the line.
moveTillPoint(APPROACHSPEED, forward, useBothSensors);
// Stop Moving
stopMoving(APPROACHSPEED);
// Turn west.
turnTo(WEST);
// Move till we hit the line.
moveTillPoint(APPROACHSPEED, forward, useBothSensors);
// Stop Moving.
stopMoving(APPROACHSPEED);
}
// If our left sensor is on the line, but right is not.
else if ((sensors[0] > SENSORBLACK) && (sensors[7] < SENSORBLACK))
{
// Then we turn east.
turnTo(WEST);
// Move till we hit the line.
moveTillPoint(APPROACHSPEED, forward, useBothSensors);
// Stop Moving
stopMoving(APPROACHSPEED);
// Turn north.
turnTo(SOUTH);
// Move till we hit the line.
moveTillPoint(APPROACHSPEED, forward, useBothSensors);
// Stop Moving.
stopMoving(APPROACHSPEED);
}
// If both sensors are on the line.
else
{
// Move forward a few ticks
moveTicks(AFEW, APPROACHSPEED);
stopMoving(APPROACHSPEED);
}
break;
}
// Turn the error adjust back on.
errorAdjust = true;*/
}
