3.ＲＣカーの開発

#include<conio.h>
#include<lnp.h>
#include<dsensor.h>

int main(int argc, char **argv) {
	char sendmsg;
	ds_active(&SENSOR_3);
	ds_rotation_on(&SENSOR_3);
	ds_rotation_set(&SENSOR_3,0);

	cputs("prop");
	while(1){
		if(SENSOR_1<0xf000){ // left
			sendmsg='d';
		}
		else if(SENSOR_2<0xf000){ // right
			sendmsg='e';
		}
		else{
			sendmsg='f';
		}
		send_msg(sendmsg);
		if(ROTATION_3>=7){ // front
			sendmsg='a';
		}
		else if(ROTATION_3<=-7){ // back
			sendmsg='b';
		}
		else{
			sendmsg='c';
		}
		send_msg(sendmsg);
	}
	return 0;
}

int main(int argc, char *argv[]) {
	cputs("on");
	sem_init(&t_motor,0,1);
	t_stop=execi(&stop,0,NULL,100,DEFAULT_STACK_SIZE); // make stop task
	t_st_stop=execi(&st_stop,0,NULL,90,DEFAULT_STACK_SIZE); // make steering stop task
	t_tson=execi(&tson,0,NULL,50,DEFAULT_STACK_SIZE); // make touch task
	t_drive=execi(&drive,0,NULL,10,DEFAULT_STACK_SIZE); // make drive task
	t_steering=execi(&steering,0,NULL,10,DEFAULT_STACK_SIZE); // make steering task
	return 0;
}

iint drive(){ // drive function
	char getmsg;
	while(1){
		wait_event(&rt_dr_wakeup,0); //wait for lnp_rcx_message
		getmsg=lnp_rcx_message;
		clear_msg();
		sem_wait(&t_motor);
		if(getmsg=='a'){
			motor_c_speed(MAX_SPEED);
			motor_c_dir(rev);
		}
		else if(getmsg=='b'){
			motor_c_speed(MAX_SPEED);
			motor_c_dir(fwd);
		}
		else{
			motor_c_dir(off);
		}
		sem_post(&t_motor);
	}
	return 0;
}

wakeup_t rt_dr_wakeup(wakeup_t data){ // lnp check for drive func
	return lnp_rcx_message=='a' || lnp_rcx_message=='b' || lnp_rcx_message=='c';
}

int steering(){ // steering function
	char getmsg;
	ds_active(&SENSOR_2);
	ds_rotation_on(&SENSOR_2);
	ds_rotation_set(&SENSOR_2,0);
	motor_a_speed(MAX_SPEED);

	while(1){
		wait_event(&rt_st_wakeup,0); //wait for lnp_rcx_message
		getmsg=lnp_rcx_message;
		clear_msg();
		if(getmsg=='d'){
			if(ROTATION_2<65){
				motor_a_dir(fwd);
				msleep(10);
			}
			else
				motor_a_dir(brake);
		}
		else if(getmsg=='e'){
			if(ROTATION_2>-65){
				motor_a_dir(rev);
				msleep(10);
			}
			else
				motor_a_dir(brake);			
		}
		else if(ROTATION_2<-2){
			while(ROTATION_2<=-2){
				motor_a_speed(MAX_SPEED/8);
				motor_a_dir(fwd);
			}
			motor_a_dir(brake);
		}
		else if(ROTATION_2>2){
			while(ROTATION_2>=2){
				motor_a_speed(MAX_SPEED/8);
				motor_a_dir(rev);
			}
			motor_a_dir(brake);
		}
	}
	return 0;
}

wakeup_t rt_st_wakeup(wakeup_t data){ // lnp check for steering func
	return lnp_rcx_message=='d' || lnp_rcx_message=='e' || lnp_rcx_message=='f';
}

int tson(){ // touch sensor function
	motor_c_speed(MAX_SPEED);
	while(1){
		wait_event(&tson_wakeup,0);
		sem_wait(&t_motor);
		if(SENSOR_1<0xf000){
			motor_c_dir(rev);
			msleep(500);
			motor_c_dir(brake);
		}
		else if(SENSOR_3<0xf000){
			motor_c_dir(fwd);
			msleep(500);
			motor_c_dir(brake);
		}
		sem_post(&t_motor);
	}
	return 0;
}

wakeup_t tson_wakeup(wakeup_t data){
	return SENSOR_1<0xf000 || SENSOR_3<0xf000;
}

int st_stop(){ // steering stop function
	while(1){
		wait_event(&rt_wakeup,0);
		motor_a_dir(brake);
		msleep(50);
	}
	return 0;
}

wakeup_t rt_wakeup(wakeup_t data){
	return ROTATION_2>=65 || ROTATION_2<=-65;
}

int stop(){ // rcx stop function
	int stopchar;
	stopchar=getchar();
	if(stopchar==KEY_RUN){
		cputs("stop");
		killall(100);
	}
	return 0;
}