diff options
| author | lonkaars <loek@pipeframe.xyz> | 2022-06-08 14:56:00 +0200 | 
|---|---|---|
| committer | lonkaars <loek@pipeframe.xyz> | 2022-06-08 14:56:00 +0200 | 
| commit | 867a54109c1bf25617fa46fd4c776db10d56fa8d (patch) | |
| tree | 69e487f68821bf9e64b421e5369c450394bd04a6 /robot/mode_grid.c | |
| parent | 92a43e07b10903d5f1a572ebdf9a40571b75e73d (diff) | |
`make format`0.6.0
Diffstat (limited to 'robot/mode_grid.c')
| -rw-r--r-- | robot/mode_grid.c | 83 | 
1 files changed, 39 insertions, 44 deletions
diff --git a/robot/mode_grid.c b/robot/mode_grid.c index e864db7..16dbef3 100644 --- a/robot/mode_grid.c +++ b/robot/mode_grid.c @@ -1,7 +1,7 @@  #include "mode_grid.h" -#include "orangutan_shim.h"  #include "modes.h"  #include "movement.h" +#include "orangutan_shim.h"  int g_w2_order_number; @@ -22,7 +22,7 @@ void w2_location_message() {  	print_long(g_w2_location.x);  	print(",");  	print_long(g_w2_location.y); -	delay(200);  +	delay(200);  }  int g_w2_detection = 0; @@ -32,8 +32,8 @@ char g_w2_y_location = 0;  void w2_crosswalk_stroll() {  	print("hoi"); -	while (g_w2_sensors[0] < 100 && g_w2_sensors[1] < 100 && g_w2_sensors[2] < 100 && g_w2_sensors[3] < 100 && -		   g_w2_sensors[4] < 100) { +	while (g_w2_sensors[0] < 100 && g_w2_sensors[1] < 100 && g_w2_sensors[2] < 100 && +		   g_w2_sensors[3] < 100 && g_w2_sensors[4] < 100) {  		set_motors(15, 15);  		delay(290);  		g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON); @@ -45,7 +45,7 @@ void w2_crosswalk_stroll() {  			if (g_w2_transition == 3) {  				set_motors(40, 40);  				delay(600); -				g_w2_transition	= 0; +				g_w2_transition = 0;  				w2_modes_swap(W2_M_GRID);  				return;  			} @@ -66,41 +66,42 @@ void w2_grid_crossway_detection() {  }  void w2_grid_follow_line() { -	unsigned int last_proportional=0; -	long integral=0; +	unsigned int last_proportional = 0; +	long integral				   = 0; -	while(1) { -		g_w2_position = read_line(g_w2_sensors,IR_EMITTERS_ON); +	while (1) { +		g_w2_position	 = read_line(g_w2_sensors, IR_EMITTERS_ON);  		int proportional = ((int)g_w2_position) - 2000; -		int derivative = proportional - last_proportional; +		int derivative	 = proportional - last_proportional;  		integral += proportional; -		last_proportional = proportional; -		int power_difference = proportional/20 + integral/10000 + derivative*3/2; +		last_proportional	 = proportional; +		int power_difference = proportional / 20 + integral / 10000 + derivative * 3 / 2;  		const int max = 60; -		if(power_difference > max) -		power_difference = max; -		if(power_difference < -max) -		power_difference = -max; -		 -		if(g_w2_sensors[0] >= 500 && g_w2_sensors[1] >= 250  && g_w2_sensors[2] >= 500  &&  g_w2_sensors[3] >= 250  &&g_w2_sensors[4] >= 500){ +		if (power_difference > max) power_difference = max; +		if (power_difference < -max) power_difference = -max; + +		if (g_w2_sensors[0] >= 500 && g_w2_sensors[1] >= 250 && g_w2_sensors[2] >= 500 && +			g_w2_sensors[3] >= 250 && g_w2_sensors[4] >= 500) {  			break; -		} -		else if (g_w2_sensors[0] >= 500 && g_w2_sensors[1] >= 200 && g_w2_sensors[4] < 100){ +		} else if (g_w2_sensors[0] >= 500 && g_w2_sensors[1] >= 200 && g_w2_sensors[4] < 100) {  			break; -		} -		else if(g_w2_sensors[4] >= 500 && g_w2_sensors[3] >= 200 && g_w2_sensors[0] < 100){ //for the south and west borders of the grid +		} else if (g_w2_sensors[4] >= 500 && g_w2_sensors[3] >= 200 && +				   g_w2_sensors[0] < 100) { // for the south and west borders of the grid  			break; -		} -		else if(g_w2_sensors[4] >= 500 && g_w2_sensors[3] >= 200 && g_w2_sensors[2] <100 && g_w2_sensors[0] < 100){ //sharp right corners +		} else if (g_w2_sensors[4] >= 500 && g_w2_sensors[3] >= 200 && g_w2_sensors[2] < 100 && +				   g_w2_sensors[0] < 100) { // sharp right corners  			break;  		} -		else{ -			if(power_difference < 0 && (g_w2_sensors[2] > 100 || g_w2_sensors[3] > 100 || g_w2_sensors[1] > 100) ){ -			set_motors(max+power_difference, max);} -			else if( power_difference > 0 && ( g_w2_sensors[2] > 100 || g_w2_sensors[3] > 100 || g_w2_sensors[1] > 100)){ -			set_motors(max, max-power_difference);} +		else { +			if (power_difference < 0 && +				(g_w2_sensors[2] > 100 || g_w2_sensors[3] > 100 || g_w2_sensors[1] > 100)) { +				set_motors(max + power_difference, max); +			} else if (power_difference > 0 && +					   (g_w2_sensors[2] > 100 || g_w2_sensors[3] > 100 || g_w2_sensors[1] > 100)) { +				set_motors(max, max - power_difference); +			}  		}  	}  } @@ -108,7 +109,7 @@ void w2_grid_follow_line() {  void w2_begin_location() {  	g_w2_location.x = 4;  	g_w2_location.y = 0; -	g_w2_direction  = W2_ORT_WEST; +	g_w2_direction	= W2_ORT_WEST;  }  void w2_end_destination() { @@ -325,21 +326,15 @@ void w2_mode_maze() {  		if (power_difference > max) power_difference = max;  		if (power_difference < -max) power_difference = -max; -		if (g_w2_sensors[0] < 100 && g_w2_sensors[1] < 100 && g_w2_sensors[2] < 100 && g_w2_sensors[3] < 100 && -			g_w2_sensors[4] < 100) { -		} else if (g_w2_sensors[0] >= 500 && g_w2_sensors[1] >= 250 && g_w2_sensors[2] >= 500 && -				   g_w2_sensors[3] >= 250 && g_w2_sensors[4] >= 500) { -			w2_crossway_detection(); -		} else if (g_w2_sensors[0] >= 500 && g_w2_sensors[1] >= 200 && g_w2_sensors[4] < 100) { -			w2_half_rotation_left(); -		} else { -			if (power_difference < 0 && (g_w2_sensors[2] > 100 || g_w2_sensors[3] > 100 || g_w2_sensors[1] > 100)) -				set_motors(max + power_difference, max); -			else if (power_difference > 0 && -					 (g_w2_sensors[2] > 100 || g_w2_sensors[3] > 100 || g_w2_sensors[1] > 100)) -				set_motors(max, max - power_difference); +		if (g_w2_sensors[0] < 100 && g_w2_sensors[1] < 100 && g_w2_sensors[2] < 100 && +g_w2_sensors[3] < 100 && g_w2_sensors[4] < 100) { } else if (g_w2_sensors[0] >= 500 && +g_w2_sensors[1] >= 250 && g_w2_sensors[2] >= 500 && g_w2_sensors[3] >= 250 && g_w2_sensors[4] >= +500) { w2_crossway_detection(); } else if (g_w2_sensors[0] >= 500 && g_w2_sensors[1] >= 200 && +g_w2_sensors[4] < 100) { w2_half_rotation_left(); } else { if (power_difference < 0 && +(g_w2_sensors[2] > 100 || g_w2_sensors[3] > 100 || g_w2_sensors[1] > 100)) set_motors(max + +power_difference, max); else if (power_difference > 0 && (g_w2_sensors[2] > 100 || g_w2_sensors[3] > +100 || g_w2_sensors[1] > 100)) set_motors(max, max - power_difference);  		}  	}  }  */ -  |