Spring 2015 Program Code in RobotC
#pragma config(Sensor, S1, touchSensor, sensorTouch)
#pragma config(Sensor, S2, lightSensorA, sensorLightActive)
#pragma config(Sensor, S3, lightSensorB, sensorLightActive)
#pragma config(Sensor, S4, sonarSensor, sensorSONAR)
#pragma config(Motor, motorB, rightMotor, tmotorNXT, PIDControl, reversed, driveLeft, encoder)
#pragma config(Motor, motorC, leftMotor, tmotorNXT, PIDControl, reversed, driveRight, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
#include "NXTMenu2015.c";
#include "NXTNavigation2015.c";
void followLineToBlocks() {
float difference;
float sum;
float factor;
float vPower;
float variableAmount = 15.0;
float constantAmount = 15.0;
SensorType[S3] = sensorLightActive; // Make sure you turn the light 'on'
SensorType[S2] = sensorLightActive; // Make sure you turn the light 'on'
while(SensorValue[(short) sonarSensor] > (8*2.54)) {
difference = (float) (SensorRaw[lightSensorB] - SensorRaw[lightSensorA]);
sum = (float) (SensorRaw[lightSensorB] + SensorRaw[lightSensorA]);
nxtDisplayTextLine(5, "Snr %4d %4d", SensorRaw[lightSensorB], SensorRaw[lightSensorA]); // Just a debug message
factor = difference / sum; // will be a positive or negative number between +1 and -1
vPower = variableAmount * factor;
nxtDisplayTextLine(6, "f v %4.2f %4.2f", factor, vPower); // Just a debug message
motor[motorC] = (int) (constantAmount + vPower);
motor[motorB] = (int) (constantAmount - vPower);
nxtDisplayTextLine(7, "m m %4d %4d", (constantAmount + vPower), (constantAmount - vPower)); // Just a debug message
}
motor[motorB] = 0;
motor[motorC] = 0;
SensorType[S3] = sensorLightInactive; // Make sure you turn the light 'off'
SensorType[S2] = sensorLightInactive; // Make sure you turn the light 'off'
}
/*---------------------
RED_FUNCTION
Get blocks and bring them back to base
---------------------*/
void redFunction() {
setPowerLevel(30);
driveForward(6,40);
followLineToBlocks();
driveForward(16);
driveBackward();
stopOnTouch();
driveBackward(12);
driveForward(12);
// At this point, you may want to drive back a few more inches to deliver the blocks
// Then drive forward a few inches to release the blocks.
wait(2);
}task main()
{
config.centerOfWheelToCenterOfRobotMM = 90.50;
config.wheelDiameterMM = 96.0;
config.gearRatio = 1.0;
configureWheelCircumferenceMM();
setPowerLevel(30);
bFloatDuringInactiveMotorPWM = true;
newMenu("Swissvale Library");
addMenuItem("Prog1-DriveForw", RED_FUNCTION);
addMenuItem("Prog2-", ORANGE_FUNCTION);
addMenuItem("Prog3-", YELLOW_FUNCTION);
addMenuItem("EXIT", TERMINATOR_FUNCTION);
while(true) {
showMenu(); // The menu is shown
menu.selectedItem = startMenu(); // An item is selected
switch (menu.selectedItem) // Now switch based on the menu value of the selected item
{
case RED_FUNCTION: redFunction(); break;
case ORANGE_FUNCTION: break;
case YELLOW_FUNCTION: break;
case TERMINATOR_FUNCTION: playSoundFile("Goodbye.rso"); wait(2); stopAllTasks(); break;
}
menu.returnNow = false; // This has to be reset so that once a selection is made, the robot will execute it
stopMoving(); // I don't want you moving now
}
}