You are on page 1of 2

#pragma config(Sensor, S4, sonar, sensorEV3_Ultrasonic)

#pragma config(Motor, motorB, right, tmotorEV3_Large, PIDContr


ol, driveRight, encoder)
#pragma config(Motor, motorC, left, tmotorEV3_Large, PIDContr
ol, driveLeft, encoder)
#pragma config(Motor, motorD, ZacharyRamras, tmotorEV3_Medium, PIDCont
rol, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard
!!*//
void avoid_obstacle(); //These are prototypes. We haven't covered them yet in
class
void turn(int x);
int SPEED = 30;
int TURN = 175;
int STEP = 175;
int OBSTACLE_SENSE = 15; //these constants are set for my robot. Yours may be
different.
int AVOIDANCE_RADIUS = 1300;
void avoid_obstacle() //this avoids the obstacle by making a circle around it
. Values work for my
{ //but may need to be changed for yours.
if (getUSDistance(sonar) < OBSTACLE_SENSE)
{
int t_left = 0;
turn(t_left);
resetMotorEncoder(left);
while(getMotorEncoder(left) < AVOIDANCE_RADIUS)
{
setMotorSpeed(right, 60);
setMotorSpeed(left, 40);
}
turn (left);
}
}
void forward()
{
resetMotorEncoder(right);
while(getMotorEncoder(right)<4200)
{
avoid_obstacle(); //called suring forward routine wher
e you are most
setMotorSpeed(right, 50); //to encounter an obstacle
setMotorSpeed(left, 50);
}
}
void stopRobot()
{
setMotorSpeed(right, 0);
setMotorSpeed(left, 0);
wait1Msec(500);
}
void turn(int x)
{
resetMotorEncoder(right);
resetMotorEncoder(left);
if(x==1)
{
while(getMotorEncoder(right)<TURN)
{
setMotorSpeed(right, SPEED);
setMotorSpeed(left, -SPEED);
}
}else{
while(getMotorEncoder(left)<TURN)
{
setMotorSpeed(right, -SPEED);
setMotorSpeed(left, SPEED);
}
}
stopRobot();
}
void increment()
{
resetMotorEncoder(right);
while(getMotorEncoder(right)<STEP)
{
setMotorSpeed(right, SPEED);
setMotorSpeed(left, SPEED);
}
stopRobot();
}
task main()
{
setMotorSpeed(ZacharyRamras, 20);
int turn_dir = 1;
for (int i=0; i<8; i++)
{
forward();
turn(turn_dir);
increment();
turn(turn_dir);
turn_dir = -turn_dir;
}
}

You might also like