/
main.c
102 lines (89 loc) · 2.42 KB
/
main.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
#include <msp430.h>
#include "rangeFinder.h"
#include "testMethods.h"
#include "motorDriver.h"
/*
* main.c
* Author: Ian R Goodbody
* Function: Implements maze navigation
*/
void main(void)
{
WDTCTL = WDTPW | WDTHOLD; // Stop watchdog timer
initRangeFinders();
initMotors();
stop();
P1DIR |= LEFT_LED|RIGHT_LED;
P1OUT &= ~(LEFT_LED|RIGHT_LED);
int fBuffer[BUFFER_LN]; // Code probably unnecessary but it cannot hurt
int lBuffer[BUFFER_LN];
int rBuffer[BUFFER_LN];
int fMedian;
int lMedian;
int rMedian;
unsigned int lWheelSpeed = 4040;
unsigned int rWheelSpeed = 4000;
unsigned int runTime = 350;
int i;
fillBuffers(fBuffer, lBuffer, rBuffer);
while(1)
{
P1OUT &= ~(LEFT_LED|RIGHT_LED); // turn off LEDs
stop();
for (i = BUFFER_LN; i > 0; i--)
{
readFront(fBuffer);
waitMiliseconds(1);
readLeft(lBuffer);
waitMiliseconds(1);
readRight(rBuffer);
waitMiliseconds(1);
}
fMedian = median(fBuffer);
lMedian = median(lBuffer);
rMedian = median(rBuffer);
if(fMedian < 0x01F0) // Crash into wall test; ~2.5 inch threshold
{
if(lMedian < 0x01FF) // There is no wall remotely close to the left side,
{ // Initiate sweeping turn
lWheelSpeed = 2600;
rWheelSpeed = 5000;
}
else if(lMedian < 0x0266) // Getting too far from the wall start turning in
{
P1OUT |= LEFT_LED; // Red LED on, green off
P1OUT &= ~RIGHT_LED;
rWheelSpeed = 4270;
lWheelSpeed = 4040;
}
else if(lMedian > 0x02E4) // Getting too close to the wall start turning out
{
P1OUT |= RIGHT_LED; // Green LED on, red off
P1OUT &= ~LEFT_LED;
rWheelSpeed = 3900;
lWheelSpeed = 4040;
}
else // Acceptable distance from wall, cruise forward normally
{
//P1OUT |= RIGHT_LED;
P1OUT &= ~(RIGHT_LED|LEFT_LED);
rWheelSpeed = 4000;
lWheelSpeed = 4040;
}
setLeftWheel(lWheelSpeed, FORWARD);
setRightWheel(rWheelSpeed, FORWARD);
runTime = 350;
}
else
{
// About to run into a wall, initiate hard right turn
P1OUT |= RIGHT_LED|LEFT_LED;
setLeftWheel(5080,FORWARD);
setRightWheel(5260, BACKWARD);
runTime = 450;
}
go();
waitMiliseconds(runTime);
}
}
// Merry Christmas Captain Trimble!