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; }