task main() {
initializeRobot();
bool programRunning = false;
while (true) {
wait1Msec(5);
if (programStarted() && !programRunning) { // I.E. program JUST started
wait1Msec(selection[DELAY_MENU]*1000); // Wait for selected delay
currentRoutine=selection[STARTPOS_MENU]; // Set routine
if (gyroEnabled()) StartTask(gyroTask); // Start gyro tracking
resetGyro(); // Set gyro to 0
resetDrive(); // Set encoders to 0
programRunning=true; // Set the running veriable so this doesn't execute again
}
if (programStarted()) {
if (DEBUG) nxtDisplayCenteredTextLine(4, "%i", currentRoutine);
//Constantly update arm movements
if (armState != 0) motor[BlockArm] = getTargetingPower(armState, nMotorEncoder[BlockArm], 0.04, 80);
if (currentInc < 100 && !done) { // make sure step never wraps
if (completed) {
//Start sequences
initRoutine(); currentInc++; // Increment step, make sure it doesn't ever wrap
}
runTargets();
}
} else {
processMenuInput();
}
}
}
void setupDrives(){
for (int i = 0; i < noOfDrives; i++){
for (int j = 0; j < 4; j++){
drives[i].data[j] = packedData[i][j];
}
resetDrive(i);
}
}
void runTargets() {
if (DEBUG) nxtDisplayCenteredTextLine(6, "%i", nMotorEncoder[FLDrive]); //Debug
if (DEBUG) nxtDisplayCenteredTextLine(7, "%i", nMotorEncoder[FRDrive]);
if (mode == RAW_MODE) {
long leftPos = nMotorEncoder[FLDrive];
long rightPos = nMotorEncoder[FRDrive];
if ((inRange(leftPos, leftTarget-DRV_RANGE, leftTarget+DRV_RANGE) &&
inRange(rightPos, rightTarget-DRV_RANGE, rightTarget+DRV_RANGE)) ||
time1[T2] > maxTime) {
completed=true;
} else {
int leftPow = getTargetingPower(leftTarget, leftPos, 7.0/leftSpeed, leftSpeed);
int rightPow = getTargetingPower(rightTarget, rightPos, 7.0/rightSpeed, rightSpeed);
motor[FLDrive] = leftPow; motor[FRDrive] = rightPow; motor[BLDrive] = leftPow; motor[BRDrive] = rightPow;
}
} else if (mode == GYRO_DEGREES) {
if (inRange(gAngle, leftTarget-GYRO_RANGE, leftTarget+GYRO_RANGE) ||
time1[T2] > maxTime) {
completed=true;
} else {
int turnPower = getTargetingPower(leftTarget, gAngle, 1, AUTON_TURN_SPEED);
motor[FLDrive] = turnPower; motor[FRDrive] = -turnPower;
motor[BLDrive] = turnPower; motor[BRDrive] = -turnPower;
}
} else if (mode == ARC_GYRO_DEGREES) {
if (inRange(gAngle, leftTarget-GYRO_RANGE, leftTarget+GYRO_RANGE) ||
time1[T2] > maxTime) {
completed=true;
} else {
int turnPower = getTargetingPower(leftTarget, gAngle, 1.8, 100);
if (leftSpeed>0) {
int idlePower = getTargetingPower(0, nMotorEncoder[FRDrive], 7.0/50, 50);
motor[FLDrive] = turnPower*leftSpeed; motor[FRDrive] = idlePower;
motor[BLDrive] = turnPower*leftSpeed; motor[BRDrive] = idlePower;
} else {
int idlePower = getTargetingPower(0, nMotorEncoder[FLDrive], 7.0/50, 50);
motor[FLDrive] = idlePower; motor[FRDrive] = -turnPower*rightSpeed; //negative
motor[BLDrive] = idlePower; motor[BRDrive] = -turnPower*rightSpeed; //negative
}
}
} else if (mode == RAISE_ARM || mode == LOWER_ARM || mode == ELEVATE_ARM) {
// Actual movement is handled in loop
if (inRange(nMotorEncoder[BlockArm], armState-50, armState+50) ||
time1[T2] > maxTime) {
completed=true;
}
} else if (mode == OPEN_CLAMP || mode == CLOSE_CLAMP) {
setClamp(clampState);
completed=true;
} else if (mode == PAUSE) {
if (time1[T2] > maxTime) completed=true;
} else if (mode == INTERNAL_RAMP) {
int speed=0;
//Ramp is ~8 degrees
if (rampStage==0) { //get on ramp
if (gAngle<-5) {
rampStage=1;
//PlayImmediateTone(440, 10);
} else {
speed = -70;
}
} else if (rampStage>=1) {
speed = gAngle*6/(rampStage);
if (gAngle>=-2 && gAngle<=2) rampStage++; //PlayImmediateTone(444, 10);
if (rampStage>=4) {
completed=true;
} else {
wait1Msec(rampStage*50);
}
}
motor[FLDrive] = speed; motor[FRDrive] = speed; motor[BLDrive] = speed; motor[BRDrive] = speed;
} else {
completed=true;
}
if (completed) resetDrive(); // Reset encoders to 0
return;
}
void pre_auton()
{
setUp();
resetGyro();
resetDrive();
}
void loop() {
for (int i = 0; i < 3; i++){ // make sure it's fairly bright
writeNum(currentDrive);
}
counter++;
expire--;
if (expire < 0){
currentChar = NULL;
}
if (keyboard.available()) {
// read the next key
char c = keyboard.read();
// change current drive if enter pressed
if (c == PS2_ENTER) {
currentDrive++;
if (currentDrive >= noOfDrives){
currentDrive = 0;
}
resetDrive(currentDrive); // the current drive cannot be looping
}
if (c == PS2_DELETE){
currentDrive = 0;
for (int i = 0; i < noOfDrives; i++){
resetDrive(i);
}
}
if (c == PS2_TAB) {
drives[currentDrive].recording = true;
drives[currentDrive].loopCap = 200; // default
}
if (c == PS2_ESC) {
// end loop
if (drives[currentDrive].recording == true){
drives[currentDrive].loopCap = drives[currentDrive].currentNote;
}
}
// a char has been pressed!
if (c != NULL){
expire = 120;
currentChar = c;
}
}
int freq; // get frequency of next note
switch(currentChar){
case 'a':
freq = 2;
break;
case 's':
freq = 3;
break;
case 'd':
freq = 4;
break;
case 'f':
freq = 5;
break;
case 'g':
freq = 6;
break;
case 'h':
freq = 7;
break;
case 'j':
freq = 8;
break;
case 'k':
freq = 9;
break;
case 'l':
freq = 10;
break;
case '1':
currentDrive = 0;
resetDrive(0);
break;
case '2':
currentDrive = 1;
resetDrive(1);
break;
case '3':
currentDrive = 2;
resetDrive(2);
break;
case '4':
currentDrive = 3;
resetDrive(3);
break;
default:
freq = 0;
}
if (freq != 0){
frame(drives[currentDrive].data, freq);
}
// recording
if (drives[currentDrive].recording){
if (counter % 50 == 0){ // read every 120ms
//Serial.print(freq);
drives[currentDrive].notes[drives[currentDrive].currentNote] = freq;
drives[currentDrive].currentNote++;
if (drives[currentDrive].currentNote >= drives[currentDrive].loopCap){
drives[currentDrive].recording = false; // turn recording off
drives[currentDrive].currentNote = 0; // reset currentNote
drives[currentDrive].looping = true;
//Serial.println();
currentDrive++; // increment drive id
if (currentDrive >= noOfDrives){
currentDrive = 0;
}
resetDrive(currentDrive); // the current drive cannot be looping
}
}
}
// looping
for (int i = 0; i < noOfDrives; i++){
if (drives[i].looping){
// this drive is in loop cycle
int driveFreq = drives[i].notes[drives[i].currentNote];
//Serial.println(driveFreq);
frame(drives[i].data, driveFreq);
if (counter % 50 == 0){ // change note every 120ms
drives[i].currentNote++;
if (drives[i].currentNote >= drives[i].loopCap){
drives[i].currentNote = 0; // restart loop!
}
}
}
}
delay(1.8); // 1ms delay for step motor
for (int i = 0; i < 7; i++){ // refresh 7 sgement
pinMode(segments[i], OUTPUT);
digitalWrite(segments[i], 1);
}
}