ARS Tutorial2

ARS-arbiter.c

/* We use the most important first in the if statement. If follow_left and follow_right are not 0, then we use the follow
otherwise we check the next one, which is forage */
task arbiter() {
	if ( follow_left || follow_right) {
		move(follow_left, follow_right);
	} else if ( forage_left || forage_right) {
		move(forage_left, forage_right); 
	}
}

// just a new helper function
void move(int left_motor, int right_motor) {
	setMotorSpeed(left, left_motor);
	setMotorSpeed(right, right_motor); 
}

ARS-follow.c

int follow_left = 0; int follow_right = 0;

task followMotors() { 
	while (true) {
		if (follow_left || follow_right) { 
			setMotorSpeed(left, follow_left);
			setMotorSpeed(right, follow_right);
			mode = FOLLOW_MODE;
		}
	}
}

task follow() {
	while (true) { 
		// NOTE: no suppression
			
		if(online()) {
			// move forward
			initialFound = true; 
			follow_left = MOVING_SPEED;
			follow_right = MOVING_SPEED;
		} else if (initialFound) { 
			bool foundLine = false;

			while ( !foundLine ) {
				resetGyro(S1);
				
				for(int i = 0; i <= 180 i+=5) {
					
					while (abs(getGyroDegrees(S1)) < i) {
						// keep at turning speed until gyro has tried up to 180
						follow_left = TURNING_SPEED;
						follow_right = -TURNING_SPEED;
					}
					foundLine = online();
					if (foundLine) {
						break;
					} else if (i == 180){
						// turn off follow
						follow_left = 0;
						follow_right = 0;
					}
				}
			}
			
		} else {
			// turn off follow
			follow_left = 0;
			follow_right = 0; 
		}
	}
}

ARS-forage.c

int forage_left = 0; int forage_right = 0;

task forage() {
	
	while(true) {
		// NOTE: No suppression
		
		// Go forwards
		forage_left = MOVING_SPEED;
		forage_right = MOVING_SPEED;
		
		// Set the motors to turn. We can then almost hardcode the turning logic in a 'scan' function later. 
		T1.reset();
		if(Time1(T1) >= random(2000)) {
			// turn
			forage_left = TURNING_SPEED;
			forage_right = -TURNING_SPEED;
		}
	}
}