''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''' ''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''' '' '' '' VDE TUD Roboterwettbewerb 2003 '' '' (Aufgabe 3) '' '' '' '' - Aluminator - '' '' '' ''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''' ''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''' ' Richtungsports Motoren DEFINE out_mdirect_left PORT[1] DEFINE out_mdirect_right PORT[2] ' Geschwindigkeitsports Motor DEFINE out_mspeed_left DA[2] DEFINE out_mspeed_right DA[1] ' IR Detektoren DEFINE in_ir_outer_left PORT[3] DEFINE in_ir_inner_left PORT[7] DEFINE in_ir_center PORT[5] DEFINE in_ir_inner_right PORT[8] DEFINE in_ir_outer_right PORT[4] DEFINE in_ir_ball PORT[6] ' TSOP Detektoren DEFINE in_tsop_outer_left PORT[12] DEFINE in_tsop_inner_left PORT[9] DEFINE in_tsop_inner_right PORT[10] DEFINE in_tsop_outer_right PORT[11] ' Schußmechanismus DEFINE in_shoot_working PORT[14] DEFINE out_shoot_load PORT[16] DEFINE out_shoot_fire PORT[13] ' Konstanten DEFINE MSPEED_STOP 0 DEFINE MSPEED_SLOW 130 DEFINE MSPEED_MEDIUM 161 DEFINE MSPEED_FAST 193 DEFINE MSPEED_FASTEST 255 DEFINE STATE_PATH_LEFT 0 DEFINE STATE_PATH_ONLINE 1 DEFINE STATE_PATH_RIGHT 2 DEFINE STATE_PATH_OUTER_OFFLINE 3 DEFINE STATE_PATH_OUTER_ENTERLINE 4 DEFINE STATE_PATH_OUTER_BEFORELINE 5 DEFINE STATE_PATH_INNER_OFFLINE 6 DEFINE STATE_PATH_INNER_ENTERLINE 7 DEFINE STATE_PATH_INNER_BEFORELINE 8 DEFINE STATE_SHOOT_LEFT 10 DEFINE STATE_SHOOT_ON 11 DEFINE STATE_SHOOT_RIGHT 12 ' Variable DEFINE state BYTE DEFINE linepassed BYTE DEFINE loststate BYTE ' Startwerte einstellen state = STATE_PATH_OUTER_OFFLINE linepassed = 0 loststate = STATE_PATH_OUTER_OFFLINE out_mspeed_left = MSPEED_FASTEST out_mspeed_right = MSPEED_FASTEST out_mdirect_left = 0 out_mdirect_right = 0 out_shoot_load = ON out_shoot_fire = OFF #pathloop ''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''' IF state <> STATE_PATH_OUTER_OFFLINE THEN GOTO ifnot_path_outer_offline IF NOT (in_ir_outer_right AND NOT in_ir_outer_left) THEN GOTO ifnot_path_outer_offline1 out_mdirect_left = OFF out_mspeed_left = MSPEED_FASTEST out_mdirect_right = OFF out_mspeed_right = MSPEED_MEDIUM state = STATE_PATH_OUTER_BEFORELINE GOTO pathloop #ifnot_path_outer_offline1 IF NOT (in_ir_outer_left AND NOT in_ir_outer_right) THEN GOTO ifnot_path_outer_offline2 out_mdirect_left = OFF out_mspeed_left = MSPEED_MEDIUM out_mdirect_right = OFF out_mspeed_right = MSPEED_FASTEST state = STATE_PATH_OUTER_BEFORELINE GOTO pathloop #ifnot_path_outer_offline2 IF NOT (in_ir_outer_left AND in_ir_outer_right) THEN GOTO ifnot_path_outer_offline3 state = STATE_PATH_OUTER_BEFORELINE GOTO pathloop #ifnot_path_outer_offline3 IF NOT in_ir_center THEN GOTO pathloop state = STATE_PATH_ONLINE GOTO pathloop #ifnot_path_outer_offline ''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''' IF state <> STATE_PATH_OUTER_BEFORELINE THEN GOTO ifnot_path_outer_beforeline IF NOT in_ir_center THEN GOTO pathloop out_mdirect_left = ON out_mspeed_left = MSPEED_FASTEST out_mdirect_right = OFF out_mspeed_right = MSPEED_FASTEST state = STATE_PATH_OUTER_ENTERLINE linepassed = 0 GOTO pathloop #ifnot_path_outer_beforeline ''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''' IF state <> STATE_PATH_OUTER_ENTERLINE THEN GOTO ifnot_path_outer_enterline IF NOT ((linepassed=1) AND (in_ir_outer_right OR in_ir_inner_right)) THEN GOTO ifnot_path_outer_enterline1 out_mdirect_left = OFF out_mspeed_left = MSPEED_MEDIUM out_mdirect_right = OFF out_mspeed_right = MSPEED_FASTEST state = STATE_PATH_RIGHT GOTO pathloop #ifnot_path_outer_enterline1 IF (linepassed=0) AND NOT in_ir_outer_right THEN linepassed=1 GOTO pathloop #ifnot_path_outer_enterline ''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''' IF state <> STATE_PATH_INNER_OFFLINE THEN GOTO ifnot_path_inner_offline IF NOT (in_ir_outer_left AND NOT in_ir_outer_right) THEN GOTO ifnot_path_inner_offline1 out_mdirect_left = OFF out_mspeed_left = MSPEED_MEDIUM out_mdirect_right = OFF out_mspeed_right = MSPEED_FASTEST state = STATE_PATH_INNER_BEFORELINE GOTO pathloop #ifnot_path_inner_offline1 IF NOT (in_ir_outer_right AND NOT in_ir_outer_left) THEN GOTO ifnot_path_inner_offline2 out_mdirect_left = OFF out_mspeed_left = MSPEED_FASTEST out_mdirect_right = OFF out_mspeed_right = MSPEED_MEDIUM state = STATE_PATH_INNER_BEFORELINE GOTO pathloop #ifnot_path_inner_offline2 IF NOT (in_ir_outer_left AND in_ir_outer_right) THEN GOTO ifnot_path_inner_offline3 state = STATE_PATH_INNER_BEFORELINE GOTO pathloop #ifnot_path_inner_offline3 IF NOT in_ir_center THEN GOTO pathloop state = STATE_PATH_ONLINE GOTO pathloop #ifnot_path_inner_offline ''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''' IF state <> STATE_PATH_INNER_BEFORELINE THEN GOTO ifnot_PATH_INNER_beforeline IF NOT in_ir_center THEN GOTO pathloop out_mdirect_left = OFF out_mspeed_left = MSPEED_FASTEST out_mdirect_right = ON out_mspeed_right = MSPEED_FASTEST state = STATE_PATH_INNER_ENTERLINE linepassed = 0 GOTO pathloop #ifnot_PATH_INNER_beforeline ''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''' IF state <> STATE_PATH_INNER_ENTERLINE THEN GOTO ifnot_path_inner_enterline IF NOT ((linepassed=1) AND (in_ir_outer_left OR in_ir_inner_left)) THEN GOTO ifnot_path_inner_enterline1 out_mdirect_left = OFF out_mspeed_left = MSPEED_FASTEST out_mdirect_right = OFF out_mspeed_right = MSPEED_MEDIUM state = STATE_PATH_LEFT GOTO pathloop #ifnot_PATH_INNER_enterline1 IF (linepassed=0) AND NOT in_ir_outer_left THEN linepassed=1 GOTO pathloop #ifnot_path_inner_enterline ''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''' IF in_ir_ball THEN GOTO ifnot_path_ball out_mdirect_left = OFF out_mspeed_left = MSPEED_FASTEST out_mdirect_right = OFF out_mspeed_right = MSPEED_SLOW state = STATE_SHOOT_ON #directspin IF in_tsop_inner_left NAND in_tsop_inner_right THEN GOTO directspin out_mdirect_right = OFF GOTO shootloop #ifnot_path_ball ''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''' IF state <> STATE_PATH_LEFT THEN GOTO ifnot_path_left IF NOT (in_ir_inner_right AND NOT (in_ir_inner_left OR in_ir_center)) THEN GOTO ifnot_path_left1 out_mdirect_left = OFF out_mspeed_left = MSPEED_FASTEST out_mdirect_right = OFF out_mspeed_right = MSPEED_FAST GOTO pathloop #ifnot_path_left1 IF in_ir_inner_right OR in_ir_center OR in_ir_inner_left OR in_ir_outer_right THEN GOTO ifnot_path_left2 out_mdirect_left = OFF out_mspeed_left = MSPEED_FASTEST out_mdirect_right = ON out_mspeed_right = MSPEED_FAST GOTO pathloop #ifnot_path_left2 IF in_ir_inner_right OR in_ir_center OR in_ir_inner_left OR (NOT in_ir_outer_right) THEN GOTO ifnot_path_left3 out_mdirect_left = OFF out_mspeed_left = MSPEED_FAST out_mdirect_right = ON out_mspeed_right = MSPEED_FASTEST GOTO pathloop #ifnot_path_left3 IF NOT in_ir_center THEN GOTO pathloop state = STATE_PATH_ONLINE GOTO pathloop #ifnot_path_left ''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''' IF state <> STATE_PATH_ONLINE THEN GOTO ifnot_path_online IF NOT (NOT (in_ir_inner_left OR in_ir_inner_right) AND in_ir_center) THEN GOTO ifnot_path_online1 out_mdirect_left = OFF out_mspeed_left = MSPEED_FASTEST out_mdirect_right = OFF out_mspeed_right = MSPEED_FASTEST GOTO pathloop #ifnot_path_online1 IF NOT (in_ir_inner_left AND NOT in_ir_inner_right) THEN GOTO ifnot_path_online2 out_mdirect_left = OFF out_mspeed_left = MSPEED_FAST out_mdirect_right = OFF out_mspeed_right = MSPEED_FASTEST state = STATE_PATH_RIGHT GOTO pathloop #ifnot_path_online2 IF in_ir_outer_left OR in_ir_inner_left OR in_ir_center OR in_ir_inner_right OR in_ir_outer_right THEN GOTO ifnot_path_online5 state = loststate loststate = STATE_PATH_OUTER_OFFLINE GOTO pathloop #ifnot_path_online5 IF NOT ((NOT in_ir_outer_left) OR in_ir_inner_left OR in_ir_center OR in_ir_inner_right OR in_ir_outer_right) THEN GOTO ifnot_path_online3 loststate = STATE_PATH_LEFT GOTO pathloop #ifnot_path_online3 IF NOT (in_ir_outer_left OR in_ir_inner_left OR in_ir_center OR in_ir_inner_right OR (NOT in_ir_outer_right)) THEN GOTO pathloop loststate = STATE_PATH_RIGHT GOTO pathloop #ifnot_path_online ''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''' IF state <> STATE_PATH_RIGHT THEN GOTO pathloop IF NOT (in_ir_inner_left AND NOT (in_ir_inner_right OR in_ir_center)) THEN GOTO ifnot_path_right1 out_mdirect_left = OFF out_mspeed_left = MSPEED_FAST out_mdirect_right = OFF out_mspeed_right = MSPEED_FASTEST GOTO pathloop #ifnot_path_right1 IF in_ir_inner_right OR in_ir_center OR in_ir_inner_left OR in_ir_outer_left THEN GOTO ifnot_path_right2 out_mdirect_left = ON out_mspeed_left = MSPEED_FAST out_mdirect_right = OFF out_mspeed_right = MSPEED_FASTEST GOTO pathloop #ifnot_path_right2 IF in_ir_inner_right OR in_ir_center OR in_ir_inner_left OR (NOT in_ir_outer_left) THEN GOTO ifnot_path_right3 out_mdirect_left = ON out_mspeed_left = MSPEED_FASTEST out_mdirect_right = OFF out_mspeed_right = MSPEED_FAST GOTO pathloop #ifnot_path_right3 IF NOT in_ir_center THEN GOTO pathloop state = STATE_PATH_ONLINE GOTO pathloop #shootloop ''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''' IF state <> STATE_SHOOT_LEFT THEN GOTO ifnot_shoot_left IF NOT (in_tsop_inner_right AND NOT in_tsop_inner_left) THEN GOTO ifnot_shoot_left1 out_mspeed_left = MSPEED_FASTEST out_mspeed_right = MSPEED_FAST GOTO shootloop #ifnot_shoot_left1 IF NOT (in_tsop_outer_left AND NOT in_tsop_outer_right) THEN GOTO ifnot_shoot_left2 out_mspeed_left = MSPEED_FASTEST out_mspeed_right = MSPEED_SLOW GOTO shootloop #ifnot_shoot_left2 IF NOT (in_tsop_inner_left AND in_tsop_inner_right) THEN GOTO ifnot_shoot_left3 out_mspeed_left = MSPEED_FASTEST out_mspeed_right = MSPEED_FASTEST state = STATE_SHOOT_ON GOTO shootloop #ifnot_shoot_left3 IF NOT (in_tsop_outer_right AND NOT in_tsop_outer_left) THEN GOTO ifnot_shoot_left4 out_mspeed_left = MSPEED_SLOW out_mspeed_right = MSPEED_FASTEST state = STATE_SHOOT_RIGHT GOTO shootloop #ifnot_shoot_left4 IF NOT (in_tsop_inner_left AND NOT in_tsop_inner_right) THEN GOTO shootloop out_mspeed_left = MSPEED_FAST out_mspeed_right = MSPEED_FASTEST state = STATE_SHOOT_RIGHT GOTO shootloop #ifnot_shoot_left ''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''' IF state <> STATE_SHOOT_ON THEN GOTO ifnot_on IF NOT (in_tsop_inner_right AND NOT in_tsop_inner_left) THEN GOTO ifnot_on1 out_mspeed_left = MSPEED_FASTEST out_mspeed_right = MSPEED_FAST state = STATE_SHOOT_LEFT GOTO shootloop #ifnot_on1 IF NOT (in_tsop_outer_left AND NOT in_tsop_outer_right) THEN GOTO ifnot_on2 out_mspeed_left = MSPEED_FASTEST out_mspeed_right = MSPEED_SLOW state = STATE_SHOOT_LEFT GOTO shootloop #ifnot_on2 IF NOT (in_tsop_inner_left AND NOT in_tsop_inner_right) THEN GOTO ifnot_on3 out_mspeed_left = MSPEED_FAST out_mspeed_right = MSPEED_FASTEST state = STATE_SHOOT_RIGHT GOTO shootloop #ifnot_on3 IF NOT (in_tsop_outer_right AND NOT in_tsop_outer_left) THEN GOTO ifnot_on4 out_mspeed_left = MSPEED_SLOW out_mspeed_right = MSPEED_FASTEST state = STATE_SHOOT_RIGHT GOTO shootloop #ifnot_on4 #shoot_load_loop IF in_shoot_working THEN GOTO shoot_load_loop out_shoot_load = OFF out_shoot_fire = ON #shoot_fire_loop IF in_shoot_working THEN GOTO shoot_fire_loop out_shoot_load = ON out_shoot_fire = OFF pause 75 out_mdirect_left = OFF out_mspeed_left = MSPEED_FASTEST out_mdirect_right = ON out_mspeed_right = MSPEED_FASTEST pause 25 out_mdirect_left = OFF out_mdirect_right = OFF state = STATE_PATH_INNER_OFFLINE linepassed = 0 loststate = STATE_PATH_INNER_OFFLINE GOTO pathloop #ifnot_shoot_fire GOTO shootloop #ifnot_on ''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''' IF state <> STATE_SHOOT_RIGHT THEN GOTO shootloop IF NOT (in_tsop_inner_left AND NOT in_tsop_inner_right) THEN GOTO ifnot_shoot_right1 out_mspeed_left = MSPEED_FAST out_mspeed_right = MSPEED_FASTEST GOTO shootloop #ifnot_shoot_right1 IF NOT (in_tsop_outer_right AND NOT in_tsop_outer_left) THEN GOTO ifnot_shoot_right2 out_mspeed_left = MSPEED_SLOW out_mspeed_right = MSPEED_FASTEST GOTO shootloop #ifnot_shoot_right2 IF NOT (in_tsop_inner_left AND in_tsop_inner_right) THEN GOTO ifnot_shoot_right3 out_mspeed_left = MSPEED_FASTEST out_mspeed_right = MSPEED_FASTEST state = STATE_SHOOT_ON GOTO shootloop #ifnot_shoot_right3 IF NOT (in_tsop_outer_left AND NOT in_tsop_outer_right) THEN GOTO ifnot_shoot_right4 out_mspeed_left = MSPEED_FASTEST out_mspeed_right = MSPEED_SLOW state = STATE_SHOOT_RIGHT GOTO shootloop #ifnot_shoot_right4 IF NOT (in_tsop_inner_right AND NOT in_tsop_inner_left) THEN GOTO shootloop out_mspeed_left = MSPEED_FASTEST out_mspeed_right = MSPEED_FAST state = STATE_SHOOT_RIGHT GOTO shootloop end