I am not sure if i am doing this right. I am trying to get The Sonar sense to work while the Autonomous program is working. What the goal is Let the Program run on autonomous Then when We get near a wall it turns or backs up a lil and stops there. Code:
#pragma config(Hubs, S1, HTMotor, HTServo, HTMotor, none)
#pragma config(Sensor, S2, sonarSensor, sensorSONAR)
#pragma config(Motor, mtr_S1_C1_1, motorD, tmotorNormal, openLoop, reversed)
#pragma config(Motor, mtr_S1_C1_2, motorE, tmotorNormal, openLoop)
#pragma config(Motor, mtr_S1_C3_1, motorF, tmotorNormal, openLoop)
#pragma config(Motor, mtr_S1_C3_2, motorG, tmotorNormal, openLoop)
#pragma config(Servo, srvo_S1_C2_1, , tServoStandard)
#pragma config(Servo, srvo_S1_C2_6, , tServoContinuousRotation)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
/////////////////////////////////////////////////////////////////////////////////////////////////////
//
// Autonomous Mode Code Template
//
// This file contains a template for simplified creation of an autonomous program for an Tetrix robot
// competition.
//
// You need to customize two functions with code unique to your specific robot.
//
/////////////////////////////////////////////////////////////////////////////////////////////////////
#include "JoystickDriver.c" //Include file to "handle" the Bluetooth messages.
/////////////////////////////////////////////////////////////////////////////////////////////////////
//
// initializeRobot
//
// Prior to the start of autonomous mode, you may want to perform some initialization on your robot.
// Things that might be performed during initialization include:
// 1. Move motors and servos to a preset position.
// 2. Some sensor types take a short while to reach stable values during which time it is best that
// robot is not moving. For example, gyro sensor needs a few seconds to obtain the background
// "bias" value.
//
// In many cases, you may not have to add any code to this function and it will remain "empty".
//
/////////////////////////////////////////////////////////////////////////////////////////////////////
void initializeRobot()
{
servo[servo2] = 240; // Place code here to sinitialize servos to starting positions.
// Sensors are automatically configured and setup by ROBOTC. They may need a brief time to stabilize.
servo[servo1] = 200;
return;
}
/////////////////////////////////////////////////////////////////////////////////////////////////////
//
// Main Task
//
// The following is the main code for the autonomous robot operation. Customize as appropriate for
// your specific robot.
//
// The types of things you might do during the autonomous phase (for the 2008-9 FTC competition)
// are:
//
// 1. Have the robot follow a line on the game field until it reaches one of the puck storage
// areas.
// 2. Load pucks into the robot from the storage bin.
// 3. Stop the robot and wait for autonomous phase to end.
//
// This simple template does nothing except play a periodic tone every few seconds.
//
// At the end of the autonomous period, the FMS will autonmatically abort (stop) execution of the program.
//
/////////////////////////////////////////////////////////////////////////////////////////////////////
task main()
{
initializeRobot();
waitForStart(); // Wait for the beginning of autonomous phase.
while(true)
{
if(SensorValue(sonarSensor)>15)
{motor[motorD] = 100;
motor[motorE] = 100;
wait1Msec(1000);
}
{motor[motorD] = -60;
motor[motorE] = 60;
wait1Msec(500);
}
{motor[motorD] = 50;
motor[motorE] = 50;
wait1Msec(2000);
}
{motor[motorD] = -80;
motor[motorE] = 80;
wait1Msec(1000);
}
{motor[motorD] = 80;
motor[motorE] = 80;
wait1Msec(5500);
}
{motor[motorD] = 0; //Stop 'rightmotor'.
motor[motorE] = 0; //Stop 'leftmotor'.
}
}
}
If you can help us, Please try

. Thanks
