task flywheelStabilization() { //modulates motor powers to maintain constant flywheel velocity
clearTimer(T1);
float prevError;
float error;
float integral;
int numbbup = 0; //debug
float totalError = 0;
int numloops = 0;
while (true)
{
prevError = targetVelocity - flywheelVelocity;
integral = 0;
while (abs(targetVelocity - flywheelVelocity) < bangBangErrorMargin * targetVelocity && targetVelocity > 0/*true*/) { EndTimeSlice(); }
//bang bang control
bangBangCount += 1;
numbbup += (targetVelocity > flywheelVelocity ? 1 : 0);
bbpercentup = 100 * numbbup / bangBangCount;
bangBangPerSec = (float)((float)bangBangCount * 1000) / (float)(time1(T1) + .1);
while (abs(targetVelocity - flywheelVelocity) > bangBangErrorMargin * flywheelVelocity * 0.75 && targetVelocity > 0) {
setLauncherPower((targetVelocity > flywheelVelocity) ? (127) : ( 0));
EndTimeSlice();
}
setLauncherPower(defaultPower);
while (targetVelocity == 0) { EndTimeSlice(); } //pauses while
}
}
task main () {
// Create two new timer index numbers
int timer1 = TMRnewTimer();
int timer2 = TMRnewTimer();
// Configure timer1 for 2000ms
TMRsetup(timer1, 100);
// Configure timer2 for 5000ms
TMRsetup(timer2, 5000);
// Reset and start both timers
TMRreset(timer1);
TMRreset(timer2);
while (true) {
// If timer1 expires, make a small noise and reset it.
if (TMRisExpired(timer1)) {
PlaySound(soundBlip);
while(bSoundActive) EndTimeSlice();
TMRreset(timer1);
}
// If timer2 expires, make a small noise and reset it.
if (TMRisExpired(timer2)) {
PlaySound(soundShortBlip);
while(bSoundActive) EndTimeSlice();
TMRreset(timer2);
}
EndTimeSlice();
}
}
task autonomous() {
//start flywheel
initializeTasks();
setFlywheelRange(2);
wait1Msec(1000);
startTask(fire);
//wait until first set of preloads are fired
waitUntilNotFiring(15000);
while (firing) { EndTimeSlice(); }
stopTask(fire);
turn(120); //turn toward stack
motor[feedMe] = 127;
driveStraight(150, 1, 1, 60); //move to second stack
turn(-30);
driveStraight(3200, 1, 1, 60); //drive across field
autonProgress = 1;
turn(-83); // turn toward net
autonProgress = 2;
//fire remaining balls
startTask(fire);
while (true) { EndTimeSlice(); }
}
void CEvBlk::Stop(double time)
{
if (pTS)
EndTimeSlice();
StartTimeSlice(time);
pData[pTS->iLen++] = TS_STOP;
EndTimeSlice();
}
task main() {
float temp;
byte state = 0;
nxtDisplayTextLine(0, "Dexter Industries");
nxtDisplayCenteredBigTextLine(1, "T Probe");
nxtDisplayCenteredTextLine(3, "Test 1");
nxtDisplayCenteredTextLine(5, "Connect sensor");
nxtDisplayCenteredTextLine(6, "to S1");
wait1Msec(2000);
eraseDisplay();
nxtDisplayTextLine(0, "Dexter Industries");
nxtDisplayCenteredTextLine(7, "< switch scale >");
//loop to read temp
while (true) {
switch(nNxtButtonPressed) {
// If the right button is pressed, cycle through the scales
case kRightButton:
if (++state > 2)
state = 0;
while (nNxtButtonPressed != kNoButton) EndTimeSlice();
break;
// If the left button is pressed, cycle through the scales in reverse
case kLeftButton:
if (--state < 0)
state = 2;
// debounce the button
while (nNxtButtonPressed != kNoButton) EndTimeSlice();
break;
}
nxtDisplayCenteredBigTextLine(1, "Temp:");
switch(state) {
// if state: 0, display temp in degrees celcius
case 0: DTMPreadTemp(DTMP, temp);
nxtDisplayCenteredBigTextLine(3, "%4.2f", temp);
nxtDisplayCenteredBigTextLine(5, "Celcius");
break;
// if state: 1, display temp in Fahrenheit
case 1: DTMPreadTempF(DTMP, temp);
nxtDisplayCenteredBigTextLine(3, "%4.2f", temp);
nxtDisplayCenteredBigTextLine(5, "Fahrenh.");
break;
// if state: 2, display temp in Kelvin
case 2: DTMPreadTempK(DTMP, temp);
nxtDisplayCenteredBigTextLine(3, "%4.2f", temp);
nxtDisplayCenteredBigTextLine(5, "Kelvin");
break;
}
wait1Msec(10);
}
}
task main()
{
int tempAngle;
eraseDisplay();
wait1Msec(11);
nxtDisplayStringAt(0, 63, "Minigolf 2013");
nxtDisplayStringAt(0, 48, "GyroSensor:");
nxtDisplayStringAt(0, 40, "SonarSensor:");
//StartTask(GyroDeviceDriver);
StartTask(DistDeviceDriver);
StartTask(getHeading);
while(!_GyrodriverInitFinish) // Gyro initialisierung
{
EndTimeSlice();
}
tempAngle = (int)_fGyroAngle;
wait1Msec(4000);
while(tempAngle != (int)_fGyroAngle)
{
nxtDisplayStringAt(0, 48, "GYRO FAIL, %02d",(int)_fGyroAngle);
nxtDisplayStringAt(0, 40, "Drift after INIT");
}
searchObjekt();
wait1Msec(500);
calc_koordinaten_Ball(_ObjectAngle,_ObjectDistance+45);
wait1Msec(500);
calc_degree_hit();
wait1Msec(500);
calc_koordinaten_Drive();
wait1Msec(500);
turn2Degree(900);
wait1Msec(500);
driveDistance(_distance_X2Drive);
wait1Msec(500);
turn2Degree(0);
wait1Msec(500);
driveDistance(_distance_Y2Drive);
wait1Msec(500);
turn2Degree(_degree_Hit);
wait1Msec(500);
driveDistance(50);
wait1Msec(500);
schlagen(40,60);
while(1)
{
EndTimeSlice();
}
StopAllTasks();
}
task main () {
int magFieldValue = 0;
int calibrationValue = 0;
nxtDisplayCenteredTextLine(0, "HiTechnic");
nxtDisplayCenteredBigTextLine(1, "MAGNETIC");
nxtDisplayCenteredTextLine(3, "Field Sensor");
nxtDisplayCenteredTextLine(4, "Test 1");
nxtDisplayCenteredTextLine(5, "Connect Sensor");
nxtDisplayCenteredTextLine(6, "to S1");
wait1Msec(2000);
nxtDisplayCenteredTextLine(5, "Press enter");
nxtDisplayCenteredTextLine(6, "to set bias");
wait1Msec(2000);
eraseDisplay();
while(true) {
eraseDisplay();
nxtDisplayTextLine(1, "Resetting");
nxtDisplayTextLine(2, "bias");
wait1Msec(500);
// Start the calibration and display the offset
calibrationValue = HTMAGstartCal(HTMAG);
nxtDisplayTextLine(2, "Bias: %4d", calibrationValue);
PlaySound(soundBlip);
while(bSoundActive) EndTimeSlice();
while(nNxtButtonPressed != kNoButton) EndTimeSlice();
while(nNxtButtonPressed != kEnterButton) {
eraseDisplay();
// Read the current calibration offset
calibrationValue = HTMAGreadCal(HTMAG);
// Read the current magnetic field strength
magFieldValue = HTMAGreadVal(HTMAG);
nxtDisplayTextLine(1, "Reading");
// Display the current calibration value
nxtDisplayTextLine(2, "Bias: %4d", calibrationValue);
nxtDisplayClearTextLine(4);
// Display the current magnetic field strength
nxtDisplayTextLine(4, "Mag: %4d", magFieldValue);
nxtDisplayTextLine(6, "Press enter");
nxtDisplayTextLine(7, "to recalibrate");
wait1Msec(100);
}
}
}
task main () {
int raw = 0;
int nrm = 0;
// Get control over the buttons
nNxtButtonTask = -2;
eraseDisplay();
nxtDisplayTextLine(0, "Dexter Industries");
nxtDisplayCenteredBigTextLine(1, "dFlex");
nxtDisplayCenteredTextLine(3, "Test 2");
nxtDisplayCenteredTextLine(5, "Connect sensor");
nxtDisplayCenteredTextLine(6, "to S1");
wait1Msec(2000);
eraseDisplay();
nxtDisplayTextLine(0, "dFlex Calibration");
nxtDisplayTextLine(2, "Left: set min");
nxtDisplayTextLine(3, "Right: set max");
nxtDisplayTextLine(7, "Grey: exit");
while (true) {
switch(nNxtButtonPressed) {
// if the left button is pressed calibrate the black value for the sensor
case kLeftButton:
DFLEXcalLow(DFLEX);
PlaySound(soundBeepBeep);
while(bSoundActive) EndTimeSlice();
break;
// if the left button is pressed calibrate the white value for the sensor
case kRightButton:
DFLEXcalHigh(DFLEX);
PlaySound(soundBeepBeep);
while(bSoundActive) EndTimeSlice();
break;
}
nxtDisplayClearTextLine(5);
nxtDisplayClearTextLine(6);
// Read the raw value of the sensor
raw = DFLEXvalRaw(DFLEX);
// Read the normalised value of the sensor
nrm = DFLEXvalNorm(DFLEX);
// Display the raw and normalised values
nxtDisplayTextLine(5, "R: %4d N: %4d", raw, nrm);
// Display the values for black and white
nxtDisplayTextLine(6, "B: %4d W: %4d", dflexlow, dflexhigh);
wait1Msec(50);
}
}
//begin user input region
task lift() {
while (vexRT[deployBtn] == 0) { EndTimeSlice(); }
setLauncherPower(-40, -127, 0);
wait1Msec(75);
setLauncherPower(0);
wait1Msec(750);
while (true) {
setLauncherPower(-127*vexRT[liftBtn] - 40*vexRT[deployBtn], -127, 0);
EndTimeSlice();
}
}
/*void waitForMovementToFinish(motorGroup *group, int timeout=DEF_WAIT_TIMEOUT) {
waitForMovementToFinish(timeout, 1, group);
}*/
void waitForMovementToFinish(motorGroup *group, int timeout=DEF_WAIT_TIMEOUT) { //TODO: delete this as soon as possible
long movementTimer = resetTimer();
while (time(movementTimer) < timeout) { //wait for targeting to stabilize
if (group->moving==TARGET && !errorLessThan(group, group->waitErrorMargin))
movementTimer = resetTimer();
EndTimeSlice();
}
while (group->moving!=TARGET && group->moving!=NO) EndTimeSlice();
}
void parseInput()
{
writeDebugStreamLine("Beging parsing...");
ubyte BytesRead[20];
ubyte currByte[] = {0};
ubyte prevByte[] = {0};
ubyte conn[] = {0};
int cid;
string tmpString;
int index = 0;
while (true)
{
alive();
if (nxtGetAvailHSBytes() > 0)
{
nxtReadRawHS(currByte[0], 1);
if ((prevByte[0] == 27) && (currByte[0] == 'S')) {
index = 0;
memset(rxbuffer, 0, sizeof(rxbuffer));
wait1Msec(1);
nxtReadRawHS(conn[0], 1);
cid = conn[0] - 48;
writeDebugStreamLine("Conn: %d", cid);
while (true) {
while (nxtGetAvailHSBytes() == 0) EndTimeSlice();
nxtReadRawHS(currByte[0], 1);
if ((prevByte[0] == 27) && (currByte[0] == 'E')) {
rxbuffer[index--] = 0;
rxbuffer[index--] = 0;
PlaySound(soundShortBlip);
while(bSoundActive) EndTimeSlice();
break;
}
prevByte[0] = currByte[0];
rxbuffer[index++] = currByte[0];
}
for (int i = 0; i < ((index / 19) + 1); i++) {
memset(BytesRead[0], 0, 20);
memcpy(BytesRead[0], rxbuffer[i*19], 19);
StringFromChars(tmpString, BytesRead);
writeDebugStream(tmpString);
}
genResponse(cid);
}
prevByte[0] = currByte[0];
}
}
}
task main() {
float pressure;
byte state = 0;
nxtDisplayTextLine(0, "Dexter Industries");
nxtDisplayCenteredTextLine(1, "dPressure 250");
nxtDisplayCenteredTextLine(3, "Test 1");
nxtDisplayCenteredTextLine(5, "Connect sensor");
nxtDisplayCenteredTextLine(6, "to S1");
wait1Msec(2000);
eraseDisplay();
nxtDisplayTextLine(0, "Dexter Industries");
nxtDisplayCenteredTextLine(7, "< switch scale >");
//loop to read temp
while (true) {
switch(nNxtButtonPressed) {
// If the right button is pressed, cycle through the scales
case kRightButton:
if (++state > 1)
state = 0;
while (nNxtButtonPressed != kNoButton) EndTimeSlice();
break;
// If the left button is pressed, cycle through the scales in reverse
case kLeftButton:
if (--state < 0)
state = 1;
// debounce the button
while (nNxtButtonPressed != kNoButton) EndTimeSlice();
break;
}
nxtDisplayCenteredBigTextLine(1, "Pressure:");
switch(state) {
// if state: 0, display temp in degrees celcius
case 0: DPRESSreadPress250kPa(DPRESS, pressure);
nxtDisplayCenteredBigTextLine(3, "%4.2f", pressure);
nxtDisplayCenteredBigTextLine(5, "kPa");
break;
// if state: 1, display temp in Fahrenheit
case 1: DPRESSreadPress250PSI(DPRESS, pressure);
nxtDisplayCenteredBigTextLine(3, "%4.2f", pressure);
nxtDisplayCenteredBigTextLine(5, "PSI.");
break;
}
wait1Msec(10);
}
}
void calibrate() {
wait10Msec(20);
while(nNxtButtonPressed != kEnterButton) {
nxtDisplayCenteredTextLine(0, "Robonauts");
nxtDisplayCenteredTextLine(1, "Offense");
nxtDisplayCenteredBigTextLine(3, "Calibrate Compass");
nxtDisplayTextLine(4, "Abs: %4d", HTMCreadHeading(Compass));
nxtDisplayCenteredTextLine(6, "Press Enter");
}
eraseDisplay();
nxtDisplayTextLine(2, "Setting");
nxtDisplayTextLine(3, "target");
wait1Msec(500);
// Set the current heading as the value for the offset to be used as the
// new zero-point for the relative heading returned by
// HTMCreadRelativeHeading()
_target = HTMCsetTarget(Compass);
PlaySound(soundBlip);
while(bSoundActive) {
EndTimeSlice();
}
while(nNxtButtonPressed != kEnterButton) {
nxtDisplayCenteredTextLine(0, "Robonauts");
nxtDisplayCenteredTextLine(1, "Offense");
nxtDisplayCenteredBigTextLine(3, "START ROBOT");
nxtDisplayCenteredTextLine(6, "Press Enter");
}
eraseDisplay();
}
////////////////////////////BALL COUNTER TASK
task BALLCOUNTER ()
{
int CounterIn = 0;
int CounterOut = 0;
while(1)
{
//COUNT BALLS COMING IN
if(SensorValue[IR1] == 0 && CounterIn == 0)
{
BallCounter++;
CounterIn++;
}
if(SensorValue[IR1] == 1)
{
CounterIn = 0;
}
//COUNT BALLS COMING OUT
if(SensorValue[IR4] == 0 && CounterOut == 0)
{
CounterOut++;
}
if(SensorValue[IR4] == 1 && CounterOut == 1)
{
if(BallCounter > 0)
{
BallCounter--;
}
CounterOut = 0;
}
EndTimeSlice(); //OR DELAY 20 MILLI
}
}
task main() {
int _chVal = 0;
nxtDisplayCenteredTextLine(0, "HiTechnic");
nxtDisplayCenteredBigTextLine(1, "Proto");
nxtDisplayCenteredTextLine(3, "Test 1");
nxtDisplayCenteredTextLine(5, "Connect SMUX to");
nxtDisplayCenteredTextLine(6, "S1 and HTPB to");
nxtDisplayCenteredTextLine(7, "SMUX Port 1");
wait1Msec(2000);
PlaySound(soundBeepBeep);
while(bSoundActive) EndTimeSlice();
eraseDisplay();
while(true) {
eraseDisplay();
// get the value for ADC channel 0, we want a 10 bit answer
_chVal = HTPBreadADC(HTPB, 0, 10);
nxtDisplayTextLine(4, "A0: %d", _chVal);
wait1Msec(10);
}
}
task main () {
nNxtButtonTask = -2;
nxtDisplayCenteredTextLine(0, "Mindsensors");
nxtDisplayCenteredBigTextLine(1, "Angle");
nxtDisplayCenteredTextLine(3, "Test 1");
nxtDisplayCenteredTextLine(5, "Connect sensor");
nxtDisplayCenteredTextLine(6, "to S1");
wait1Msec(2000);
eraseDisplay();
nxtDisplayCenteredTextLine(0, "GlideWheel-AS");
nxtDisplayTextLine(1, "-------------------");
nxtDisplayTextLine(5, "-------------------");
while (true) {
// Reset the angle to 0
if (nNxtButtonPressed == kEnterButton)
{
MSANGresetAngle(MSANG);
while (nNxtButtonPressed != kNoButton) EndTimeSlice();
}
// Read the current angle, accumulated angle and RPM and display them
nxtDisplayTextLine(2, "Ang: %7d deg", MSANGreadAngle(MSANG));
nxtDisplayTextLine(3, "Raw: %7d", MSANGreadRaw(MSANG));
nxtDisplayTextLine(4, "RPM: %7d", MSANGreadRPM(MSANG));
nxtDisplayTextLine(7, " Reset angle");
wait1Msec(50);
}
}
task main () {
short msg = 0x00FD;
eraseDisplay();
nxtDisplayTextLine(6, "Press [enter]");
nxtDisplayTextLine(7, "to send msg");
while(true) {
// Increment the msg
msg++;
while(nNxtButtonPressed != kEnterButton) EndTimeSlice();
while(nNxtButtonPressed != kNoButton) EndTimeSlice();
// Send the message to the RCX and display
PlaySound(soundBlip);
nxtDisplayCenteredBigTextLine(2, "0x%04X", msg);
HTRCXsendWord(HTRCX, msg);
}
}
// Allow the user to calibrate the scales
void calibrateScales()
{
int calibrateWeight = 0;
eraseDisplay();
nxtDisplayCenteredTextLine(0, "GlideWheel-AS");
nxtDisplayCenteredTextLine(2, "Place the object");
nxtDisplayCenteredTextLine(3, "on the scales");
nxtDisplayCenteredTextLine(4, "and press");
nxtDisplayCenteredTextLine(5, "[enter]");
nxtDisplayCenteredTextLine(6, "to calibrate");
while (nNxtButtonPressed != kEnterButton) EndTimeSlice();
debounce();
eraseDisplay();
calibrateWeight = weighObject();
nxtDisplayCenteredTextLine(0, "GlideWheel-AS");
nxtDisplayCenteredTextLine(2, "Enter the weight");
nxtDisplayCenteredTextLine(3, "in grams");
nxtDisplayCenteredTextLine(7, "- OK +");
while (true)
{
nxtDisplayCenteredBigTextLine(5, "%d", calibrateWeight);
switch(nNxtButtonPressed)
{
case kLeftButton: PlayTone(500,10); calibrateWeight--; calibrateWeight = max2(0, calibrateWeight); break;
case kRightButton: PlayTone(1000,10); calibrateWeight++; break;
case kEnterButton: PlayTone(1500,10);gramsPerUnit = (float)calibrateWeight / (float)MSANGreadRaw(MSANG); eraseDisplay(); return;
}
wait1Msec(20);
debounce();
}
}
task stage1() {
int stage_one_up = 0;
while (true) {
if (vexRT[Btn5U] == 1 && stage_one_up == 0) {
motor[LIFT_STAGE_1] = FULL_FORWARD;
wait1Msec(4000);
stage_one_up = 1;
}
else if (vexRT[Btn5D] == 1 && stage_one_up == 1) {
motor[LIFT_STAGE_1] = FULL_REVERSE;
wait1Msec(3000);
stage_one_up = 0;
}
else {
motor[LIFT_STAGE_1] = OFF;
}
EndTimeSlice();
}//END OF WHILE LOOP
}//End of STAGE1 TASK
task main () {
// Get control over the buttons
nNxtButtonTask = -2;
eraseDisplay();
nxtDisplayTextLine(0, "HTCS Test 2");
nxtDisplayTextLine(2, "Press orange");
nxtDisplayTextLine(3, "button to start");
nxtDisplayTextLine(4, "calibration.");
nxtDisplayTextLine(5, "Press grey");
nxtDisplayTextLine(6, "button to exit.");
while(nNxtButtonPressed != kEnterButton) EndTimeSlice();
eraseDisplay();
nxtDisplayTextLine(3, "Starting");
nxtDisplayTextLine(4, "calibration.");
// This call calibrates the white value.
if (!HTCScalWhite(HTCS)) {
eraseDisplay();
PlaySound(soundException);
nxtDisplayTextLine(3, "ERROR!!");
nxtDisplayTextLine(5, "Calibration");
nxtDisplayTextLine(6, "failed!!");
wait1Msec(2000);
StopAllTasks();
}
wait1Msec(1000);
}
task stage3() {
int stage_three_up = 0;
while (true) {
if (vexRT[Btn7U] == 1 && stage_three_up == 0) {
motor[LIFT_STAGE_3] = FULL_FORWARD;
wait1Msec(3500);
stage_three_up = 1;
}
else if (vexRT[Btn7D] == 1 && stage_three_up == 1) {
motor[LIFT_STAGE_3] = FULL_REVERSE;
wait1Msec(3000);
stage_three_up = 0;
}
else {
motor[LIFT_STAGE_3] = OFF;
}
EndTimeSlice();
}//END OF WHILE LOOP
}
task autonomous() {
initializeTasks();
stopTask(seymoreControl);
//startTask(skillPointAuto);
//startTask(stationaryAuto);
//startTask(hoardingAuto);
//startTask(classicAuto);
//startTask(skillz);
if (SensorValue[ternaryPot] < 1187) {
if (SensorValue[binaryPot] < 1217) {
startTask(stationaryAuto);
}
else {
startTask(skillPointAuto);
}
}
else if (SensorValue[ternaryPot] < 2577) {
if (SensorValue[binaryPot] < 1217) {
startTask(classicAuto);
}
else {
startTask(hoardingAuto);
}
}
else {
startTask(skillz);
}
while(true) { EndTimeSlice(); }
}
task main () {
int raw = 0;
int nrm = 0;
// Get control over the buttons
nNxtButtonTask = -2;
LSsetActive(LEGOLS);
eraseDisplay();
nxtDisplayTextLine(0, "Light Sensor Cal.");
nxtDisplayTextLine(2, "Left: set black");
nxtDisplayTextLine(3, "Right: set white");
nxtDisplayTextLine(7, "Grey: exit");
while (true) {
switch(nNxtButtonPressed) {
// if the left button is pressed calibrate the black value for the sensor
case kLeftButton:
LScalLow(LEGOLS);
PlaySound(soundBeepBeep);
while(bSoundActive) EndTimeSlice();
break;
// if the left button is pressed calibrate the white value for the sensor
case kRightButton:
LScalHigh(LEGOLS);
PlaySound(soundBeepBeep);
while(bSoundActive) EndTimeSlice();
break;
}
nxtDisplayClearTextLine(5);
nxtDisplayClearTextLine(6);
// Read the raw value of the sensor
raw = LSvalRaw(LEGOLS);
// Read the normalised value of the sensor
nrm = LSvalNorm(LEGOLS);
// Display the raw and normalised values
nxtDisplayTextLine(5, "R: %4d N: %4d", raw, nrm);
// Display the values for black and white
nxtDisplayTextLine(6, "B: %4d W: %4d", lslow[LEGOLS * 4], lshigh[LEGOLS * 4]);
wait1Msec(50);
}
}
task main ()
{
ubyte counter = 0;
ubyte oldCounter = 0;
long rawValue = 0;
long oldRawValue = 0;
nxtDisplayCenteredTextLine(0, "Mindsensors");
nxtDisplayCenteredBigTextLine(1, "PSP-Nx");
nxtDisplayCenteredTextLine(3, "Test 3");
wait1Msec(2000);
PlaySound(soundBeepBeep);
while(bSoundActive) EndTimeSlice();
eraseDisplay();
while (true)
{
// Get the current counter value, wraps at 255
counter = PSPV4readRefSignalCount(PSPNXV4);
if (oldCounter != counter)
{
// Raw value will also "see" commands from the remote
// that are not recognised as "play", "stop", "forward" or "rewind".
// You could use this to control additional aspects of your robot!
rawValue = PSPV4readRawRefTXValue(PSPNXV4);
if (oldRawValue != rawValue)
{
PlaySound(soundShortBlip);
switch(rawValue)
{
case RAW_RIGHT: nxtDisplayCenteredBigTextLine(3, "RIGHT");
goBotRight();
break;
case RAW_LEFT: nxtDisplayCenteredBigTextLine(3, "LEFT");
goBotLeft();
break;
case RAW_FWD: nxtDisplayCenteredBigTextLine(3, "FWD");
goBotFwd();
break;
case RAW_REV: nxtDisplayCenteredBigTextLine(3, "REV");
goBotRev();
break;
case RAW_STOP: nxtDisplayCenteredBigTextLine(3, "STOP");
goBotStop();
break;
}
}
nxtDisplayTextLine(7, "Raw: 0x%03X", rawValue);
// Update the counters and signals
oldCounter = counter;
oldRawValue = rawValue;
}
wait1Msec(50);
}
}
//#subregion waiting
void waitForMovementToFinish(int timeout=DEF_WAIT_TIMEOUT, int numGroups=DEF_WAIT_LIST_LEN, motorGroup* groups=defGroupWaitList) { //that's right, nested pointers (help me)
long movementTimer = resetTimer();
while (time(movementTimer) < timeout) { //wait for targeting to stabilize
for (int i=0; i<numGroups; i++) {
if (groups[i].moving==TARGET && !errorLessThan(&groups[i], &groups[i]->waitErrorMargin)) {
movementTimer = resetTimer();
continue; //TODO: ??
}
}
EndTimeSlice();
}
for (int i=0; i<numGroups; i++) //wait for maneuvers and durational movement to finish
while (groups[i].moving!=TARGET && groups[i].moving!=NO)
EndTimeSlice();
}
task tankDrive() {
while (true) {
motor [DRIVE_MOTOR_RIGHT] = vexRT[Ch2];
motor [DRIVE_MOTOR_LEFT] = vexRT[Ch3];
}
EndTimeSlice();
}
void CEvBlk::MarkAsFinished()
{
LockEvBlk();
if (pTS)
EndTimeSlice();
pCatItem->UpdateTime(this);
pEvHead->bFinished = true;
FreeEvBlk();
}
// Stop the calibration and complain loudly if somethign goes wrong
void stopCalibration() {
if (!HTMCstopCal(HTCOMPASS)) {
eraseDisplay();
nxtDisplayTextLine(1, "ERROR: Calibration");
nxtDisplayTextLine(2, "has failed.");
nxtDisplayTextLine(4, "Check connection");
nxtDisplayTextLine(5, "and try again.");
PlaySound(soundException);
while(bSoundActive) EndTimeSlice();
wait1Msec(5000);
StopAllTasks();
} else {
nxtDisplayTextLine(1, "SUCCESS: ");
nxtDisplayTextLine(2, "Calibr. done.");
PlaySound(soundUpwardTones);
while(bSoundActive) EndTimeSlice();
wait1Msec(5000);
}
}
task main () {
int _x_accel = 0;
int _y_accel = 0;
int _z_accel = 0;
int angleI = 0;
float angleF = 0.0;
int rotI = 0;
float rotF = 0.0;
nxtDisplayCenteredTextLine(0, "MicroInfinity");
nxtDisplayTextLine(1, "CruizCore XG1300L");
nxtDisplayCenteredTextLine(3, "Test 1");
wait1Msec(2000);
eraseDisplay();
// There are 3 ranges the Cruizcore XG1300L can measure in
// up to 2G
// up to 4G
// up to 8G
MICCsetRange8G(MICC);
// Make sure you always reset the sensor at the beginning of your program
// The robot needs to be completely stationary or your heading and gyro
// data won't be accurate.
MICCreset(MICC);
while(bSoundActive) EndTimeSlice();
nxtDisplayTextLine(0, "CruizCore XG1300L");
while (true) {
// Read the relative heading from the sensor.
angleI = MICCreadRelativeHeading(MICC);
angleF = angleI / 100.0;
// Read the rate of turn
rotI = MICCreadTurnRate(MICC);
rotF = rotI / 100.0;
// Read the acceleration data from the sensor
if (!MICCreadAccel(MICC, _x_accel, _y_accel, _z_accel)) {
nxtDisplayTextLine(4, "ERROR!!");
wait1Msec(2000);
StopAllTasks();
}
nxtDisplayTextLine(2, "Heading: %4.2f", angleF);
nxtDisplayTextLine(3, "RoT: %4.2f", rotF);
nxtDisplayTextLine(5, "X: %5.2f", _x_accel/100.0);
nxtDisplayTextLine(6, "Y: %5.2f", _y_accel/100.0);
nxtDisplayTextLine(7, "Z: %5.2f", _z_accel/100.0);
wait1Msec(50);
}
}
task main () {
float x_Phi = 0.0; // should be 0.494233933 according to Excel
float X_val = -20;
float X_mean = -19.80093313; // mu
float X_std = 13.77254704; // sigma
x_Phi = Phi(X_val, X_mean, X_std);
displayTextLine(2, "Phi(x): %f", x_Phi);
while(nNxtButtonPressed != kEnterButton) EndTimeSlice();
}
