void Turn(int power, long distance)
{
writeDebugStreamLine("new *****");
//writeDebugStreamLine("value: %d", SensorValue[Gyro]);
//int zero = initialize() + 59;//59 is added to perfectly 0 it.
long sum = 0;
//writeDebugStreamLine("delta: %d", SensorValue[Gyro] - zero);// begining delta is 59 +-1 often
//writeDebugStreamLine("zero: %d", zero);
//writeDebugStreamLine("value: %d", SensorValue[Gyro]);
//nMotorEncoder[motorB] = 0;
motor[motorD] = power;
motor[motorE] = -1 * power;
while( abs(sum) < abs(distance))// +-8400 is 90 degrees
{
//writeDebugStreamLine("%d", SensorValue[Gyro] - zero);
sum = sum + deadband(HTGYROreadRot(HTGYRO), 5);
wait1Msec(10);// update interval
writeDebugStreamLine("sum: %d", sum);
writeDebugStreamLine("Gyro: %4d", HTGYROreadRot(HTGYRO));
nxtDisplayTextLine(4, "Gyro: %4d", HTGYROreadRot(HTGYRO));
}
//writeDebugStreamLine("sum: %d", sum);
motor[motorD] = 0;
motor[motorE] = 0;
wait10Msec(1); // just to let the encoders reset when the robot is still
nMotorEncoder[motorE] = 0;
nMotorEncoder[motorD] = 0;
}
void turnTowardsRobot() {
//int tmpUS = USScanVal;
time1[T2] = 0;
while(searchingForBot) {
while (USScanVal > 92 ) {
wait1Msec(20);
if (abs(rotSpeed) > 3) {
rotSpeed = HTGYROreadRot(HTGYRO);//find gyro rotation speed
heading += (rotSpeed * 0.02) / 0.3;//find gyro heading in degrees??
}
//PlayTone(400, 500);
motor[motorLeft] = -5;
motor[motorRight] = 5;
if (heading >= USScanVal) {
motor[motorLeft] = 0;
motor[motorRight] = 0;
searchingForBot = false;
}//end if
break;
}//end while
while (USScanVal < 92) {
wait1Msec(20);
if (abs(rotSpeed) > 3) {
rotSpeed = HTGYROreadRot(HTGYRO);//find gyro rotation speed
heading += (rotSpeed * 0.02);//find gyro heading in degrees??
}
//PlayTone(400, 500);
motor[motorLeft] = 5;
motor[motorRight] = -5;
if (heading <= USScanVal) {
motor[motorLeft] = 0;
motor[motorRight] = 0;
searchingForBot = false;
}//end if
break;
}//end while
if (USScanVal == 92) {
//do nothing
break;
}
}//end search
}//end void
void turnTowardsCenter() {
if (turnedLeft) {
while(heading > 92) {
wait1Msec(20);
if (abs(rotSpeed) > 3) {
rotSpeed = HTGYROreadRot(HTGYRO);//find gyro rotation speed
heading += (rotSpeed * 0.02) * 0.7;//find gyro heading in degrees??
}
motor[motorLeft] = 5;
motor[motorRight] = -5;
if (heading <= 92) {
motor[motorLeft] = 0;
motor[motorRight] = 0;
break;
}
}
turnedLeft = false;
turnedRight = false;
}
if (turnedRight) {
while(heading < 92) {
wait1Msec(20);
if (abs(rotSpeed) > 3) {
rotSpeed = HTGYROreadRot(HTGYRO);//find gyro rotation speed
heading += (rotSpeed * 0.02) / 0.3;//find gyro heading in degrees??
}
motor[motorLeft] = -5;
motor[motorRight] = 5;
if (heading >= 92) {
motor[motorLeft] = 0;
motor[motorRight] = 0;
break;
}
}
turnedLeft = false;
turnedRight = false;
}
}
task main ()
{
float rotSpeed = 0;
float heading = 0;
// Calibrate the gyro, make sure you hold the sensor still
HTGYROstartCal(HTGYRO);
// Reset the timer.
time1[T1] = 0;
while (true)
{
// Wait until 20ms has passed
while (time1[T1] < 20)
wait1Msec(1);
// Reset the timer
time1[T1]=0;
// Read the current rotation speed
rotSpeed = HTGYROreadRot(HTGYRO);
// Calculate the new heading by adding the amount of degrees
// we've turned in the last 20ms
// If our current rate of rotation is 100 degrees/second,
// then we will have turned 100 * (20/1000) = 2 degrees since
// the last time we measured.
heading += rotSpeed * 0.02;
// Display our current heading on the screen
nxtDisplayCenteredBigTextLine(3, "%2.0f", heading);
}
}
void gyroTurn(float power, float fDegrees, eDirection direct)
{//Robot turns at a specific power, a certain angle, either right or left
HTGYROstartCal(gyro); //Activate gyroscope
wait1Msec(100); //Let it adjust
float fCurrent = 0.0; //Set heading to 0 degrees
float fRotSpeed = 0.0; //Current rotational speed is 0
if (direct == dLeft) //If told to turn left
{
leftTurn(power); //Set motors to turn left
}
else //If tol to turn right
{
rightTurn(power); //Set motors to turn right
}
do //Loop through following until conditions are met
{
wait1Msec(MEASUREMENT_MS); //Give a moment to recalibrate
fRotSpeed = HTGYROreadRot(gyro); //Rotation speed is now the gyro's input
fCurrent += fRotSpeed * (MEASUREMENT_MS / 1000.0); //Current heading is what is was before
//plus the value of the rate of turn times the amount of time that rate value was taken
} while (abs(fCurrent) < fDegrees); //Repeat until the heading is the amount told to turn
stopMotors(); //You got there, now stop
}
//------------
//=================================================
// Task to handle the gyro and the other sensors hooked up to the sensor mux
//=================================================
task gyro()
{
float currDir = 0.0; float prevDir = 0.0;
long currtime,prevtime;
ir_mux_status=HTSMUXreadPowerStatus(IR_MUX); // read the sensor multiplexor status
gyro_mux_status=HTSMUXreadPowerStatus(GYRO_MUX); // read the sensor multiplexor status
while (ir_mux_status || gyro_mux_status) // check good battery power on both muxes
{
//PlayTone(750,25);
wait1Msec(500);
}
wait1Msec(300);
for (int i=0;i<5;i++) // check if there is too much spread in the data
{
if (gyro_noise>10)
{
PlayTone (250,25);
wait1Msec(500);
}
}
calibrate = 2;
prevtime = nPgmTime;
while(true)
{
currtime=nPgmTime;
rawgyro = (float)HTGYROreadRot(HTGYRO);
constHeading += (rawgyro - drift) * (float)(currtime-prevtime)/1000;
relHeading += (rawgyro - drift) * (float)(currtime-prevtime)/1000;
prevtime = currtime;
wait1Msec(1);
prevDir = currDir;
}
}
void initializeRobot()
{
// Place code here to initialize servos to starting positions.
// Sensors are automatically configured and setup by ROBOTC. They may need a brief time to stabilize.
nMotorPIDSpeedCtrl[wristMotor] = mtrSpeedReg;
nMotorEncoder[wristMotor] = 0; //Resets Encoder Values
//servoChangeRate[wrist] = 2; // Slows the Change Rate of wrist to 2 positions per update. Default = 10
servo[wrist] = wristPosition;
servo[elbow] = elbowPosition;
servo[trapperL] = trapperPosition;
servo[trapperR] = 256 - trapperPosition;
servo[tipperL] = tipperPosition;
servo[tipperR] = 256 - tipperPosition;
Gyro_offset = HTGYROstartCal(HTGYRO); //start calibration, but do not trust results
for (int i = 0 ; i < 10; i++) //instead read 10 values and generate average offset
{
sum = sum + HTGYROreadRot(HTGYRO);
wait1Msec(50);
}
Gyro_offset = sum / 10.0;
return;
}
task main () {
nxtDisplayCenteredTextLine(0, "HiTechnic");
nxtDisplayCenteredBigTextLine(1, "GYRO");
nxtDisplayCenteredTextLine(3, "SMUX Test");
nxtDisplayCenteredTextLine(5, "Connect SMUX to");
nxtDisplayCenteredTextLine(6, "S1 and sensor to");
nxtDisplayCenteredTextLine(7, "SMUX Port 1");
wait1Msec(2000);
nxtDisplayCenteredTextLine(5, "Press enter");
nxtDisplayCenteredTextLine(6, "to set relative");
nxtDisplayCenteredTextLine(7, "heading");
wait1Msec(2000);
// Before using the SMUX, you need to initialise the driver
HTSMUXinit();
// Tell the SMUX to scan its ports for connected sensors
HTSMUXscanPorts(HTSMUX);
// The sensor is connected to the first port
// of the SMUX which is connected to the NXT port S1.
// To access that sensor, we must use msensor_S1_1. If the sensor
// were connected to 3rd port of the SMUX connected to the NXT port S4,
// we would use msensor_S4_3
wait1Msec(2000);
eraseDisplay();
time1[T1] = 0;
while(true) {
if (time1[T1] > 1000) {
eraseDisplay();
nxtDisplayTextLine(1, "Resetting");
nxtDisplayTextLine(1, "heading");
wait1Msec(500);
// Start the calibration and display the offset
nxtDisplayTextLine(2, "Offset: %4d", HTGYROstartCal(msensor_S1_1));
PlaySound(soundBlip);
while(bSoundActive);
time1[T1] = 0;
}
while(nNxtButtonPressed != kEnterButton) {
eraseDisplay();
nxtDisplayTextLine(1, "Reading");
// Read the current calibration offset and display it
nxtDisplayTextLine(2, "Offset: %4d", HTGYROreadCal(msensor_S1_1));
nxtDisplayClearTextLine(4);
// Read the current rotational speed and display it
nxtDisplayTextLine(4, "Gyro: %4d", HTGYROreadRot(msensor_S1_1));
nxtDisplayTextLine(6, "Press enter");
nxtDisplayTextLine(7, "to recalibrate");
wait1Msec(100);
}
}
}
task keepHeading(){
float rotSpeed = 0;
// Calibrate the gyro, make sure you hold the sensor still
while(true){
while (time1[T3] < 20)
wait1Msec(1);
// Reset the timer
time1[T3]=0;
// Read the current rotation speed
rotSpeed = HTGYROreadRot(HTGYRO);
// Calculate the new heading by adding the amount of degrees
// we've turned in the last 20ms
// If our current rate of rotation is 100 degrees/second,
// then we will have turned 100 * (20/1000) = 2 degrees since
// the last time we measured.
globalHeading += rotSpeed * 0.02;
nxtDisplayCenteredTextLine(1,"GH:%f",globalHeading);
wait1Msec(1);
if(globalHeading>180)
globalHeading = -180;
else if(globalHeading<-180)
globalHeading = 180;
}
}
void turn(int direction,int degrees,int powerLevel){//Dir:1 = right, 0 = left::Degrees
//Accurately turn with GYRO
float rotSpeed = 0;
float heading = 0;
int slow = powerLevel/2;
// Calibrate the gyro, make sure you hold the sensor still
HTGYROstartCal(HTGYRO);
while(true){
while (time1[T1] < 20)
wait1Msec(1);
// Reset the timer
time1[T1]=0;
// Read the current rotation speed
rotSpeed = HTGYROreadRot(HTGYRO);
// Calculate the new heading by adding the amount of degrees
// we've turned in the last 20ms
// If our current rate of rotation is 100 degrees/second,
// then we will have turned 100 * (20/1000) = 2 degrees since
// the last time we measured.
heading += rotSpeed * 0.02;
if(heading>degrees*3.0/4.0)
powerLevel=slow;
if(direction==1)
right(powerLevel);
else if(direction==0)
left(powerLevel);
if(abs(heading)>degrees)
break;
}
}
void driveSonar(int direction,float distance,int powerLevel){//Dir:distance(centimeters):1 = reverse, 0 = forward:
//Drive in a straight line GYRO
float rotSpeed = 0;
float heading = 0;
int dir = direction==0?1:-1;
// Calibrate the gyro, make sure you hold the sensor still
HTGYROstartCal(HTGYRO);
time1[T2] = 0;
while(SensorValue[sonarSensor] > distance){
while (time1[T1] < 20)
wait1Msec(1);
// Reset the timer
time1[T1]=0;
// Read the current rotation speed
rotSpeed = HTGYROreadRot(HTGYRO);
// Calculate the new heading by adding the amount of degrees
// we've turned in the last 20ms
// If our current rate of rotation is 100 degrees/second,
// then we will have turned 100 * (20/1000) = 2 degrees since
// the last time we measured.
heading += rotSpeed * 0.02;
if(direction==0){
motor[driveLeft] = (powerLevel+powerLevel*.15*heading)*dir;
motor[driveRight] = -(powerLevel-powerLevel*.15*heading)*dir;
}else{
motor[driveLeft] = (powerLevel-powerLevel*.15*heading)*dir;
motor[driveRight] = -(powerLevel+powerLevel*.15*heading)*dir;
}
}
}
task update_gyro ()
{
HTGYROstartCal(Gyro);
while(true)
{
float angle=HTGYROreadRot(Gyro)/100.0;
happy_angle=happy_angle+angle;
wait1Msec (10);
}
}
void turnTask(float turnDegrees, int motorSpeed)
{
float rotSpeed = 0;
float heading = 0;
// Calibrate the gyro, make sure you hold the sensor still
HTGYROstartCal(HTGYRO);
// Get current program time in milliseconds.
int lastPgmTime = nPgmTime;
int currentPgmTime = nPgmTime;
// Begin moving robot
if (turnDegrees > 0)
{
motor[leftMotor] = motorSpeed;
motor[rightMotor] = motorSpeed * -1;
} else
{
motor[rightMotor] = motorSpeed;
motor[leftMotor] = motorSpeed * -1;
}
while (abs(heading) < abs(turnDegrees))
{
// Read the current rotation speed
if (lastPgmTime + 20 < nPgmTime)
{
currentPgmTime = nPgmTime;
rotSpeed = HTGYROreadRot(HTGYRO);
// Calculate the new heading by adding the amount of degrees
// we've turned in the last 20ms
// If our current rate of rotation is 100 degrees/second,
// then we will have turned 100 * (20/1000) = 2 degrees since
// the last time we measured.
heading += rotSpeed * (((float)currentPgmTime - (float)lastPgmTime) / 1000);
lastPgmTime = currentPgmTime;
// Display our current heading on the screen
nxtDisplayCenteredBigTextLine(3, "%3.2f", heading);
}
}
// stop motors
motor[leftMotor] = 0;
motor[rightMotor] = 0;
}
void initializeRobot()
{
// Finds average base gyro reading
for(int i = 0; i < 100; i++)
{ //sums up first hundred gyro readings
initial += HTGYROreadRot(HTGYRO);
wait10Msec(1);
}
initial = initial / 100; //divides by 100 to find the average reading
return;
}
task main()
{
initializeRobot();
while(true)
{
Gyro_value = HTGYROreadRot(HTGYRO);
wait1Msec(10);
}
}
task gyroTask()
{
float lastTime = nPgmTime;
while(1)
{
gyroData.degsec = HTGYROreadRot(GYRO);
gyroData.deg += dbc(gyroData.degsec,1)*((nPgmTime-lastTime)/1000);
lastTime = nPgmTime;
wait1Msec(1);
}
}
task update_gyroSensor()
{
HTGYROstartCal(gyroSensor);
while(true)
{
float angle=HTGYROreadRot(gyroSensor)/100.0;
happy_angle=happy_angle+angle;
nxtDisplayTextLine(7, "%d", happy_angle);
wait1Msec (10);
}
}
void initializeRobot()
{
Gyro_offset = HTGYROstartCal(HTGYRO); //start calibration, but do not trust results
for (int i = 0 ; i < 20; i++) //instead read 20 values and generate average offset
{
sum = sum + HTGYROreadRot(HTGYRO);
wait1Msec(10);
}
Gyro_offset = sum / 20.0;
return;
}
task gyros() {//Updates the forwardsAngle global variable (degrees) with the current deviation from 90 degrees.
nSchedulePriority = kHighPriority;//This exemplifies my hate of ROBOTC, but it pretty much just sets it to high CPU priority.
INTR intr;//Integrator.
initIntr(intr);
forwardsAngle = 0;
while(true) {//Constantly integrating gyro rotational velocity reading in degrees/sec to get current heading in degrees.
forwardsAngle = integrate(intr, HTGYROreadRot(forwardsTilt));
wait1Msec(5);
}
}
/////////////////////////////////////////////////////////////////////////////////////////////////////
//
// GyroTurn
//
// This is a generic movement routine that turns the robot a specific number of degrees using a gyro
// sensor to measure angular velocity
//
/////////////////////////////////////////////////////////////////////////////////////////////////////
void GyroTurn(int iSpeed, int iDegrees, driveState iCmd)
{
float vcurrposition = 0;
int vprevtime = nPgmTime;
int vcurrtime;
float vcurrRate;
int voffset;
float deltaSecs;
float degChange;
if (iCmd == LEFT)
{
motor[DriveLeft] = -1 * iSpeed;
motor[DriveRight] = iSpeed;
voffset = -1;
}
else
{
motor[DriveLeft] = iSpeed;
motor[DriveRight] = -1 * iSpeed;
voffset = 1;
}
while (vcurrposition < iDegrees)
{
// This tells us the current rate of rotation in degrees per
// second.
vcurrRate = HTGYROreadRot(gyro) * voffset;
// How much time has elapsed since we last checked, which we use
// to determine how far we've turned
vcurrtime = nPgmTime;
deltaSecs = (vcurrtime - vprevtime) / 1000.0;
if (deltaSecs < 0)
{
deltaSecs = ((float)((vcurrtime + 1024) - (vprevtime + 1024))) / 1000.0;
}
// Calculate how many degrees the heading changed.
degChange = vcurrRate * deltaSecs;
vcurrposition = vcurrposition + degChange;
vprevtime = vcurrtime;
}
motor[DriveLeft] = 0;
motor[DriveRight] = 0;
//The following line is used to pause the robot in between movements
wait1Msec(100);
}
void turn(int iSpeed, int iDegrees, int code)
{
float vcurrposition = 0;
int vprevtime = nPgmTime;
int vcurrtime;
float vcurrRate;
int voffset;
float deltaSecs;
float degChange;
if (code == L_CODE)
{
motor[mLeft] = iSpeed;
motor[mRight] = -iSpeed;
voffset = 1;
}
else if(code == R_CODE)
{
motor[mLeft] = -iSpeed;
motor[mRight] = iSpeed;
voffset = -1;
}
while (vcurrposition < iDegrees)
{
// This tells us the current rate of rotation in degrees per
// second.
vcurrRate = HTGYROreadRot(gyro) * voffset;
// How much time has elapsed since we last checked, which we use
// to determine how far we've turned
vcurrtime = nPgmTime;
deltaSecs = (vcurrtime - vprevtime) / 1000.0;
if (deltaSecs < 0)
{
deltaSecs = ((float)((vcurrtime + 1024) - (vprevtime + 1024))) / 1000.0;
}
// Calculate how many degrees the heading changed.
degChange = vcurrRate * deltaSecs;
vcurrposition = vcurrposition + degChange;
vprevtime = vcurrtime;
}
motor[mLeft] = 0;
motor[mRight] = 0;
wait1Msec(100);
}
// This task adds to happy_angle to keep the robots current heading in degrees.
// The if statement is so that the value does not pass 360, in the case you make a 360.
task update_gyroSensor()
{
HTGYROstartCal(gyroSensor);
while(true)
{
float angle = HTGYROreadRot(gyroSensor) / 100.0;
happy_angle = happy_angle + angle;
if (happy_angle == 360)
{
happy_angle = 0;
}
wait1Msec(1);
}
}
task main()
{
initializeRobot();
waitForStart(); // Wait for the beginning of autonomous phase.
///////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////
//// ////
//// Add your robot specific autonomous code here. ////
//// ////
///////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////
while (true)
{
PlaySound(soundBlip); // Play the sound, 'soundBeepBeep'.
Gyro_value=HTGYROreadRot(HTGYRO); //read the gyro sensor
IR_value=HTDIRreadDCDir(S3);
for(int i = 0; i < 200; i++)
{
motor[leftMotor] = 50; // Travel straight
motor[rightMotor] = 50;
wait1Msec(50);
}
// Wait for 2.5 seconds
motor[leftMotor] = 0; // Stop
motor[rightMotor] = 0;
for (int i = 0; i < 200; i++) // Start looking for IR beacon
{
IR_value = HTDIRreadDCDir(S3);
IR_offset = IR_value - 5; // 5 is center value
motor[leftMotor] = 50 - 5*IR_offset;
motor[rightMotor] = 50 + 5*IR_offset;
wait1Msec(50); // Wait before looping again.
}
motor[leftMotor] = 0; // Stop
motor[rightMotor] = 0;
PlaySound(soundBlip); // Play the sound, 'soundBeepBeep'.
while(true) {wait1Msec(1);} // Loop forever doing nothing
}
}
void initializeRobot()
{
// 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.
Gyro_offset = HTGYROstartCal(HTGYRO); //start calibration, but do not trust results
for (int i = 0 ; i < 10; i++) //instead read 10 values and generate average offset
{
sum = sum + HTGYROreadRot(HTGYRO);
wait1Msec(50);
}
Gyro_offset = sum / 10.0;
return;
}
void turn(int angle, short speed) // Steven Mostovoy
{
if (angle > 0)
angle += 65;
else
angle -= 65;
if (angle < 0)
{
angle *= -1;
speed *= -1;
}
wait1Msec(200);
HTGYROstartCal(S2);
wait1Msec(200);
int gyro[3] = {0,0,0};
int gyroMid[3] = {0,0,0};
int gyroFinal[4] = {0,0,0,0};
motor[driveLeft] = speed;
motor[driveRight] = -speed;
int integral = 0;
while (true)
{
for (int i = 2; i > 0; --i)
gyro[i] = gyro[i-1];
gyro[0] = HTGYROreadRot(S3);
float mu = 10.0/11.0;
gyroMid[0] = gyro[0];
for (int i = 1; i < 3; ++i)
gyroFinal[i] = mu * (gyro[i] - gyroMid[i-1]) + gyroMid[i-1];
for (int i = 3; i > 0; --i)
gyroFinal[i] = gyroMid[i-1];
gyroFinal[0] = gyroMid[1];
integral += (gyroFinal[0] + gyroFinal[1]*2 + gyroFinal[2]*2 + gyroFinal[3])/6;
//writeDebugStreamLine("%d", integral);
if (abs(integral * 0.0604) > angle) break;
wait1Msec(10);
}
motor[driveLeft] = 0;
motor[driveRight] = 0;
}
task main()
{
int heading = 0, y, x, interval;
HTGYROstartCal(gyro);
while(1)
{
y = nPgmTime;
nxtDisplayTextLine(1, "y: %d", y);
x = HTGYROreadRot(gyro);
nxtDisplayTextLine(3, "%d", x);
wait10Msec(100);
interval = nPgmTime - y;
nxtDisplayTextLine(4, "time: %d", nPgmTime);
nxtDisplayTextLine(5, "Time-y: %f", interval);
wait10Msec(100);
}
}
//====================================
// Gyro Calibration helper function
//====================================
float MyHTCal(long caltime)
{
long highest = -1000, lowest = 10000;
float average = 0;
long starttime = nPgmTime;
long samples=0;
long data;
while (nPgmTime < starttime+caltime) // loop for the requested number of seconds
{
samples +=1; // count the number of iterations for averaging
data = HTGYROreadRot(HTGYRO); // get a new reading from the GYRO
average += (float)data; // add in the new value to the average
if (highest < data) highest = data; // adjust the highest value if necessary
if (lowest> data) lowest = data; // likewise for the lowest value
}
gyro_noise=abs(highest-lowest); // save the spread in the data for diagnostic display
return average/samples; // and return the average drift
}
task Gyro()
{
HTGYROstartCal(sensor_gyro);
float vel_curr = 0.0;
float vel_prev = 0.0;
float dt = 0.0;
int timer_gyro = 0;
Time_ClearTimer(timer_gyro);
while (true) {
vel_prev = vel_curr;
dt = (float)Time_GetTime(timer_gyro)/(float)1000.0; // msec to sec
Time_ClearTimer(timer_gyro);
vel_curr = (float)HTGYROreadRot(sensor_gyro);
heading += ((float)vel_prev+(float)vel_curr)*(float)0.5*(float)dt;
Time_Wait(1);
}
}
//=================================================================
//=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=
float MyHTCal(long caltime)
{
float average = 0;
long starttime = nPgmTime;
long samples=0;
long data;
highest = -10000;
lowest = 10000;
while (nPgmTime < starttime+caltime)
{
samples +=1;
data = HTGYROreadRot(HTGYRO);
average += (float)data;
if (highest < data) highest = data;
if (lowest> data) lowest = data;
}
return average/samples;
}
void turn(int direction) {
int mul = direction;
resetEncoders();
//float degreesToTurn = degrees - (degrees/27);
motor[motorD] = 15 * mul;
motor[motorE] = -15 * mul;
float current_rot = HTGYROreadRot(HTGyro) +14;
float time = time1[T1];
float distance = ((prev_rot+current_rot)/2)*time/1000;
total_distance += distance;
prev_rot = current_rot;
clearTimer(T1);
wait1Msec(10);
motor[motorD] = 0;
motor[motorE] = 0;
}
