/
race2wall.c
99 lines (75 loc) · 1.73 KB
/
race2wall.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
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <sys/socket.h>
#include <arpa/inet.h>
#include <assert.h>
#include <sys/select.h>
#include <math.h>
#include "picomms.h"
#include "robotMovement.h"
#define SPEED 40
// Basic Right wall following code
void wallFollowingR()
{
int rightSideIR, leftSideIR, rightFrontIR, leftFrontIR, distAhead, i;
while (1)
{
get_front_ir_dists(&leftFrontIR, &rightFrontIR);
get_side_ir_dists(&leftSideIR, &rightSideIR);
distAhead = get_us_dist();
if (distAhead < 45){
if (leftFrontIR - rightFrontIR > 0){
set_motors(-SPEED, SPEED);
}
else {
set_motors(20, -20);
}
}
else if (rightFrontIR > 30 && rightSideIR > 25)
{ set_motors(SPEED+30, SPEED-8); } //turn right
else if (rightFrontIR < 20 )
{ set_motors (SPEED-10, SPEED+25); } //turn left
else
{set_motors(SPEED+8, SPEED+8); } // Straight
}
}
// Basic Left wall following code
void wallFollowingL()
{
int rightSideIR, leftSideIR, rightFrontIR, leftFrontIR, distAhead, i;
while (1)
{
get_front_ir_dists(&leftFrontIR, &rightFrontIR);
get_side_ir_dists(&leftSideIR, &rightSideIR);
distAhead = get_us_dist();
if (distAhead < 40){
if (leftFrontIR - rightFrontIR > 0){
set_motors(-SPEED, SPEED);
}
else {
set_motors(SPEED, -SPEED);
}
}
if (leftFrontIR > 30 )
{ set_motors(SPEED-10, SPEED+10); } //turn left
else if (leftFrontIR < 15 )
{ set_motors (SPEED+10, SPEED-10); } //turn right
else
{set_motors(SPEED, SPEED); } // Straight
}
}
void setAnglesStart()
{
set_ir_angle(RIGHT, 35);
set_ir_angle(LEFT, -35);
}
int main ()
{
connect_to_robot();
initialize_robot();
setAnglesStart();
//wallFollowingR();
wallFollowingL();
}