root / branches / autonomous_recharging / code / projects / autonomous_recharging / dragonfly / seeking.c @ 138
History | View | Annotate | Download (2.37 KB)
1 | 93 | jykong | #include "seeking.h" |
---|---|---|---|
2 | |||
3 | #include <move.h> |
||
4 | 105 | bcoltin | #include <serial.h> |
5 | 93 | jykong | |
6 | 105 | bcoltin | #include "recharge_i2c.h" |
7 | #include "recharge_defs.h" |
||
8 | |||
9 | 93 | jykong | #include <wl_token_ring.h> |
10 | |||
11 | #define SEEK_WITH_BOM 1 |
||
12 | #define SEEK_WITH_HOMING 2 |
||
13 | |||
14 | 126 | bcoltin | #define SEEKING_BOM_VELOCITY -170 |
15 | 100 | bcoltin | #define SEEKING_HOMING_VELOCITY -160 |
16 | |||
17 | // values for homing sensor readings
|
||
18 | #define LEFT_LOW 6 |
||
19 | #define LEFT_HIGH 7 |
||
20 | #define CENTER_LOW 18 |
||
21 | #define CENTER_HIGH 19 |
||
22 | #define RIGHT_LOW 12 |
||
23 | #define RIGHT_HIGH 13 |
||
24 | |||
25 | //function prototypes
|
||
26 | int seek_station_with_bom(int station); |
||
27 | int seek_station_with_homing_sensor(void); |
||
28 | |||
29 | 105 | bcoltin | int seeking_state = SEEK_WITH_BOM;
|
30 | 93 | jykong | |
31 | void seek_station(int station) |
||
32 | { |
||
33 | 105 | bcoltin | if (seeking_state == SEEK_WITH_BOM)
|
34 | seeking_state = seek_station_with_bom(station); |
||
35 | 93 | jykong | else
|
36 | 105 | bcoltin | seeking_state = seek_station_with_homing_sensor(); |
37 | 93 | jykong | } |
38 | |||
39 | 120 | bcoltin | int old_reading = 0; |
40 | |||
41 | 93 | jykong | int seek_station_with_bom(int station) |
42 | { |
||
43 | int charging_loc = wl_token_get_my_sensor_reading(station);
|
||
44 | 120 | bcoltin | if (charging_loc != old_reading)
|
45 | { |
||
46 | RECHARGE_DEBUG_PRINT("BOM reading: ");
|
||
47 | RECHARGE_DEBUG_PUTI(charging_loc); |
||
48 | RECHARGE_DEBUG_PRINT("\n");
|
||
49 | old_reading = charging_loc; |
||
50 | } |
||
51 | 93 | jykong | |
52 | //move to charging station if charging_loc != -1
|
||
53 | //at this point, wl_csloc is set to the max bom reading
|
||
54 | if(charging_loc != -1) |
||
55 | { |
||
56 | int desiredbom = 12; // for moving backwards |
||
57 | int err = desiredbom - charging_loc;
|
||
58 | if (err < -8) |
||
59 | err += 16;
|
||
60 | else if (err > 8) |
||
61 | err -= 16;
|
||
62 | int w = -err * 2; |
||
63 | int v = SEEKING_BOM_VELOCITY;
|
||
64 | 137 | bcoltin | |
65 | 126 | bcoltin | move(v, w); |
66 | 93 | jykong | } |
67 | |||
68 | 137 | bcoltin | //check if we can use homing sensor
|
69 | 126 | bcoltin | int widthcount = recharge_i2c_get_homing_reading();
|
70 | 93 | jykong | |
71 | if(widthcount>= 4) |
||
72 | 126 | bcoltin | return SEEK_WITH_HOMING;
|
73 | 93 | jykong | return SEEK_WITH_BOM;
|
74 | } |
||
75 | |||
76 | int seek_station_with_homing_sensor(void) |
||
77 | { |
||
78 | //TODO: create function in homing.h to return LEFT, FORWARD, RIGHT, or NONE
|
||
79 | 105 | bcoltin | int widthcount = recharge_i2c_get_homing_reading();
|
80 | 93 | jykong | |
81 | if (widthcount < LEFT_LOW) //can't use beacon, switch to BOM |
||
82 | return SEEK_WITH_BOM;
|
||
83 | else if(widthcount >= CENTER_LOW) |
||
84 | move(SEEKING_HOMING_VELOCITY, 0);
|
||
85 | else
|
||
86 | { |
||
87 | if(widthcount >= LEFT_LOW && widthcount <= LEFT_HIGH)
|
||
88 | move(SEEKING_HOMING_VELOCITY, (widthcount) / 3);
|
||
89 | else if (widthcount >= RIGHT_LOW && widthcount <= RIGHT_HIGH) |
||
90 | move(SEEKING_HOMING_VELOCITY, -(widthcount) / 3);
|
||
91 | else
|
||
92 | { |
||
93 | 100 | bcoltin | RECHARGE_DEBUG_PRINT("Unexpected homing sensor value of ");
|
94 | RECHARGE_DEBUG_PUTI(widthcount); |
||
95 | RECHARGE_DEBUG_PRINT(".\n");
|
||
96 | 93 | jykong | } |
97 | } |
||
98 | |||
99 | return SEEK_WITH_HOMING;
|
||
100 | } |