elevator.c

#pragma config(Sensor, dgtl2,  l1,    sensorTouch)
#pragma config(Sensor, dgtl3,  l2,    sensorTouch)
#pragma config(Sensor, dgtl4,  l3,    sensorTouch)
#pragma config(Sensor, dgtl5,  shaft, sensorQuadEncoder)
#pragma config(Motor,  port2,  power, tmotorServoContinuousRotation, openLoop)

#define motor_speed 15
#define f1 0
#define f2 500
#define f3 1000
#define get_pos SensorValue[shaft]



void set_pos(int pos){
	if (get_pos > pos) {
		while (get_pos > pos) {
			motor[power] = motor_speed * -1;
		}
	} else {
		while (get_pos < pos) {
			motor[power] = motor_speed;	
		}
	}
	motor[power] = 0;
}

void watch_input() {
	bool free = true;
	for (;;) {
		if (SensorValue[l1] == 1) {
				if(free) { set_pos(f1); }
		}	
	}
}

task main(){
	set_pos(500);
	//for(;;) { watch_input(); }
}