Sample Code for working with Sonar Sensor

Here is the sample code for working with the sonar sensor in RobotC

 

#pragma config(Sensor, dgtl2,  sonarSensor,    sensorSONAR_inch)
#pragma config(Motor,  port1,           leftMotor,     tmotorVex393_HBridge, openLoop)
#pragma config(Motor,  port6,           clawMotor,     tmotorVex269_MC29, openLoop, reversed)
#pragma config(Motor,  port7,           armMotor,      tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port10,          rightMotor,    tmotorVex393_HBridge, openLoop)
//*!!Code automatically generated by ‘ROBOTC’ configuration wizard               !!*//

/*—————————————————————————————————-*\
|*                                – Clawbot Single Joystick Control –                                 *|
|*                                      ROBOTC on VEX 2.0 Cortex                                      *|
|*                                                                                                    *|
|*  This program uses a single joystick, either right or left to drive the robot. Use notes below     *|
|*  to reconfigure which joystick is used. The joystick buttons are used to raise and lower the arm.    *|
|*  The joystick buttons are used to open and close the claw.                                                                                    *|
|*                                                                                                                                                                                                        *|
|*                                        ROBOT CONFIGURATION                                         *|
|*    NOTES:                                                                                          *|
|*    1)    Ch1 is the X axis and Ch2 is the Y axis for the RIGHT joystick.                             *|
|*    2)    Ch3 is the Y axis and Ch4 is the X axis for the LEFT joystick.                              *|
|*        3)    Button 5U and 5L are on the front left side of the joystick.                                                                *|
|*        3)    Button 6U and 6L are on the front right side of the joystick.                                                                *|
|*                                                                                                    *|
|*    MOTORS & SENSORS:                                                                               *|
|*    [I/O Port]        [Name]          [Type]                               [Description]            *|
|*    Motor – Port 2   rightMotor    VEX 393 Motor                         Right drive motor          *|
|*    Motor – Port 6   clawMotor     VEX 393 Motor w/ Motor Controler 29   Claw motor                 *|
|*    Motor – Port 7   armMotor      VEX 393 Motor w/ Motor Controler 29   Arm motor                  *|
|*    Motor – Port 10  leftMotor     VEX 393 Motor                         Left drive motor           *|
\*—————————————————————————————————-*/
bool autoMode = false;  //this will control whether or not we are in auto mode
//+++++++++++++++++++++++++++++++++++++++++++++| MAIN |+++++++++++++++++++++++++++++++++++++++++++++++

task main ()
{

while(1 == 1)
{
if (vexRT[Btn8R] == 1){ //that is, if we push the right side button on the controller
autoMode = !autoMode; //this toggles the autoMode.  ! means “not” so “not false” is true and “not true” is false
}
if (autoMode){

int distance = 0;  //create a holder for the distance value
distance = SensorValue[sonarSensor];  //get the distance from the sonic rang efinder
if (distance < 10){  //if it’s less than some value
motor[leftMotor] = 0;  //turn off both motors
motor[rightMotor] = 0;
}
else{
motor[leftMotor] = 127;  //otherwise, turn on both motors.
motor[rightMotor] = 127;\
}//end autoMode
}//end automode
else{  //this means we are in teleoperated mode.
//this part is the sample code
motor[leftMotor]  = (vexRT[Ch2] + vexRT[Ch1])/2;  // (y + x)/2
motor[rightMotor] = (vexRT[Ch2] – vexRT[Ch1])/2;  // (y – x)/2

// Raise, lower or do not move arm
if(vexRT[Btn5U] == 1)           //If button 5U is pressed…
{
motor[armMotor] = 127;        //…raise the arm.
}
else if(vexRT[Btn5D] == 1)      //Else, if button 5D is pressed…
{
motor[armMotor] = -127;       //…lower the arm.
}
else                              //Else (neither button is pressed)…
{
motor[armMotor] = 0;          //…stop the arm.
}

// Open, close or do not more claw
if(vexRT[Btn6U] == 1)           //If Button 6U is pressed…
{
motor[clawMotor] = 127;          //…close the gripper.
}
else if(vexRT[Btn6D] == 1)      //Else, if button 6D is pressed…
{
motor[clawMotor] = -127;         //…open the gripper.
}
else                              //Else (neither button is pressed)…
{
motor[clawMotor] = 0;            //…stop the gripper.
}//end btn6u
}//end teleop mode
}//end while
}//end task main
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++