This repository has been archived by the owner on Jun 14, 2021. It is now read-only.
-
Notifications
You must be signed in to change notification settings - Fork 0
/
subroutines.c
198 lines (174 loc) · 6.44 KB
/
subroutines.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
/*
This file contains the subroutines that the robot performs when it is in each
mode. The code is placed in this source file in order to de-clutter the main
function and improve readability.
The robot operates on 4 subroutines, searching, moving forwards, returning and
displaying the results
A 'wait for inputs' routine is also defined here and is used on startup to
stabilise the signal processing and to wait for a button press to start
05.03.20
*/
#include <xc.h>
#include <pic18f4331.h>
#include "dc_motor.h"
#include "RFID.h"
#include "signal_processing.h"
#include "subroutines.h"
#include "LCDIO.h"
// This subroutine scans for the beacon by turning left on the spot
volatile char scanForBeacon(struct DC_motor *mL, struct DC_motor *mR, int speed,
struct Movement_storage *move, volatile char *exit_flag)
{
move-> move_type[move->move_number] = 1; // store move type as left
turnLeft(mL,mR,speed); // continuously turn on the spot
clearLCD();
LCDString("SEARCHING"); // Display on LCD
// Runs until the beacon is located or RFID is scanned
while(1)
{
// First, acquire the raw signal using the motion feedback module
unsigned int raw_data = (unsigned int)((CAP1BUFH << 8) | CAP1BUFL);
// Now, classify the signals to find if we are looking at the beacon
char beacon_location = classify_data(raw_data);
// if RFID read, return home
if(*exit_flag == 1)
{
return 2; // go to returnHome subroutine
}
// if beacon is straight ahead, exit this subroutine
else if(beacon_location == 1)
{
move-> move_number += 1; // increment move number
return 1; // go to moveToBeacon subroutine
}
}
}
// This subroutine moves the robot forwards until it either finds the beacon
// or loses the bomb - in which case it starts searching again
volatile char moveToBeacon(struct DC_motor *mL, struct DC_motor *mR, int speed,
struct Movement_storage *move, volatile char *exit_flag)
{
move->move_type[move->move_number] = 0; // store move type as forwards
moveForward(mL,mR,speed); // move robot forwards
clearLCD();
LCDString("MOVING TO BOMB");
/*
This stores the number of consecutive times the robot misses the bomb.
Once it is over the threshold value, the robot starts searching again.
Increasing the threshold to around 20000 means the robot rarely adjusts
its course... around 10000 means the robot adjusts frequently
*/
int error_counter = 0;
const int ERROR_THRESHOLD = 17000;
// Runs until RFID has been scanned and break statement executes
while(1)
{
// First, acquire the raw signal using the motion feedback module
unsigned int raw_data = (unsigned int)((CAP1BUFH << 8) | CAP1BUFL);
// Now, classify the signals to find if we are looking at the beacon
char beacon_location = classify_data(raw_data);
// if RFID read, return home
if(*exit_flag == 1)
{
return 2; // go to returnHome subroutine
}
// if beacon lost, add 1 to error counter
else if(beacon_location == 0)
{
error_counter += 1;
}
// if beacon found, reset error counter to zero
else
{
error_counter = 0;
}
// if the beacon has been lost for more cycles than the threshold, start
// searching again
if(error_counter >= ERROR_THRESHOLD)
{
move-> move_number += 1; // increment move number
// if we run out of space to store the moves, return home
if(move->move_number >= 19)
{
return 2; // go to returnHome subroutine
}
else
{
return 0; // go back to sweep mode
}
}
}
}
// This subroutine makes the robot return to its starting position
volatile char returnHome(struct DC_motor *mL, struct DC_motor *mR, int move_speed,
int search_speed, struct Movement_storage *move)
{
// stop and display returning on LCD
stop(mL,mR,move_speed);
clearLCD();
LCDString("RETURNING HOME");
// Go back through array of moves and undo each previous move
for(int i=move->move_number;move->move_number >0; move->move_number--)
{
// if move was forward
if(move->move_type[move->move_number] == 0)
{
// reverse for same time as initial move
moveBackward(mL,mR,move_speed);
while(move->time_taken[move->move_number] > 0);
}
// if move was left
else if(move->move_type[move->move_number] == 1)
{
// turn right for same time as initial move
turnRight(mL,mR,search_speed);
while(move->time_taken[move->move_number] > 0);
}
}
return 3; // go to stopAndDisplay subroutine
}
// This subroutine makes the robot stop and wait for further inputs at the end
volatile char stopAndDisplay(struct DC_motor *mL,struct DC_motor *mR, int speed,
volatile char RFID_buffer[])
{
stop(mL, mR,speed); // stop moving
// Display RFID and checksum result
display_RFID(RFID_buffer);
check_RFID(RFID_buffer);
// Now, simply wait for button press to reset robot
while(1)
{
while(PORTDbits.RD2 == 1) // when button is pressed
{
clearLCD();
LCDString("RESETTING ROBOT");
for(int i=0; i<10;i++)
{
__delay_ms(100);
}
clearLCD();
Reset();
}
}
}
// This function is allows the user to press the button to start the search
void waitForInput(void)
{
// display some text for the user
clearLCD();
SetLine(1);
LCDString("PRESS BUTTON");
SetLine(2);
LCDString("TO START SEARCH");
// wait until button is pressed
while(PORTDbits.RD2 == 0);
// display text to confirm start
clearLCD();
SetLine(1);
LCDString("STARTING SEARCH");
for(int i=0; i<10;i++)
{
__delay_ms(100);
}
clearLCD();
}