void initializeRobot()
{
clearDebugStream();
gyroCal();
//servo[grabberServo]=???;
return;
}
task main()
{
clearDebugStream();
writeDebugStreamLine("xbee5");
time1[T1] = 0;
nxtEnableHSPort(); //Enable High Speed Port #4
nxtSetHSBaudRate(115200); //Xbee Default Speed
nxtHS_Mode = hsRawMode; //Set to Raw Mode (vs. Master/Slave Mode)
while (true)
{
int t = time1[T1];
int ax, ay, az, tx, ty, tz, gx, gy, gz;
MSIMUreadAccelAxes(AIMU, ax, ay, az);
MSIMUreadTiltAxes(AIMU, tx, ty, tz);
MSIMUreadGyroAxes(AIMU, gx, gy, gz);
dataLog(7, t, (az / 100.0));
dataLog(8, t, (tz / 1000.0));
dataLog(9, t, gz);
wait1Msec(50);
}
}
task main()
{
EventList eList;
clearDebugStream();
ClearTimer(T1);
ClearTimer(T4);
while(1)
{
getEvents(&eList);
processAll(&eList);
processDrive(&eList);
for(int i = 0; i<eventCnt;i++)
{
switch(bevents[i].keyID){
//case *name-of-button* :
//*actions* ;
//break;
};
}
//_________________________________________________________________________
}
}
// Y is our IR input, X is our dependant variable.
task main()
{
clearDebugStream();
while(true)
{
sum_y = 0; //from _x->_y
for (int i = 0; i<20 ; i++)
{
HTIRS2readAllACStrength(IR, acS1, acS2, acS3, acS4, acS5);
irInput[i] = acS3; //READINGS: Fill our array with data from the middle node
sum_y += irInput[i]; //CALCULATE: The sum of each value in the array. (its easiest this way)
sum_xTimesy += xInput[i] * irInput[i] ;//EQUATION: Top left half
Sleep(50);
}//APROOVED
mean_xTimesy = (sum_x * sum_y)/20; //EQUATION: Top right half
slopeLOBF = (sum_xTimesy-mean_xTimesy)/(sum_xSquared - mean_sum_xSquared); //CALCULATE: LOBF slope
//y_intLOBF = (sum_y/10) - (5.5 * slopeLOBF); //CALCULATE: LOBF y-intercept -> 5.5 = mean_x
/*
while (y_intLOBF >= y_intTH) //So long as we are above our y-intercept threshold AKA know we are close to the top of the curve
{
if (slopeLOBF <= slopeTH || slopeTH >=) //Wait untill our LOBF slope falls within our calibrated threshold
{
//Make beeping noise perhaps turn left because we have determined the IRSeekerV2 to be facing the IRBeacon
}
}
*/
writeDebugStream("%i, ", slopeLOBF);
writeDebugStream("%i,", y_intLOBF);
writeDebugStreamLine("%i,", numLoop);
numLoop++;
Sleep(50);
}
}
void initialize() {
servoInit();
motorInit(&desiredEncVals);
//Initialize to zeroes
memset(&desiredMotorVals, 0, sizeof(desiredMotorVals));
memset(&desiredEncVals, 0, sizeof(desiredEncVals));
clearDebugStream();
}
task main() {
clearDebugStream();
nMotorEncoder[LEFT_WHEEL] = 0;
nMotorEncoder[RIGHT_WHEEL] = 0;
bPlaySounds = true;
set_starting_position(84.0, 30.0, 0.0);
drawMap();
StartTask(vehicle_compute_position);
//StartTask(vehicle_draw_position);
navigate_to_waypoint(104, 30);
drawParticles();
navigate_to_waypoint(124, 30);
drawParticles();
navigate_to_waypoint(144, 30);
drawParticles();
navigate_to_waypoint(180, 30);
drawParticles();
navigate_to_waypoint(180, 54);
drawParticles();
navigate_to_waypoint(164, 54);
drawParticles();
navigate_to_waypoint(126, 54);
drawParticles();
// Moved left
// Going up
navigate_to_waypoint(126, 74);
drawParticles();
navigate_to_waypoint(126, 94);
drawParticles();
navigate_to_waypoint(126, 104);
drawParticles();
navigate_to_waypoint(126, 124);
drawParticles();
navigate_to_waypoint(126, 144);
drawParticles();
navigate_to_waypoint(126, 168);
drawParticles();
navigate_to_waypoint(126, 148);
drawParticles();
navigate_to_waypoint(126, 126);
drawParticles();
// From here, move in 20 cm ranges
navigate_to_waypoint(30, 54);
drawParticles();
navigate_to_waypoint(84, 54);
drawParticles();
navigate_to_waypoint(84, 30);
drawParticles();
StopTask(vehicle_compute_position);
//StopTask(vehicle_draw_position);
wait10Msec(60000); // wait 1MIN
}
task main()
{
clearDebugStream();
while (true)
{
testPointerDereference();
writeDebugStreamLine("'*' (Pointer Dereference) passed");
wait1Msec(100);
}
}
void initialize() {
clearDebugStream();
writeDebugStream("This is JoyRecord\n");
//Initialize to zeroes
memset(&desiredMotorVals, 0, sizeof(desiredMotorVals));
memset(&desiredEncVals, 0, sizeof(desiredEncVals));
motorInit(&desiredEncVals);
servoInit();
}
/////////////////////////////////////////////////////////////////////////////////////////////////////
//
// Main Task
//
// The following is the main code for the tele-op robot operation Customize as appropriate for
// your specific robot
//
// Game controller / joystick information is sent periodically (about every 50 milliseconds) from
// the FMS (Field Management System) to the robot Most tele-op programs will follow the following
// logic:
// 1 Loop forever repeating the following actions:
// 2 Get the latest game controller / joystick settings that have been received from the PC
// 3 Perform appropriate actions based on the joystick + buttons settings This is usually a
// simple action:
// * Joystick values are usually directly translated into power levels for a motor or
// position of a servo
// * Buttons are usually used to start/stop a motor or cause a servo to move to a specific
// position
// 4 Repeat the loop
//
// Your program needs to continuously loop because you need to continuously respond to changes in
// the game controller settings
//
// At the end of the tele-op period the FMS will autonmatically abort (stop) execution of the program
//
/////////////////////////////////////////////////////////////////////////////////////////////////////
bool bAtBeacon(void)
{
int zones[5];
HTIRS2readAllACStrength(IR, zones[0], zones[1], zones[2], zones[3], zones[4]);
clearDebugStream();
for(int i = 0; i < 5; i++)
{
writeDebugStreamLine("zone %d:\t%d", i, zones[i]);
}
return ((zones[1] > 50) && (zones[2] > 50) && (abs(zones[1] - zones[2]) < 10));
}
task main()
{
clearDebugStream();
printnVexRCRecieveState();
startTask(MotorSlewRateTask);
startTask(DrivetrainControlTask);
startTask(LiftControlTask);
while (true) { EndTimeSlice(); }
}
task main()
{
clearDebugStream();
gyroCal();
waitForStart();
nMotorEncoder[BR] = 0;
move(-65, 25);
turn(35, 25);
move(-70, 25);
}
task main()
{
initializeRobot();
waitForStart(); // Wait for the beginning of autonomous phase.
clearDebugStream();
Move(16.01, 0.75);
wait10Msec(10);
Turn(-(180 - 110.55));
wait10Msec(10);
Move(46.11, 1);
}
task main()
{
// Clear all text from the debugstream window
clearDebugStream();
//sets LED to flash to show that the printer is printing
setLEDColor(ledRed);
//credits
displayCenteredTextLine(2, "Made by Xander Soldaat");
displayCenteredTextLine(4, "and Cyrus Cuenca");
//verion number
displayCenteredTextLine(6, "Version 1.1");
//GitHub link
displayCenteredTextLine(8, "http://github.com/cyruscuenca/g-pars3");
//supported commands
displayCenteredTextLine(10, "Supported commands: G1");
float x, y, z, e, f = 0.0;
long fd = 0;
char buffer[128];
long lineLength = 0;
string gcmd = "G1"; // always a g1
fd = fileOpenRead(fileName); // fileName = gcode.rtf
if (fd < 0) // if file is not found/cannot open
{
writeDebugStreamLine("Could not open %s", fileName);
return;
}
while (true)
{
lineLength = readLine(fd, buffer, 128);
if (lineLength > 0)
{
readNextCommand(buffer, lineLength, x, y, z, e, f);
executeCommand(gcmd, x, y, z, e, f);
}
else
// we're done, no lines left to read from the file
return;
// Wipe the buffer by setting its contents to 0
memset(buffer, 0, sizeof(buffer));
//LED turns green to show that the print is done
setLEDColor(ledGreen);
}
}
task main()
{
initializeRobot();
StartTask(Drive);
const float Conversion = 9.8/200;
int X_axis;
int Y_axis;
int Z_axis;
int time;
int delta_time;
int time_old;
int accel_old;
int accel_new;
float velocity_old;
float velocity_new;
float distance;
clearDebugStream();
ClearTimer(T3);
time_old = nPgmTime;
accel_old = 0.0;
velocity_old = 0;
distance = 0;
while(true)
{
HTACreadAllAxes(Accel, X_axis, Y_axis, Z_axis);
time = nPgmTime; // this timer wraps around
delta_time = abs(time - time_old); // delta time in ms
if (delta_time < 1) // protect against divide by zero
{
delta_time = 1;
}
accel_new = X_axis;
velocity_new = velocity_old + 0.001 * (float)delta_time * 0.5 *(accel_new + accel_old);
distance = distance + 0.001 * (float)delta_time * 0.5 *(velocity_new + velocity_old);
time_old = time;
accel_old = accel_new;
velocity_old = velocity_new;
writeDebugStreamLine("X:%d, Y:%d, Z:%d\nVelocity:%f, Distance:%f",X_axis, Y_axis, Z_axis, velocity_new,distance);
wait10Msec(3);
}
}
////////////////////////////////////////////////////////////////////////////// TASK MAIN!!! \(^0^)/
task main()
{
clearDebugStream();
gyroCal();
getSeeker();
int left = 0;
int delay = 0;
int endDump = 0;
int ramp = 0;
int* switches = getButtons();
//asign meaning to the touch sensors
if(switches[3])
left = 1;
else
left = 0;
if(switches[2])
delay = 1;
else
delay = 0;
if(switches[1])
endDump = 1;
else
endDump = 0;
if(switches[0])
ramp = 1;
else
ramp = 0;
//Round starts here
waitForStart();
nMotorEncoder[BR] = 0;
nMotorEncoder[wrist1] = 0;
nMotorEncoder[arms] = 0;
if(endDump)
EndDump(left, delay);
else if(ramp)
Ramp(left, delay);
else
IRramp(left , delay);
}
task main()
{
clearDebugStream();
ClearTimer(T1);
ClearTimer(T4);
while(1)
{
getEvents();
for(int i = 0; i<eventCnt;i++)
{
switch(bevents[i].keyID){
case JOY1_GREEN:
if(bevents[i].state == B_PRSD)
{
motor[fan1] = 100;
motor[fan2] = 100;
}
else if(bevents[i].state == B_RLSD)
{
motor[fan1] = 0;
motor[fan2] = 0;
}
break;
case JOY1_BLUE:
if(bevents[i].state == B_PRSD)
{
motor[fan1] = 30;
motor[fan2] = 30;
}
else if(bevents[i].state == B_RLSD)
{
motor[fan1] = 0;
motor[fan2] = 0;
}
break;
};
}
//_________________________________________________________________________
}
}
task main()
{
initializeRobot();
//waitForStart(); // Wait for the beginning of autonomous phase.
clearDebugStream();
motor[leftRear] = -50;
motor[leftFront] = -50;
motor[rightRear] = 50;
motor[rightFront] = 50;
while (true)
{
loop1();
}
}
task usercontrol()
{
startTask(pid);
clearDebugStream();
while (true)
{
//++++++++++++++++++++Shooter++++++++++++++++++++
if (vexRT[Btn5DXmtr2]){
if (vexRT[Btn8UXmtr2]) targetRPM = full;
else if (vexRT[Btn8LXmtr2])targetRPM = skills;
else if (vexRT[Btn8RXmtr2])targetRPM = half;
else if (vexRT[Btn8DXmtr2])targetRPM = close;
}
else targetRPM = 0;
if (vexRT[Btn6DXmtr2]) targetRPM = 0;
//pid overwrite
if (vexRT[Btn6UXmtr2]) shooter(vexRT[Ch2Xmtr2]);
//------------------End Shooter------------------
//+++++++++++++++++++++Drive+++++++++++++++++++++
if (vexRT[Btn5U]) drive(vexRT[Ch2]/2+vexRT[Ch1]/2,vexRT[Ch2]/2-vexRT[Ch1]/2);
else if (vexRT[Btn5D]) drive(vexRT[Ch2]/4+vexRT[Ch1]/4,vexRT[Ch2]/4-vexRT[Ch1]/4);
else drive(vexRT[Ch2]+vexRT[Ch1], vexRT[Ch2]-vexRT[Ch1]);
/*tank drive
runLeft(vexRT[Ch3]);
runRight(vexRT[Ch2]);
*/
//--------------------End Drive--------------------
//++++++++++++++++++++++Intake++++++++++++++++++++++
if (vexRT[Btn6U]) intake(127);
else if (vexRT[Btn6D]) intake(-127);
else intake(0);
//--------------------End Intake--------------------
//++++++++++++++++++++Solenoids+++++++++++++++++++++
//------------------End Solenoids------------------
}//while
}//task
task flywheelControl(){
flywheelOn = true;
clearDebugStream();
float kP=2.1; //2.2;//was 1.675
float kI=0.005;//1//07;//was 0.0025
float kD=0.0;
//float kP=3.0;//was 1.675
//float kI=0.0;//1//07;//was 0.0025
//float kD=0.0;
//float kP=0.8001;//was 0.72
//float kI=0.05532;
int limit = 15;
while(true){
// if(currentGoalVelocity==VELOCITY_PIPE)
// kP=2.05;//2.2 for NON AUTO - 1.5 for auto
// else
// kP=2.0;
error = (currentGoalVelocity - getFlywheelVelocity());
integral = integral + error;
derivative = error-lastError;
lastError=error;
//if(integral>(100/kI))
// integral = 100/kI;
output = error*kP + integral*kI+derivative*kD;
if(output >25){
if(output>motor[flywheel4]+limit){
motor[flywheel4]=motor[flywheel4]+limit;
}else if(output<motor[flywheel4]-limit){
motor[flywheel4]=motor[flywheel4]-limit;
}else{
motor[flywheel4]=output;
}
}else if(output<20){
motor[flywheel4]=20;
//integral=0;
}
if(debugMode)
writeDebugStreamLine("Motors: %d, Error: %d, P: %d, I: %d Integral: %d Derivative: %d", motor[flywheel1], error, error*kP, integral*kI, integral, derivative*kD);
delay(50);
}
}
task main()
{
initializeRobot();
const float Conversion = 9.8/200;
float time = 0;
int X_axis_old =0;
int Velocity = 0;
int Velocity_old = 0;
float Distance = 0,distation = 0;
//int Distance_old = 0;
int offsetagv_X = 0 ,offsetagv_Y = 0;
float theta_X, phi_Y;
clearDebugStream();
for(int i = 0; i <= 20; i++)
{
HTACreadAllAxes(Accel, X_axis, Y_axis, Z_axis);
offsetagv_X += abs(X_axis);
offsetagv_Y += abs(Y_axis);
offset_Z += abs(Z_axis);
wait1Msec(100);
}
StartTask(getaccel);
offset_X = (float) (offsetagv_X/20);
offset_Y = (float) (offsetagv_Y/20);
offset_Z = (float) (offset_Z/20);
theta_X = atan((offset_X)/(sqrt((offset_Z*offset_Z)+(offset_Y*offset_Y))));
phi_Y = atan((offset_Y)/(sqrt((offset_Z*offset_Z)+(offset_X*offset_X))));
while(Distance <= distation)
{
time = time1[T1]/1000;
Velocity = (time)*(X_axis+X_axis_old)/2+Velocity_old;
Distance = time*(Velocity+Velocity_old)/2+Distance;
Velocity_old += Velocity;
wait10Msec(1);
}
}
void init() {
playTone(700,10);
clearDebugStream();
intakeAutonomousIndexer = false;
intakeAutonomousIntake = false;
intakeAutonomousShoot = false;
//Startup modes
if(!debugMode)
debugMode = (bool) SensorValue[debug];
intakeAutonomousIndexer = false;
intakeAutonomousIntake = false;
intakeAutonomousIndexer = false;
startTask(intakeControl);
startTask(reverseFlywheel);
}
task main()
{
clearDebugStream();
writeDebugStreamLine(" N: fib(N) Recursive Iterative");
for (long i = 1; i < 1000; ++i)
{
nElapsedTime_recursion = nPgmTime;
nResult_via_recursion = fib_via_recursion(i);
nElapsedTime_recursion = nPgmTime - nElapsedTime_recursion;
nElapsedTime_iteration = nPgmTime;
nResult_via_iteration = fib_via_iteration(i);
nElapsedTime_iteration = nPgmTime - nElapsedTime_iteration;
writeDebugStreamLine("%3d: %9d %7d msec %7d msec", i, nResult_via_recursion, nElapsedTime_recursion, nElapsedTime_iteration);
wait1Msec(1);
}
}
task main()
{
initializeRobot();
waitForStart(); // wait for start of tele-op phase
clearDebugStream();
StartTask(AwesomeDrive);
StartTask(Arm);
StartTask(Buttons);
long messageCountPrev = 0;
while (true)
{
if (messageCountPrev == ntotalMessageCount)
{
StopTask(AwesomeDrive);
StopTask(Arm);
StopTask(Buttons);
StopTask(LameDrive);
motor[mtr_S1_C2_1] = 0;
motor[mtr_S1_C2_2] = 0;
motor[mtr_S1_C3_1] = 0;
motor[mtr_S1_C3_2] = 0;
motor[mtr_S1_C4_1] = 0;
motor[mtr_S1_C4_2] = 0;
wait10Msec(100);
StartTask((driveStyle == 0) ? AwesomeDrive : LameDrive);
StartTask(Arm);
StartTask(Buttons);
}
messageCountPrev = ntotalMessageCount;
wait10Msec(100);
}
}
task main()
{
clearDebugStream();
writeDebugStreamLine("This is PIDTest\n");
memset(&desiredEncVals, 0, sizeof(desiredEncVals));
motorInit(&desiredEncVals);
semaphoreInitialize(PIDconstantSemaphore);
startTask(PID);
long loopStartTimeMs = nPgmTime;
semaphoreLock(PIDconstantSemaphore);
for (pid_kp=0; pid_kp<5; pid_kp++) {
for (pid_ki=0; pid_ki<0; pid_ki+=0.01) {
for (pid_kd=0; pid_kd<0; pid_kd++) {
writeDebugStream("%f, %f, %f, ", pid_kp, pid_ki, pid_kd);
if (bDoesTaskOwnSemaphore(PIDconstantSemaphore)) {
semaphoreUnlock(PIDconstantSemaphore);
}
while(motorGetEncoder((tMotor) MecMotor_FR) < 5000){
hogCPU();
motorUpdateState();
desiredMotorVals.power[MecMotor_FR] = 50;
releaseCPU();
}
semaphoreLock(PIDconstantSemaphore);
motor[MecMotor_FR] = 0; //can do this because we have lock on semaphore
writeDebugStream("Changing constants!\n");
wait1Msec(1000); //wait for motor to spin down
}
}
}
}
task main()
{
clearDebugStream();
int servoVal = 0;
servo[irTurret] = servoVal;
wait10Msec(100);
while (true)
{
for (int i = 0; i < 5; i++)
{
servoVal++;
servo[irTurret] = 70 + i;
eraseDisplay();
nxtDisplayCenteredBigTextLine(1, "IR: %d", SensorValue[ir]);
nxtDisplayCenteredBigTextLine(4, "Ser: %d", 70 + I);
//if (SensorValue[ir] == 6 || (SensorValue[ir] == 0 && irPrev == 7))
// break;
wait10Msec(50);
}
}
while (true){}
}
void init() {
playTone(700,10);
clearDebugStream();
intakeAutonomousIndexer = false;
intakeAutonomousIntake = false;
intakeAutonomousShoot = false;
//Startup modes
if(!debugMode)
debugMode = (bool) SensorValue[debug];
intakeAutonomousIndexer = false;
intakeAutonomousIntake = false;
intakeAutonomousIndexer = false;
flywheelShots();
startTask(intakeControl, kHighPriority);
startTask(flywheelVelocityCalculation, kHighPriority);
startTask(reverseFlywheel);
startTask(LCD);
}
task main()
{
initializeRobot();
waitForStart(); // Wait for the beginning of autonomous phase.
clearDebugStream();
//Off ramp
Move(-82.5, 0.5);
wait10Msec(10);
Move(-10, 0.2);
//grab goal
wait10Msec(60);
servo[hook1] = 0;
servo[hook2] = 255;
wait10Msec(30);
Turn(30);
wait10Msec(10);
Move(110, 1);
wait10Msec(10);
Turn(150);
wait10Msec(10);
//score
motor[lift] = 75;
wait10Msec(800);
motor[lift] = 0;
wait10Msec(10);
servo[output] = 250;
wait10Msec(500);
//release goal
servo[hook1] = 255;
servo[hook2] = 0;
wait10Msec(300);
Move(5, 1);
wait10Msec(30);
Turn(180);
}
task main()
{
clearDebugStream();
heap_Init();
//heap_Malloc(3, 1);
//heap_Print(0,11);
//heap_Malloc(2, 2);
//heap_Print(0,11);
//heap_Expand(0,3,1);
//heap_Print(0,11);
//heap_PrintStats(0,11);
block b;
block_Initialize(&b, 2);
writeDebugStreamLine("%d,%d",b.loc,b.size);
heap_Print(0,11);
writeDebugStreamLine("%d",block_Shrink(&b,1));
heap_Print(0,11);
writeDebugStreamLine("%d,%d",b.loc,b.size);
}
task main()
{
gyroCal();
//waitForStart();
clearDebugStream();
writeDebugStreamLine("starting!");
nMotorEncoder[FrontR] = 0;
nMotorEncoder[grabber1] = 0;
nMotorEncoder[grabber2] = 0;
int beac = getSeeker();
if(beac == -1)
{
writeDebugStreamLine("This no good");
Stop(0);
}
else if(beac == 1)
{
writeDebugStreamLine("orientation 1 is running");
turn(-41, 25);
move(50, 30, 1, 0);
turn(82, 25);
move (40, 40, 1, 0);
}
else if(beac == 2)
{
writeDebugStreamLine("orientation 2 is running");
turn(-19, 25);
move(60, 30, 1, 0);
turn(30, 25);
move(40, 40, 1, 0);
}
else if(beac == 3)
{
writeDebugStreamLine("orientation 3 is running");
move(60, 30, 1, 0);
turn(-36, 25);
move(40, 40, 1, 0);
}
else
{
writeDebugStreamLine("the sensor thought it was in region %d", beac);
}
//move(-10 , 30, 1, 1);
//writeDebugStreamLine("finished enitial move");
//wait10Msec(100);
//turn(82, 25);
//writeDebugStreamLine("finished first turn");
/*move(-40 , 50, 1, 0);
turn(-45, 50);
move(-10, 30, 1, 0);
ClearTimer(T1);
while(nMotorEncoder[grabber1] > -90 && time1[T1] < 1500)
{
if(abs(nMotorEncoder[grabber1]-nMotorEncoder[grabber2]) > 10)
writeDebugStreamLine("the grabbers aren't moving with each other");
motor[grabber1] = -30;
motor[grabber2] = -30;
}
motor[grabber1] = 0;
motor[grabber2] = 0;*/
}
task main()
{
initializeRobot();
clearTimer(T1);
bool isLaunching = false;
bool isDropping = false;
int mThreshold = 15; // Sets dead zones to avoid unnecessary movement
int aThreshold = 30;
int xVal, yVal; //Stores left analog stick values
float scaleFactor = 40.0 / 127; //Sets max. average motor power and maps range of analog stick to desired range of power
//waitForStart(); // wait for start of tele-op phase
int num = 0;
while (true)
{
getJoystickSettings(joystick); // Retrieves data from the joystick
//4 is forward, 2 is backwards, 7 is left, 8 is right
if(joy1Btn(4) == 1){
if(num == 4) {
motor[motorD] = 20;
motor[motorE] = 20;
} else {
finishAction(num);
num = 4;
}
} else if (joy1Btn(2) == 1){
if(num == 2) {
motor[motorD] = -23;
motor[motorE] = -23;
} else {
finishAction(num);
num = 2;
}
} else if (joy1Btn(1) == 1){
if(num == 1) {
turn(1);
} else {
finishAction(num);
num = 1;
}
}else if (joy1Btn(3) == 1){
if(num == 3) {
turn(-1);
} else {
finishAction(num);
num = 3;
}
//raise scissor lift (positive motor power)
/*
} else if (joy1Btn(9) == 1){
if(num == 9 && flag9) {
flag9 = false;
releaseBallCollector();
//
} else {
finishAction(num);
num = 9;
}
*/
/*} else if (joy1Btn(6) == 1){
if(num == 13 && flag13) {
flag13 = false;
raiseGrabber();
//
} else {
finishAction(num);
num = 13;
}//raise grabber
} else if (joy1Btn(7) == 1){
if(num == 14 && flag14) {
flag14 = false;
lowerGrabber();
//
} else {
finishAction(num);
num = 14;
}
//lower grabber */
}else if (joy1Btn(7) == 1){
clearDebugStream();
writeDebugStreamLine(path);
stopAllTasks();
}else{
if(num != 0) {
finishAction(num);
num = 0;
}
//RECORD CURRENT VARIABLES, THEN RESET EVERYTHING
}
//Controls launcher
//if(joy1Btn(1) == 1 && isLaunching == false){
//isLaunching = true;
//fireLauncher();
//isLaunching = false;
//}
//wait1Msec(1);
}
}
