aboutsummaryrefslogtreecommitdiff
path: root/robot
diff options
context:
space:
mode:
Diffstat (limited to 'robot')
-rw-r--r--robot/mode_chrg.c24
-rw-r--r--robot/mode_grid.c83
-rw-r--r--robot/mode_grid.h8
-rw-r--r--robot/mode_maze.c10
-rw-r--r--robot/movement.c67
5 files changed, 89 insertions, 103 deletions
diff --git a/robot/mode_chrg.c b/robot/mode_chrg.c
index 64497e2..5b733af 100644
--- a/robot/mode_chrg.c
+++ b/robot/mode_chrg.c
@@ -1,8 +1,8 @@
#include "mode_chrg.h"
#include "mode_grid.h"
-#include "orangutan_shim.h"
-#include "movement.h"
#include "modes.h"
+#include "movement.h"
+#include "orangutan_shim.h"
int g_w2_charged_status;
@@ -22,20 +22,19 @@ void w2_home() {
set_motors(0, 0);
play_frequency(300, 500, 7);
delay_ms(600);
- g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON);
+ g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON);
g_w2_charged_status = 1;
clear();
delay_ms(2000);
}
-
void w2_charge_cross_walk() {
if (g_w2_transition == 0) {
set_motors(-30, 30);
delay(50);
}
- 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(550);
g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON);
@@ -44,12 +43,12 @@ void w2_charge_cross_walk() {
// clear();
print("WALK");
g_w2_transition++;
- if (g_w2_transition == 3) { //TODO: document g_w2_transition
+ if (g_w2_transition == 3) { // TODO: document g_w2_transition
set_motors(40, 40);
delay(600);
set_motors(0, 0);
- g_w2_transition = 0;
+ g_w2_transition = 0;
w2_modes_swap(W2_M_MAZE);
break;
}
@@ -60,7 +59,6 @@ void w2_charge_cross_walk() {
}
}
-
void w2_mode_chrg() {
unsigned int last_proportional = 0;
long integral = 0;
@@ -99,8 +97,8 @@ void w2_mode_chrg() {
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) {
+ 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) {
w2_charge_cross_walk();
if (g_w2_mode_history[g_w2_mode_history_index] == W2_M_MAZE) {
break;
@@ -119,8 +117,8 @@ void w2_mode_chrg() {
w2_maze_rotation_half_left();
}
- 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) {
+ 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) {
clear();
w2_maze_rotation_half_left();
} else {
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);
}
}
}
*/
-
diff --git a/robot/mode_grid.h b/robot/mode_grid.h
index 3ba3db0..2df0d33 100644
--- a/robot/mode_grid.h
+++ b/robot/mode_grid.h
@@ -13,12 +13,7 @@ extern int g_w2_transition;
void w2_crosswalk_stroll();
-typedef enum {
- W2_ORT_NORTH,
- W2_ORT_EAST,
- W2_ORT_SOUTH,
- W2_ORT_WEST
-} w2_e_orientation;
+typedef enum { W2_ORT_NORTH, W2_ORT_EAST, W2_ORT_SOUTH, W2_ORT_WEST } w2_e_orientation;
// enum w2_e_section { mazeMode, gridMode, chargeMode } g_w2_parcour_mode;
@@ -28,4 +23,3 @@ typedef struct {
} w2_s_grid_coordinate;
extern w2_s_grid_coordinate g_w2_order[4];
-
diff --git a/robot/mode_maze.c b/robot/mode_maze.c
index da3fa2a..eb75e3c 100644
--- a/robot/mode_maze.c
+++ b/robot/mode_maze.c
@@ -1,19 +1,19 @@
#include "mode_maze.h"
-#include "orangutan_shim.h"
-#include "movement.h"
#include "mode_grid.h"
+#include "movement.h"
+#include "orangutan_shim.h"
unsigned int g_w2_last_proportional = 0;
long g_w2_integral = 0;
void w2_mode_maze() {
// PID controller
- g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON);
+ g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON);
int proportional = ((int)g_w2_position) - 2000;
- int derivative = proportional - g_w2_last_proportional;
+ int derivative = proportional - g_w2_last_proportional;
g_w2_integral += proportional;
g_w2_last_proportional = proportional;
- int power_difference = proportional / 20 + g_w2_integral / 10000 + derivative * 3 / 2;
+ int power_difference = proportional / 20 + g_w2_integral / 10000 + derivative * 3 / 2;
const int max = 60;
if (power_difference > max) power_difference = max;
diff --git a/robot/movement.c b/robot/movement.c
index 82829c1..61a944b 100644
--- a/robot/movement.c
+++ b/robot/movement.c
@@ -1,60 +1,59 @@
-#include "orangutan_shim.h"
#include "movement.h"
+#include "orangutan_shim.h"
-unsigned int g_w2_sensors[5] = {0};
-unsigned int g_w2_position = 0;
+unsigned int g_w2_sensors[5] = {0};
+unsigned int g_w2_position = 0;
-void w2_maze_rotation_full(){
- set_motors(0,0);
+void w2_maze_rotation_full() {
+ set_motors(0, 0);
delay_ms(500);
- set_motors(60,-60);
+ set_motors(60, -60);
delay_ms(540);
- set_motors(0,0);
- g_w2_position = read_line(g_w2_sensors,IR_EMITTERS_ON);
+ set_motors(0, 0);
+ g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON);
delay_ms(500);
}
-void w2_maze_rotation_half_left(){
- set_motors(0,0);
- set_motors(50,50);
+void w2_maze_rotation_half_left() {
+ set_motors(0, 0);
+ set_motors(50, 50);
delay_ms(150);
- set_motors(-30,30);
+ set_motors(-30, 30);
delay_ms(600);
- set_motors(0,0);
- g_w2_position = read_line(g_w2_sensors,IR_EMITTERS_ON);
+ set_motors(0, 0);
+ g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON);
delay_ms(500);
-
}
-void w2_maze_rotation_half_right(){
- set_motors(0,0);
- set_motors(50,50);
+void w2_maze_rotation_half_right() {
+ set_motors(0, 0);
+ set_motors(50, 50);
delay_ms(150);
- set_motors(30,-30);
+ set_motors(30, -30);
delay_ms(600);
- set_motors(0,0);
- set_motors(50,50);
+ set_motors(0, 0);
+ set_motors(50, 50);
delay_ms(150);
- g_w2_position = read_line(g_w2_sensors,IR_EMITTERS_ON);
+ g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON);
delay_ms(500);
}
-void w2_grid_rotation_full(){
- set_motors(60,-60);
+void w2_grid_rotation_full() {
+ set_motors(60, -60);
delay_ms(540);
- set_motors(10,10);
- g_w2_position = read_line(g_w2_sensors,IR_EMITTERS_ON);
+ set_motors(10, 10);
+ g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON);
}
-void w2_grid_rotation_half_left(){
- set_motors(-30,30);
+void w2_grid_rotation_half_left() {
+ set_motors(-30, 30);
delay_ms(600);
- set_motors(10,10);
- g_w2_position = read_line(g_w2_sensors,IR_EMITTERS_ON);
+ set_motors(10, 10);
+ g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON);
}
-void w2_grid_rotation_half_right(){
- set_motors(30,-30);
+void w2_grid_rotation_half_right() {
+ set_motors(30, -30);
delay_ms(600);
- set_motors(10,10);
- g_w2_position = read_line(g_w2_sensors,IR_EMITTERS_ON);
+ set_motors(10, 10);
+ g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON);
}