elevator_updated.c

#pragma config(Sensor, dgtl2,  b0,    sensorTouch)
#pragma config(Sensor, dgtl3,  b1,    sensorTouch)
#pragma config(Sensor, dgtl4,  b2,    sensorTouch)
#pragma config(Sensor, dgtl6,  led0,  sensorLEDtoVCC)
#pragma config(Sensor, dgtl7,  led1,  sensorLEDtoVCC)
#pragma config(Sensor, dgtl8,  led2,  sensorLEDtoVCC)
#pragma config(Sensor, dgtl10, shaft, sensorQuadEncoder)
#pragma config(Motor,  port2,  power, tmotorServoContinuousRotation, openLoop)

#define DEBUG false
#define FLIPPED false
#define DOWN_SPEED 15
#define UP_SPEED 40
#define MAX_QUEUE 10
#define FLOOR_COUNT 3
#define SHAFT_POS SensorValue[shaft]
#define ABOVE(a) (SHAFT_POS > a)
#define BELOW(a) (SHAFT_POS < a)

int floors[FLOOR_COUNT] = { 0 , 400, 800};
tSensors leds[FLOOR_COUNT] = {led0, led1, led2};
tSensors buttons[FLOOR_COUNT] = {b0, b1, b2};
int queue[MAX_QUEUE];
int front = -1;
int count = 0;

void set_led(char id, int val){
	SensorValue[leds[id]] = val;
}

int dequeue() {
	/* remove and return the head of the queue */
	if (count > 0){
		int x = queue[front];
		queue[front] = -1;
		front = (front + 1) % MAX_QUEUE;
		count--;
		return x;
	} else {
		return -1;
	}
}

void enqueue(int value) {
	/* send `value` to the tail of the queue */
	/* value should be in the range of 0..FLOOR_COUNT-1 */
	if (count == 0) {
		front = 0;
	}
	if (count < MAX_QUEUE) {
		int rear = front + count;
		queue[rear % MAX_QUEUE] = value;
		count++;
	}
}

void move(int dest){
	/* move the elevator to the given position, `dest`.
	* the elevator has a range of 0 to 1000 */
	bool above = ABOVE(dest);
	int flip = FLIPPED ? -1 : 1;
	int delta = (above ? -1 * flip * DOWN_SPEED : flip * UP_SPEED);
	while ( above ? ABOVE(dest) : BELOW(dest) ){
		motor[power] = delta;
	}
	motor[power] = 0;
}

void move_floor(int f) {
	/* translate a floor id , from 0 to MAX_FLOORS-1 */
	int dest = floors[f];
	set_led(f, 1);
	move(dest);
	waitInMilliseconds(3000);
	set_led(f, 0);
}

task write_queue() {
	/* watch for button presses and enqueue their corresponding value */
	for (;;) {
		for (int i=0; i<FLOOR_COUNT; i++){
			if (SensorValue[buttons[i]] == 1 ){
				enqueue(i);
				clearTimer(T1);
				waitInMilliseconds(300);
			}
		}
	}
}

void read_queue() {
	/* set the elevator to the current floor in the queue */
	int floor = dequeue();
	if (floor != -1) {
		/* only move if the queue is not empty */
		move_floor(floor);
	}
}

void reset(){
	for(int i=0; i<MAX_QUEUE; i++) {
		queue[i] = -1;
	}
	clearTimer(T1);
	move_floor(0);
}

void check_active() {
	if (time1(T1) > 30000) {
		reset();
	}
}

task main(){
	SensorValue[shaft] = 0;
	if (DEBUG) {
		move(100);	
	} else {
		reset();
		startTask(write_queue);
		for(;;) {
			read_queue();
			check_active();
		}
	}
}