Day 13 - Ultra Sonic sensors on VEX squarebot

*The Lab today consisted of designing a squarebot that could navigate autonomously using functions and avoid walls forever or approach a can. The setup for this lab utilized encoders to consistently have the same movements; in addition, there were sonar sensors attached to the bot so it could "see" the walls and can to avoid or approach the items.

• Autonomous robot that avoids the walls
• Here is the code

#pragma config(Sensor, in2,    rightEncoder,   sensorRotation)

#pragma config(Sensor, in3,    leftEncoder,    sensorRotation)

#pragma config(Sensor, in9,    sonarSensor,    sensorSONAR, int1)

#pragma config(Motor,  port2,           rightMotor,    tmotorServoContinuousRotation, openLoop, reversed)

#pragma config(Motor,  port3,           leftMotor,     tmotorServoContinuousRotation, openLoop)

void clearEncoders();

void forward(int x);

void tleft(int x);

void tright(int x);

void backwards(int x);

void stop(int x);

task main()

{ wait1Msec(2000);

bMotorReflected[port2]=1;

while(1)

{

if (SensorValue[sonarSensor]>3 ||SensorValue[sonarSensor]==-1)//loop until sonar finds an object less than 3 inches.

{

forward(20);//go forward

}

else

{

backwards(20);

stop(1);

tleft(70);

}

}

}

void clearEncoders() //must clear encoders every time so that way their measurements are accurate

{

SensorValue[leftEncoder] = 0;

SensorValue[rightEncoder] = 0;

}

void forward(int x)

{

clearEncoders();

while(SensorValue[leftEncoder] < x && SensorValue[rightEncoder] <x) // read both the left && right encoders

{

motor[leftMotor] = 60;

motor[rightMotor] = 50;

}

}

void tleft(int x)

{

clearEncoders();

while(SensorValue[leftEncoder] < x && SensorValue[rightEncoder] <x)

{

motor[leftMotor] = -40;

motor[rightMotor] = 40;

}

}

void tright(int x)

{

clearEncoders();

while(SensorValue[leftEncoder] < x && SensorValue[rightEncoder] <x)

{

motor[leftMotor] = 60;

motor[rightMotor] = -40;

}

}

void backwards(int x)

{

clearEncoders();

while(SensorValue[leftEncoder] < x && SensorValue[rightEncoder] <x)

{

motor[leftMotor] = -40;

motor[rightMotor] = -40;

}

}

void stop(int x)

{

clearEncoders();

while(SensorValue[leftEncoder] < x && SensorValue[rightEncoder] <x)

{

motor[leftMotor] = 0;

motor[rightMotor] = 0;

}

}

• Picture of avoiding walls

• Autonomous Robot  going towards a can
• Code

#pragma config(Sensor, in2,    rightEncoder,   sensorRotation)

#pragma config(Sensor, in3,    leftEncoder,    sensorRotation)

#pragma config(Sensor, in9,    sonarSensor,    sensorSONAR, int1)

#pragma config(Motor,  port2,           rightMotor,    tmotorServoContinuousRotation, openLoop, reversed)

#pragma config(Motor,  port3,           leftMotor,     tmotorServoContinuousRotation, openLoop)

void clearEncoders();

void forward(int x);

void tleft(int x);

void tright(int x);

void backwards(int x);

void stop(int x);

task main()

{

wait1Msec(2000);

bMotorReflected[port2] = true;

while(true)

{

if (SensorValue[sonarSensor]>20 ||SensorValue[sonarSensor]==-1)//loop until sonar finds an object <20 inches.

{

tright(20);

stop(10);

}

else //when if statement is fufilled it will stop turning right

{

forward(20);

stop(10);

if (SensorValue[sonarSensor]<5) //when these it is less than 5 inches from can it will stop going forward

{

break; //kills the continuos loop

}

}

}

}

void clearEncoders()

{

SensorValue[leftEncoder] = 0;

SensorValue[rightEncoder] = 0;

}

void forward(int x)

{

clearEncoders();

while(SensorValue[leftEncoder] < x && SensorValue[rightEncoder] <x)

{

motor[leftMotor] = 50;

motor[rightMotor] = 40;

}

}

void tleft(int x)

{

clearEncoders();

while(SensorValue[leftEncoder] < x && SensorValue[rightEncoder] <x)

{

motor[leftMotor] = -40;

motor[rightMotor] = 40;

}

}

void tright(int x)

{

clearEncoders();

while(SensorValue[leftEncoder] < x && SensorValue[rightEncoder] <x)

{

motor[leftMotor] = 40;

motor[rightMotor] = -30;

}

}

void backwards(int x)

{

clearEncoders();

while(SensorValue[leftEncoder] < x && SensorValue[rightEncoder] <x)

{

motor[leftMotor] = -40;

motor[rightMotor] = -40;

}

}

void stop(int x)

{

while(SensorValue[leftEncoder] < x && SensorValue[rightEncoder] <x)

{

motor[leftMotor] = 0;

motor[rightMotor] = 0;

}

}

• Picture