task main()
{
eraseDisplay();
// Array for accumulating Gaussian values
int accumulator[100];
for (int i = 0; i < 99; i++) {
accumulator[i] = 0;
}
// Infinite loop
while(1)
{
// Get and print uniform-distributed and Gaussian-distributed random numbers
float uniform_float = sampleUniform(1.0);
float gaussian_float = sampleGaussian(0.0, 1.0);
nxtDisplayString(1, "Uniform : %04f", uniform_float);
nxtDisplayString(2, "Gaussian: %04f", gaussian_float);
// Build a histogram of Gaussian numbers and plot on the NXT screen
// Scale random number to screen resolution
int gaussian_int = (int) (gaussian_float * 10.0) + 50;
accumulator[gaussian_int]++;
nxtSetPixel(gaussian_int, accumulator[gaussian_int]);
wait1Msec(10);
}
}
void sndOver(ubyte x)
{
ubyte xmitBuffer[kMaxMsgSize];
TFileIOResult nErrStatus;
memset(xmitBuffer,0, kMaxMsgSize);
nxtDisplayTextLine(3, "Attempt to send %d!", x);
xmitBuffer[0] = x;
// cCmdMessageWriteToBluetooth(x, 1, Queue1); <--- this works too if you really just want to send the byte x
// the following prepare you the structure in case you want to send more than 1 byte
nErrStatus = cCmdMessageWriteToBluetooth(xmitBuffer, 1, Queue1);
switch (nErrStatus){
case ioRsltSuccess:
case ioRsltCommPending:
nxtDisplayTextLine(3, "Send %d Success !", xmitBuffer);
break;
case ioRsltCommChannelBad:
nxtDisplayString(3, "Bad Channel!....");
break;
default:
nxtDisplayString(3, "Send Failed!....");
break;
}
}
//Function that allows navigation to select the order of the ball retrieval
int askForInput(int colourNumber)
{
colourNumber = 0;
bool select = 1;
while (select)
{
if (nNxtButtonPressed == 1)
{
colourNumber++;
nxtDisplayString(0, " colour # %d", colourNumber);
wait10Msec(50);
}
else if (nNxtButtonPressed == 2)
{
colourNumber--;
nxtDisplayString(0, " colour # %d", colourNumber);
wait10Msec(50);
}
else if (nNxtButtonPressed == 3)
{
nxtDisplayString(0, "# %d is%d ", colourNumber + 1);
select = false;
}
}
return colourNumber;
}
///////////////////////////////////////////////////////////////////////////////////////////////
//This is where you define the heights for the presets. Currently there is only one preset height for testing, but add other cases for the other levels.
//Only changes the value of the variable "value" that is given to the function when a button is pressed.
int readJoystick(int value)
{
float marginHeight = 1.25+1+2-.25; // (45 deg fitting + 1 inch field uncertainty + 2 inch clearance - starting height of the gripper. Units are inches.
float pegHeight1 = 18.0+marginHeight; // Peg #1 - Bottom row of pegs. Units are inches.
float pegHeight2 = 31.5+marginHeight; // Peg #2 - Middle row of pegs. Units are inches.
float pegHeight3 = 45.0+marginHeight; // Peg #3 - Top row of pegs. Units are inches. Subtracted 4 inches for testing only so that we don't damage the arm.
if (joy2Btn(1)) // Peg Height #1 (lowest peg)
{
value = inchesToTicks(pegHeight1);
nxtDisplayString(6, "btn 1 pressed");
}
if (joy2Btn(2)) // Peg Height #2 (middle peg)
{
value = inchesToTicks(pegHeight2);
nxtDisplayString(6, "btn 2 pressed");
}
if (joy2Btn(3)) // Peg Height #3 (top peg)
{
value = inchesToTicks(pegHeight3);
nxtDisplayString(6, "btn 3 pressed");
}
return value;
}
bool Bluetooth_ReadMessage(int* One, int* Two, int* Three)
{
Bluetooth_Initialize();
if(bQueuedMsgAvailable())
{
eraseDisplay();
nxtDisplayString(1, "%i", messageParm[0]);
nxtDisplayString(2, "%i", messageParm[1]);
nxtDisplayString(3, "%i", messageParm[2]);
*One = messageParm[0];
*Two = messageParm[1];
*Three = messageParm[2];
ClearMessage();
return true;
}
else
{
return false;
}
}
task main()
{
int TargetDegree = Rotation_Degrees();
while(true)
{
//globals
bool Connected = Rotation_SensorConnected();
int Degree = Rotation_Degrees();
//display debug to NXT
eraseDisplay();
nxtDisplayString(1, "Working: %i", Connected);
nxtDisplayString(2, "Degree: %i", Degree);
//move the f*****g robot!
int MotionConstant = DistanceOff(TargetDegree, Degree);
MotionConstant = (int)(MotionConstant / 2);
Move(50 + MotionConstant, 50 - MotionConstant);
nxtDisplayString(3, "Motion C: %i", MotionConstant);
//sleep
wait1Msec(50);
}
}
void moveStraightUntilLine(int threshold, bool slow, short dir, int endAtEnc){
motor[rightWheel]=motorSpeed;
motor[leftWheel]=motorSpeed;//this wheel will be linked to wheel 1
nMotorEncoder[rightWheel]=1;
nMotorEncoder[leftWheel]=1;
int last;
ClearTimer(T1);
float newVal=motorSpeed, newVal2=motorSpeed, dif=4.4853;
while((SensorValue[lightSensor1]<threshold)&&(SensorValue[lightSensor2]<threshold)&&abs(nMotorEncoder[leftWheel]-endAtEnc)>50){
last=(nMotorEncoder[rightWheel]+nMotorEncoder[leftWheel]);
wait1Msec(100);
newVal=(newVal*((float)nMotorEncoder[rightWheel]))/(dif*(float)nMotorEncoder[leftWheel]);
if(newVal>100){
newVal=100;
}else{
if(newVal<1)
newVal=1;
}
motor[leftWheel]=((short)newVal);
motor[rightWheel]=((short)newVal2);
nxtDisplayString(1, "%d, %d", nMotorEncoder[rightWheel], ((int)(dif*((float)nMotorEncoder[leftWheel]))));
nxtDisplayString(2, "%d, %d", ((int)newVal2), ((int)newVal));
}
motor[rightWheel]=0;
motor[leftWheel]=0;
}
void doInit() {
// warning if the rotatePart is not at the correct position
while ( ! isRTAtPositon ()) {
//PlaySound(soundException);
nxtDisplayString(2, "Rotation block is not");
nxtDisplayString(3, " at the init position");
nxtDisplayString(4, " (%d) ", SensorValue[sensorLightRTRotate]);
wait1Msec(100);
}
PlaySound(soundBeepBeep);
eraseDisplay();
if ( isCubeAttachedPT() ) {
// assuming the platform is at LS
state_currentPlatformPosition = STATE_PLATFORM_AT_LS1;
action_PT_down_toBottomLimit();
}
// do some initiation
state_currentRotatePosition = STATE_ROTATE_AT_INIT;
state_currentPlatformPosition = STATE_PLATFORM_AT_BOTTOM_LIMIT;
//nMotorPIDSpeedCtrl[motorLS] = mtrSpeedReg;
}
/*
This is a program for mohair's robot.
Mohair's "Bat Mobeile" for the outreach places that we go to.
This program will look for bluetooth signals coming fromm 2 nxt bricks
This is the reveiving program for the "Master, Slave" setup
it will allow us to have 1 nxt brick contolling the motors getting signals of what motors to move from another nxt
This program will use the left right and enter button of 1 of the nxt's to control 2 motors
*/
task main()
{
/*
Here just for syntax refrence (ignore)
motor[mL]=100;
motor[mR]=100;
while (true) {}
while (nNxtButtonPressed!=kRightButton) {}
motor[mR]=-100;
int x=0;
while (nNxtButtonPressed=kRightButton) {}
x++;
(nxtDisplayString(3, "%d", x);
*/
int x=0; //Ths line sets the variable x to 0 allowing us for cetain dislpay properties
int xx=0; // This allows another variale to set to 0 for certain parts with the reciving program
while(true) //Just a while loop that makes sure the program runs indefianatly
{
eraseDisplay (); //erases the display at the beining of the program to clear
xx = message; //this sets our variable to be able to reseave messages from the "Master Nxt" devise
if (xx==1) //If the varibable for the messages is equal to 1 witch is the motion for the movment of right
{
x++; //Adds +1 to display everytime right button is pressed
nxtDisplayString (3, "%d", x);
motor[mL]=100; //robot turns right when right button is pressed
motor[mR]=-100;
wait1Msec(1000); //waits so it doesn bug up
motor[mL] = 0;
motor[mR] = 0;
wait1Msec(1000);
}
if (xx==2) //If the variable equals 2 it will move left
{
x--; //Subtracts -1 to display everytime left button is pressed
nxtDisplayString (3, "%d", x);
motor[mL]=-100; //robot turns left with left button is pressed
motor[mR]=100;
wait1Msec(1000); // waits so it doesnt bug up - no
motor[mL] = 0;
motor[mR] = 0;
wait1Msec(1000);
}
if (xx==3) //If the varibable equals 3 ( if the ceneter/orange/enter button is pressed) it moves forward
{
motor[mL]=100; //robot moves forward when enter button is pressed
motor[mR]=100;
wait1Msec(1000); //waits so it doesnt bug up
motor[mL] = 0;
motor[mR] = 0;
wait1Msec(1000);
}
ClearMessage();
}
}
void moveStraight(int stopAtEnc, int stopAtMsec){
turnOnWheels(MOTOR_SPEED, MOTOR_SPEED);//left wheel will be linked to right wheel
nMotorEncoder[rightWheel]=10;
nMotorEncoder[leftWheel]=10;
int last;
ClearTimer(T1);
float newValL=MOTOR_SPEED, newValR=MOTOR_SPEED;
do{
last=(nMotorEncoder[rightWheel]+nMotorEncoder[leftWheel]);
wait1Msec(230);
newValL=(newValL*((float)nMotorEncoder[rightWheel]))/(dif*(float)nMotorEncoder[leftWheel]);
if(newValL>100){
newValL=100;
}else{
if(newValL<1)
newValL=1;
}
motor[leftWheel]=((short)newValL);
//motor[rightWheel]=((short)newValR);
#ifdef DEBUG
nxtDisplayString(1, "ENC: %d, %d", nMotorEncoder[rightWheel], ((int)(dif*((float)nMotorEncoder[leftWheel]))));
nxtDisplayString(2, "MOT: %d, %d", ((int)newValR), ((int)newValL));
#endif
}while((stopAtEnc==0||abs((nMotorEncoder[rightWheel]+nMotorEncoder[leftWheel])/2)<stopAtEnc)&&(stopAtMsec==0||time1[T1]<stopAtMsec));
motor[rightWheel]=0;
motor[leftWheel]=0;
}
task reportLineTracker()// reports the line tracker value
{while(true)
{
nxtDisplayString(2,"");//clear display
wait1Msec(10);// wait ten msec ticks
nxtDisplayString(2, "LineTracker=(%i)", SensorRaw[S4] /*SensorValue[S4]*/);// prints a message on the
}
}
task reportSonar()// reports values read from sonar sensor
{while(true)
{
nxtDisplayString(3,"");// clear display
wait1Msec(10);// wait ten msec ticks
nxtDisplayString(3, "Ultrasonic=(%i)",SensorRaw[S3] /*Sensor Value in raw readings[port number]*/);
}
}
//Default Display Function
void initdisplay()
{
eraseDisplay();
nxtDisplayString(0, "Change Machine");
nxtDisplayString(1, "Enter a $2, $5 or");
nxtDisplayString(2, "$10 bill to");
nxtDisplayString(3, "receive change in");
nxtDisplayString(4, "loonies.");
}
task reportAvgVoltage()// gives the avg voltage of the tetrix battery
{while(true)
{
int BVolt=externalBatteryAvg;// declares "B" as an integer equal to the external Battery Avg
int AVolt=.001*BVolt;// scales the value for int BVolt
nxtDisplayString(4,"");// clear display
wait1Msec(10);// wait ten msec ticks
nxtDisplayString(4, "Avg-Volt=(%f", AVolt );// prints a message on the nxt screen
}
}
task printServo()
{
while (true)
{
nxtDisplayString(1, "right gate: %d", servo[rightGate]);
nxtDisplayString(2, "left gate: %d", servo[leftGate]);
nxtDisplayString(3, "w encoder: %d", nMotorEncoder[winchMotor]);
wait1Msec(10);
}
}
task reportIRSeeker()//report IR seeker readings
{while(true)
{
int A=SensorRaw[S2];// sensor port 2
int B=10*A;// declares b as an integer and uses operators to scale output values
nxtDisplayString(1,"");//clear display
wait1Msec(10);// wait ten msec ticks
nxtDisplayString(1, "IRSeeker=(%i)", B);// prints a message on the nxt screen
}
}
task Display()
{
while(true)
{
HTACreadAllAxes(excel,excel_x_raw,excel_y_raw,excel_z_raw);
excel_x = ((excel_x_raw/100.0)/GRAVY)+2.0;//cm/s^2
excel_y = (excel_y_raw/100.0)/GRAVY;
excel_z = (excel_z_raw/100.0)/GRAVY;
eraseDisplay();
nxtDisplayString(3,"x: %d", excel_x_raw);
nxtDisplayString(4,"y: %d", excel_y_raw);
nxtDisplayString(5,"z: %d", excel_z_raw);
}
}
//Validity and Value of Note Function
bool calculate(int value)
{
eraseDisplay();
if (value == 2)
{
nxtDisplayString(0, "$2 will be");
nxtDisplayString(1, "returned.");
return true;
}
if (value == 5)
{
nxtDisplayString(0, "$5 will be");
nxtDisplayString(1, "returned.");
return true;
}
if (value == 10)
{
nxtDisplayString(0, "$10 will be");
nxtDisplayString(1, "returned.");
return true;
}
nxtDisplayString(0, "Unable to detect");
nxtDisplayString(1, "valid bill.");
return false;
}
void setDrivetrainSpeeds(int left, int right)
{
if ( left > drivetrainMaxSpeed )
left = drivetrainMaxSpeed;
else if ( left < -drivetrainMaxSpeed )
left = -drivetrainMaxSpeed;
if ( right > drivetrainMaxSpeed )
right = drivetrainMaxSpeed;
else if ( right < -drivetrainMaxSpeed )
right = -drivetrainMaxSpeed;
nxtDisplayString(1, "%i - %i", left, right);
nxtDisplayString(5, "MS %i", drivetrainMaxSpeed);
motor[leftDrive] = -left;
motor[rightDrive] = right;
}
void moveArcade()
{
// Expo makes things less sensitive around the center (a slight
// dead-band), and more aggressive at the extremes.
int straightPower = expoJoystick(joystick.joy1_y1);
int turnPower = expoJoystick(joystick.joy1_x2);
// Combine the two for each motor
int leftPow = straightPower + turnPower;
int rightPow = straightPower - turnPower;
// Ensure that we don't try to give too much power to the motor.
float maxPow = MAX(leftPow, rightPow);
if (maxPow > 100) {
float reduction = 100.0 / maxPow;
leftPow *= reduction;
rightPow *= reduction;
}
// Check the low-speed power setting. If set, reduce power by half.
bool btnPress = joy1Btn(11);
nxtDisplayString(2, "Btn=%d", btnPress);
if (!btnPress && slowSpeedButtonWasPressed)
slowSpeedEnabled = !slowSpeedEnabled;
slowSpeedButtonWasPressed = btnPress;
if (slowSpeedEnabled) {
leftPow /= 2;
rightPow /= 2;
}
motor[leftMotor] = leftPow;
motor[rightMotor] = rightPow;
}
// Main loop that executes program for robot
task main() {
initialize();
#ifdef DEBUG
StartTask(displaySONAR); // acquire and display sensor readings from a different thread
#endif
int dist = detectBox();
bool firstSpinUp = true;
int col = getColor();
while (isValidColor(col)) {
int power = calcWheelPower(dist, col);
motor[WHEELS] = -power;
if (firstSpinUp) {
wait1Msec(5000);
firstSpinUp = false;
} else
wait1Msec(3000);
feed();
wait1Msec(1000); // Pause so next ball can get aligned
col = getColor();
}
#ifdef DEBUG
nxtDisplayString(5, "TERMINATED");
#endif
terminate();
}
void updateSwerveModule(SwerveModule &swerveModule)
{
int reading = HTSPBreadADC(proto, swerveModule.number, 8);
int turnSpeed = calcPID(swerveModule.turnPID, reading);
if ( swerveModule.inverted )
turnSpeed = -turnSpeed;
servo[swerveModule.turnMotor] = -(turnSpeed+(swerveModule.turnZeroOffset+127));
nxtDisplayString(5, "%i", -(turnSpeed+(swerveModule.turnZeroOffset+127)));
/*
if ( abs(swerveModule.driveSpeed) > 10 )
motor[swerveModule.driveMotor] = (swerveModule.driveMotorInverted?-swerveModule.driveSpeed:swerveModule.driveSpeed);
else if ( turnSpeed > 10 )
motor[swerveModule.driveMotor] = -13;
else if ( turnSpeed < -10 )
motor[swerveModule.driveMotor] = 13;
else
motor[swerveModule.driveMotor] = 0;
*/
}
//************************
//Pausa e despausa programa
//-= < Para iniciar maze
//-= > Para ultima sala
//************************
void F_STATE_WAIT()
{
MTASK_SET_RUN(MT_DEFAULT,MT_ACCEL,MT_US);
eraseDisplay();
while(ESTADO_IS_CURRENT())
{
//=====================================
motor[MA]=0;
motor[MC]=0;
nxtDisplayCenteredBigTextLine(4, "MENU");
nxtDisplayString(7, " LINHA - ENTRA");
while(nNxtButtonPressed != BT_LEFT && nNxtButtonPressed != BT_RIGHT){
nxtDisplayCenteredTextLine(1, "R=%i T=%i", ACCEL_Rampa, Touch ==0);
// Le arduino para pingar os ultrasons
//ReadAllUS_short(PORT_ARD,USwall);
nxtDisplayCenteredTextLine(2, "%3i %3i %3i", USwall[0],USwall[1],USwall[2]);
}
if(nNxtButtonPressed==BT_LEFT){
ESTADO_SET_TARGET(ST_LINHA);
}else{
ESTADO_SET_TARGET(ST_ENTRA);
}
while(nNxtButtonPressed!=-1);
PlaySound(soundBlip);
eraseDisplay();
//=====================================
}
}
task main()
{
eraseDisplay();
bNxtLCDStatusDisplay = true; // Enable top status line display
setupHighSpeedLink(true);
//
// Configure the link for raw read and write. User program will have complete control over the link.
// User program will be responsible for managing the half-duplex operation and must prevent collisions!
//
nxtHS_Mode = hsRawMode;
while(true)
{
char ILOVEHKK[10] = {'I',' ','L','O','V','E',' ','H','K','K'};
transmitList = 'I';
count = 1;
TransmitASCII();
Receive();
string receivedcharacters;
StringFromChars(receivedcharacters, receiveList);
nxtDisplayString(30, receivedcharacters);
wait10Msec(100);
eraseDisplay();
}
}
void updateDrivetrain()
{
updateGyro();
if ( drivetrainTurning )
{
int output = calcPID(drivetrainTurnPID, getGyroAngle());
output = hlLimit(output, 45, -45);
setDrivetrainSpeeds(output, -output);
}
else
{
int output = hlLimit(calcPID(drivetrainPID, getDrivetrainDistance()), 100, -100);
int left = hlLimit(output, drivetrainMaxSpeed, -drivetrainMaxSpeed);
int right = hlLimit(output, drivetrainMaxSpeed, -drivetrainMaxSpeed);
if ( drivetrainGyroDrivestraight )
{
drivetrainTurnPID.error = drivetrainTurnPID.target-getGyroAngle();
int turnAmt = drivetrainTurnPID.error*DRIVETRAIN_ANGLECORRECT_CONSTANT;
nxtDisplayString(2, "%i %i", turnAmt, DRIVETRAIN_ANGLECORRECT_CONSTANT);
left += turnAmt;
right -= turnAmt;
}
setDrivetrainSpeeds(left, right);
}
}
task main()
{
int x;
int y;
int z;
while(true)
{
HTACreadAllAxes(excel,x,y,z);
eraseDisplay();
nxtDisplayString(3,"x: %d", x);
nxtDisplayString(4,"y: %d", y);
nxtDisplayString(5,"z: %d", z);
}
}
void addLog()
{
string str = textToAdd;
eraseDisplay();
nxtDisplayString(0, str);
writeDebugStreamLine(str);
}
task Intake()
{
bool intakeOn = true;
bool isBlock = false;
motor[intakeMot] = 100;
//Black, Blue, Green, Yellow, Red, White
while(numBlock < 4)
{
if(SensorValue[colorSensor] > 1) //Greater than Blue
{
wait1Msec(75);
if(SensorValue[colorSensor] > 1) //Greater than Blue
{
PlayTone(440,1);
object = "block ";
isBlock = true;
}
else
{
}
}
else
{
object = " ";
if(isBlock)
{
numBlock++;
isBlock = false;
}
}
nxtDisplayString(3,"%s",object);
nxtDisplayString(4,"numBlock: %d", numBlock);
nxtDisplayString(5,"%d",SensorValue[colorSensor]);
}
//wait1Msec(1750);//1250//2000higher angle: higher time
motor[intakeMot] = 0;
wait1Msec(1000);
motor[intakeMotOut] = -100;
wait1Msec(1500);
motor[intakeMotOut] = 0;
intakeOn = false;
}
task displayValues()
{
while(true)
{
int armEncoder = nMotorEncoder[arm];
nxtDisplayString(1, "Arm: %d ", armEncoder);
}
}
task main()
{
// Loop forever
while(true)
{
// Initial State is 1
// A static integer is initialised only the first
// time this code is executed, then keeps its
// value until changed.
static int state = 1;
switch (state)
{
case 1: // State 1
// Do this first
nxtDisplayString(3, "State 1");
wait1Msec(1000);
// change state
state++;
// jump to end of switch structure
break;
case 2: // State 2
// Do this second
nxtDisplayString(3, "State 2");
wait1Msec(1000);
// change state
state++;
// jump to end of switch structure
break;
case 3: // State 3
// Do this third
nxtDisplayString(3, "State 3");
wait1Msec(10);
// Because we don't change the state,
// this case statment will get executed
// until the program is terminated.
// jump to end of switch structure
break;
} // end switch
}
}
