void moveForwardSquare()
{
double xDist = 0;
double yDist = 0;
double angle = 0;
double distance = 0;
int prevTicksLeft = 0;
int prevTicksRight = 0;
drive_getTicks(&prevTicksLeft, &prevTicksRight);
while(distance < GRID_SIZE) {
getIR();
int changeVal;
if(irLeft < SENSOR_VALUE && irRight < SENSOR_VALUE){ // Wall either side
changeVal = (irLeft - irRight) * MULTIPLIER;
} else if ( irLeft < SENSOR_VALUE ) { // Wall to the left
changeVal = (irLeft - IR_LEFT) * MULTIPLIER;
} else if ( irRight < SENSOR_VALUE ) { // Wall to the right
changeVal = (IR_RIGHT - irRight) * MULTIPLIER;
} else { // If no walls to the side, carry on as normal
changeVal = 0;
}
drive_speed(BASE_SPEED_TICKS - changeVal, BASE_SPEED_TICKS + changeVal); // Set the new drive speed with the new changeVal
int ticksLeft, ticksRight;
drive_getTicks(&ticksLeft, &ticksRight); // get current ticks count
// calculate distances
double distRight = calcDistance(ticksRight, prevTicksRight); // Distance of left wheel
double distLeft = calcDistance(ticksLeft, prevTicksLeft); // Distance of right wheel
double distCentre = (distRight + distLeft) / 2; // The average of the left and right distance
angle = angle + (distRight - distLeft) / ROBOT_WIDTH;
// update prevTicks
prevTicksLeft = ticksLeft;
prevTicksRight = ticksRight;
xDist = xDist + distCentre * cos(angle);
yDist = yDist + distCentre * sin(angle);
distance = sqrt(xDist*xDist + yDist*yDist); // work out distance travelled using pythagoras
}
drive_speed(0,0);
}
void getTicks(void)
{
ticksLeftOld = ticksLeft;
ticksRightOld = ticksRight;
ticksLeftCalcOld = ticksLeftCalc;
ticksRightCalcOld = ticksRightCalc;
drive_getTicks(&ticksLeft, &ticksRight);
drive_getTicksCalc(&ticksLeftCalc, &ticksRightCalc);
drive_getSpeedCalc(&speedLeft, &speedRight);
}
int checkClose(){
irLeft = 0;
irRight = 0;
distance = ping_cm(8);
for(int dacVal = 0; dacVal < 160; dacVal += 8){
dac_ctr(26, 0, dacVal);
freqout(11, 1, 38000);
irLeft += input(10);
dac_ctr(27, 1, dacVal);
freqout(1, 1, 38000);
irRight += input(2);
}
if (irLeft<7) {
drive_speed(25,0);
} else if (irLeft > 9 || irLeft == 0){
drive_speed(0,25);
} else {
drive_speed(40,40);
}
if (distance<=5){
fullTurn(90);
degrees+=90;
end=1;
}
drive_getTicks(&ticksLeft, &ticksRight);
leftDeg = ((ticksLeft * 360) / 332.38128) * 3.25;
rightDeg = ((ticksRight * 360) / 332.38128) * 3.25;
degrees = degrees + (rightDeg - leftDeg);
moved = ticksLeft * 3.25;
}
int main() // main function
{
simpleterm_reopen(RX_PIN, TX_PIN, 0, BAUD);
//drive_open();
//drive_goto(0,0);
//drive_setMaxSpeed(MAX_SPEED);
int l=1;
int r=1;
drive_speed(-10,-10);
pause(1000);
drive_speed(0,0);
while(1) // Repeat indefinitely
{
for(int i = 0; i < 100; i++)
{
drive_getTicks(&l,&r); //only works when actually moving, otherwise no response
int ultrasonic = ping(PING_PIN);
//print("ultrasonic %d uS\n",ultrasonic);
print("%d %d\n", l, r);
pause(50);
}
}
}
char *drive_goto(int distLeft, int distRight)
{
char s[32];
memset(s, 0, 32);
char *reply = dhb10_reply;
int ticksL = 0, ticksR = 0, ticksLi = 0, ticksLf = 0, ticksRi = 0, ticksRf = 0;
int ticksLold = 0, ticksRold = 0;
if(!ard_opened) drive_open();
#ifdef ARLO_DEBUG
dprint(ard_dhb10_terminal, "drive_goto(%d, %d)\n", distLeft, distRight);
#endif
if(ard_blockGoto)
{
pause(20);
#ifdef ARLO_DEBUG
dprint(ard_dhb10_terminal, "if(ard_blockGoto)...\n");
#endif
drive_getTicks(&ticksLi, &ticksRi);
ticksLf = ticksLi + distLeft;
ticksRf = ticksRi + distRight;
ticksLold = ticksLi;
ticksRold = ticksRi;
#ifdef ARLO_DEBUG
dprint(ard_dhb10_terminal, "ticksLf(%d) = ticksLi(%d) + distLeft(%d)\n", ticksLf, ticksLi, distLeft);
dprint(ard_dhb10_terminal, "ticksRf(%d) = ticksRi(%d) + distReft(%d)\n", ticksRf, ticksRi, distRight);
#endif
}
sprint(s, "move %d %d %d\r", distLeft, distRight, ard_speedLimit);
reply = dhb10_com(s);
#ifdef ARLO_DEBUG
dprint(ard_dhb10_terminal, "reply = %s\n", reply);
#endif
if(ard_blockGoto)
{
int reps = 0;
int repsOld = 0;
while((reps < ard_gotoDoneReps) && (repsOld < ard_gotoDoneReps))
{
drive_getTicks(&ticksL, &ticksR);
if((abs(ticksLf - ticksL) <= (ard_deadZone)) &&
(abs(ticksRf - ticksR) <= (ard_deadZone)))
{
reps++;
}
//
else if((abs(ticksL - ticksLold) <= (ard_deadZone)) &&
(abs(ticksR - ticksRold) <= (ard_deadZone)))
{
repsOld++;
}
//
else
{
reps = 0;
repsOld = 0;
}
ticksLold = ticksL;
ticksRold = ticksR;
pause(20);
}
#ifdef ARLO_DEBUG
dprint(ard_dhb10_terminal, "Total Error: %d\n", abs(ticksLf - ticksL) + abs(ticksRf - ticksR));
#endif
//return abs(ticksLf - ticksL) + abs(ticksRf - ticksR);
}
return reply;
}
void Drive(int leftTicks, int rightTicks, int speed)
{
// Tick count when we started
int startLeftTicks;
int startRightTicks;
drive_getTicks(&startLeftTicks, &startRightTicks);
// Target tick counts
const int leftTargetTicks = startLeftTicks + leftTicks;
const int rightTargetTicks = startRightTicks + rightTicks;
// Acceleration
const int speedStep = 4; // Acceleration per 1/20th of a second
int currentSpeed = 0;
int destinationReached = 0;
int leftOldTicks;
int rightOldTicks;
drive_getTicks(&leftOldTicks, &rightOldTicks);
int stuck = 0;
const int maxTimesStuck = 10; // This is the number of times the robot has to think it's stuck before it stops
do
{
// Accelerate
currentSpeed += speedStep;
if(currentSpeed > speed)
currentSpeed = speed;
const int leftCurrentSpeed = leftTicks >= 0 ? currentSpeed : -currentSpeed;
const int rightCurrentSpeed = rightTicks >= 0 ? currentSpeed : -currentSpeed;
drive_speed(leftCurrentSpeed, rightCurrentSpeed);
pause(50);
// Current tick count
int leftCurrentTicks;
int rightCurrentTicks;
drive_getTicks(&leftCurrentTicks, &rightCurrentTicks);
// Check if we're stuck
if(leftOldTicks == leftCurrentTicks ||
rightOldTicks == rightCurrentTicks)
stuck++;
else
{
// dprint(term, "stuck: %d\n", stuck);/**/
stuck = 0;
}
if(stuck > maxTimesStuck)
{
// dprint(term, "Oh God, I really am stuck!!!\n");/**/
drive_speed(0, 0);
return;
}
leftOldTicks = leftCurrentTicks;
rightOldTicks = rightCurrentTicks;
destinationReached = (leftTicks >= 0 && leftCurrentTicks >= leftTargetTicks) ||
(leftTicks <= 0 && leftCurrentTicks <= leftTargetTicks) ||
(rightTicks >= 0 && rightCurrentTicks >= rightTargetTicks) ||
(rightTicks <= 0 && rightCurrentTicks <= rightTargetTicks);
//dprint(term, "currentSpeed: %d leftCurrentTicks: %d rightCurrentTicks: %d\n", currentSpeed, leftCurrentTicks, rightCurrentTicks);/**/
}
while(!destinationReached);
// Deaccelerate
while(currentSpeed > 0)
{
currentSpeed -= speedStep;
if(currentSpeed < 0)
currentSpeed = 0;
const int leftCurrentSpeed = leftTicks >= 0 ? currentSpeed : -currentSpeed;
const int rightCurrentSpeed = rightTicks >= 0 ? currentSpeed : -currentSpeed;
drive_speed(leftCurrentSpeed, rightCurrentSpeed);
}
}
