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;
}
}
task main(){
HTGYROstartCal(Gyro);
motor[leftWheel] = 100;
motor[rightWheel] = 100;
wait10Msec(20);
motor[leftWheel] = 0;
motor[rightWheel] = 0;
wait10Msec(100);
while(turnRight(40)){
motor[leftWheel] = -100;
motor[rightWheel] = 100;
}
motor[leftWheel] = 0;
motor[rightWheel] = 0;
wait10Msec(100);
while(SensorValue[ultraSonic] > 30){
motor[leftWheel] = 100;
motor[rightWheel] = 100;
}
motor[leftWheel] = 0;
motor[rightWheel] = 0;
}
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;
}
void initializeRobot() {
bDisplayDiagnostics=false;
setGyroPos(false);
if (HTSMUXreadPowerStatus(MUX)) { // Multiplexer is off or dead?
nxtDisplayBigStringAt(0,45," MUX ");
nxtDisplayBigStringAt(0,25,"Dead Batt");
PlayImmediateTone(440, 50);
wait1Msec(1500);
eraseDisplay();
fallbackMode=true;
selection[SONAR_MENU]=3;
}
if (SensorRaw[gyro]>=640 || SensorRaw[gyro]<=590) { // Gyro moving or disconnected
nxtDisplayBigStringAt(21,45,"Gyro");
nxtDisplayBigStringAt(15,25,"Error");
nxtDisplayCenteredTextLine(7, "%i", SensorRaw[gyro]);
PlayImmediateTone(440, 50);
wait1Msec(1500);
setGyroEnabled(false);
} else {
wait1Msec(100);
nxtDisplayBigStringAt(15,45,"Wait...");
HTGYROstartCal(gyro);
PlaySound(soundBeepBeep);
setGyroEnabled(true);
}
eraseDisplay();
setClamp(true);
initMenus();
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 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
}
void initSweg() {
//Stop moving.
motor[left] = 0;
motor[right] = 0;
//Stand up.
servo[rearFlipper] = REAR_LIFT_DEPRESSED;
servo[frontFlipper] = FRONT_LIFT_DEPRESSED;
//Wait for it to stand all the way up.
wait1Msec(2700);
//Recalibrate the gyro.
StopTask(gyros);
PlaySound(soundBeepBeep);//"Starting calibration!"
HTGYROstartCal(forwardsTilt);
wait1Msec(1000);//Give it time to calibrate
PlaySound(soundBeepBeep);//"Done calibrating!"
//K, finished with gyro calibration. Start measuring angle now.
StartTask(gyros);
//Not sure what these two lines are for.
nMotorEncoder[left] = 0;
nMotorEncoder[right] = 0;
}
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;
}
}
}
void Gyro_Start()
{
float rotSpeed = 0;
float heading = 0;
HTGYROstartCal(Gyro);
time1[T1] = 0;
StartTask(UpdateGyro);
}
//Initialization: don't touch
void initializeRobot() {
HTGYROstartCal(gyro); //Gyro calibration
distanceCalibration(); //Calculating correction constants
initializeAssignments(); //Filling assignment array
HTACreadAllAxes(HTAC, xcal, ycal, zcal); //Calibrating accelerometer
return;
}
void init(){
servo[fieldGrabberLeft] = _open;
servo[fieldGrabberRight] = 255-_open;
servo[scoopBridge] = 155;
nMotorEncoder[intake] = 0;
sticksUp();
retainAutoBall();
HTGYROstartCal(HTGYRO);
}
task update_gyro ()
{
HTGYROstartCal(Gyro);
while(true)
{
float angle=HTGYROreadRot(Gyro)/100.0;
happy_angle=happy_angle+angle;
wait1Msec (10);
}
}
void initialize()
{
HTGYROstartCal(S3);
servo[servo1] = 0;
//servo[servo2] = 10;
servo[servo3] = 250;
servo[servo4] = 139;
servo[servo5] = 75;
servo[servo2] = 0;
}
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()
{
HTGYROstartCal(HTGYRO);
nMotorEncoder[motorC] = 0;
servo[wrist] = 245;
servo[shoulder] = 255;
servo[right_servo] = 0;
servo[left_servo] = 180;
return;
}
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 main()
{
//Wait for the start command//
waitForStart();
//Start getHeading.h//
HTGYROstartCal(GYRO);
StartTask(getHeading);
//Move forward until it sees the beacon or until 5 sec is up//
motor[motorD] = -50;
motor[motorE] = -50;
time1[T3] = 0;
while (atBeacon() == false && time1[T3] < 5000) {
wait1Msec(2);
}
//Turn 90 to the right//
TurnAngle(90, false);
///////////////////////
//Move forward for 1 sec//
motor[motorD] = 80;
motor[motorE] = 80;
wait1Msec(1000);
//Stop motors//
motor[motorD] = 0;
motor[motorE] = 0;
//Move the block belt for 2.5 sec//
motor[motorC] = -100;
wait1Msec(2500);
motor[motorC] = 0;
//Move backward for 1 sec//
motor[motorD] = -50;
motor[motorE] = -50;
wait1Msec(1000);
motor[motorD] = 0;
motor[motorE] = 0;
}
void initializeRobot()
{
disableDiagnosticsDisplay();
RequestedScreen=S_MISSION; // display mission selection screen at startup
StartTask(buttons); // start user interface = buttons
StartTask(display); // start user interface - display
StartTask(sensors); // sensors task runs continuously
StartTask(datalog); // data log for IR sensors runs continuously
moveLightServo(UP);
wait1Msec(100); // give some stabilizing time before doing GYRO cal
HTGYROstartCal(HTGYRO);
alive();
return;
}
// 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);
}
}
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 initializeRobot()
{
HTGYROstartCal(HTGYRO);
LSsetActive(lineFollower);
setWrist (.95);
while (SensorValue [liftDownSensor] == 0) motor [linearSlides]=-100;
motor [linearSlides]=0;
wait1Msec (500);
// 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.
return;
}
void initializeRobot()
{
disableDiagnosticsDisplay();
RequestedScreen=S_MISSION; // display mission selection screen at startup
StartTask(buttons); // start user interface = buttons
StartTask(display); // start user interface - display
StartTask(sensors); // sensors task runs continuously
motor[lightMotor] = 50; // drop the light sensoor
wait1Msec(500); // wait for it to move down
motor[lightMotor] = 0; // and shut off the motor
wait1Msec(100); // give some stabilizing time before doing GYRO cal
HTGYROstartCal(HTGYRO);
alive();
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);
}
}
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);
}
}
void initializeRobot()
{
//resetHang(); //hang arm
//resetBucket(); //DO NOT reset the NXT motors!!
//reset drive train encoders
nMotorEncoder[leftDrive] = 0;
nMotorEncoder[rightDrive] = 0;
HTGYROstartCal(gyroSensor); //calibrate gyro
StartTask(Arm);
//beep to signal end of initialization
PlayTone(440, 30);
return;
}
task main()
{
HTGYROstartCal(GYRO);
StartTask(getHeading);
//Drive forword for 5 sec//
motor[motorD] = 100;
motor[motorE] = 100;
wait1Msec(5000);
//Stop motors//
motor[motorD] = 0;
motor[motorE] = 0;
//Turn right 90 degrees//
TurnAngle(90, 100);
//Move forwar for 2.5 sec//
motor[motorD] = 100;
motor[motorE] = 100;
wait1Msec(2500);
//Stop motors//
motor[motorD] = 0;
motor[motorE] = 0;
//Turn right 90 degrees//
TurnAngle(90, 100);
//Move forward for 2.2 sec//
motor[motorD] = 100;
motor[motorE] = 100;
wait1Msec(2200);
//Stop motors//
motor[motorD] = 0;
motor[motorE] = 0;
}
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);
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(HTGYRO));
PlaySound(soundBlip);
while(bSoundActive) EndTimeSlice();
time1[T1] = 0;
}
while(nNxtButtonPressed != kEnterButton) {
eraseDisplay();
nxtDisplayTextLine(1, "Reading");
// Read the current calibration offset and display it
nxtDisplayTextLine(2, "Offset: %4d", HTGYROreadCal(HTGYRO));
nxtDisplayClearTextLine(4);
// Read the current rotational speed and display it
nxtDisplayTextLine(4, "Gyro: %4d", HTGYROreadRot(HTGYRO));
nxtDisplayTextLine(6, "Press enter");
nxtDisplayTextLine(7, "to recalibrate");
wait1Msec(100);
}
}
}