-
Notifications
You must be signed in to change notification settings - Fork 0
/
Robot.cpp
183 lines (154 loc) · 4.11 KB
/
Robot.cpp
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
#include "Robot.h"
#include "Handle.h"
#include "Event.h"
#include "Pose.h"
#include "Localization.h"
#include "Action.h"
#include "Frequency.h"
#include "Site.h"
extern SensorController g_sensorCtrl;
extern MotorController g_motorCtrl;
extern ServoController g_servoCtrl;
extern Event g_eventCenter;
//Declare the event and robot status variables
STATUS_ROBOT robotStatus;
extern EVENT currentEvent;
extern Localization g_localization;
extern MOVEMENT_STATUS g_movement;
EVENT g_event;
extern MOVEMENT_STATUS g_prev_movement;
Pose pose(0,0,0);
Robot::Robot(void)
{
prt_debug("Creating robot\n");
isInit = false;
}
Robot::~Robot(void)
{
;
}
bool Robot::hasHitBumper()
{
return ( (currentEvent == EVENT_HIT_FRONT)
|| (currentEvent == EVENT_HIT_FRONT_LEFT)
|| (currentEvent == EVENT_HIT_FRONT_RIGHT));
}
//Use this thread to check for changes in the robots movement and update the odometry accordingly
void * updatingOdometry(void * param){
while(1){
if (g_movement!=g_prev_movement){
pose.updateOdometry();
}
}
}
//The main control loop
void Robot::run(void)
{
Handle handle;
Event event;
robotStatus = STATUS_ROBOT_EXPLORING;
currentEvent = EVENT_NULL;
g_servoCtrl.setPos(VALUE_SERVO_POS_MID);
//Set the initial position
g_localization.initializePosition(0,0,0);
cout<<"Robot::run"<<endl;
sonarScanSite();
#if 1
int param=1;
int i=1;
pthread_t odometryThread;
pthread_create(&odometryThread, NULL, updatingOdometry,(void*)¶m);
cout<<"Finished scan, moving forward"<<endl;
ActMoveForward(6000);
trainSensors();
while (1)
{
//If the robot is exploring, run this set of behaviours
if(robotStatus == STATUS_ROBOT_EXPLORING)
{
//cout<<"Entered Exploring"<<endl;
if (hasHitBumper())
{
cout<<"Handling collision"<<endl;
handle.collision();
}
else if (currentEvent == EVENT_DETECT_BLACK)
{
cout<<"Handling Docking"<<endl;
handle.docking();
}
else if (currentEvent == EVENT_TRIGGER_ACTIVATED)
{
cout<<"Handling localization"<<endl;
handle.localization();
}
else if( (currentEvent == EVENT_LOCALIZING)
&& (g_movement == STOPPED))
{
handle.reScan();
}
else
{
;
}
}
//If the robot is docking, run this set of behaviours
else if (robotStatus == STATUS_ROBOT_DOCKING)
{
if ( hasHitBumper()
&& (robotStatus != STATUS_ROBOT_DETECTING_FREQUENCY))
{
g_event= currentEvent;
cout<<"Robot: The current event is: "<<currentEvent<<" and the event is: "<<g_event<<endl;
handle.triggerSwitch();
}
}
else
{
;
}
}
#endif
printf("Robot::run - DONE~!\n");
}
//Initialisation procedure for the motors, sensors and servo
void Robot::init(void)
{
if (isInit)
{
return;
}
initSites();
cout<<"Intialised Sites"<<endl;
g_motorCtrl.init();
g_sensorCtrl.init();
g_servoCtrl.init();
isInit = true;
}
//Called when the program has ended
void Robot::fin(void)
{
printf("Robot::fin\n");
g_motorCtrl.fin();
g_sensorCtrl.fin();
g_servoCtrl.fin();
}
//The training algorithm used to determine the black tape threshold
void Robot::trainSensors()
{
int lightSensor =0;
int numberSamples=0;
int lightTotal = 0;
while(numberSamples<100)
{
lightSensor = g_sensorCtrl.getSensorValue(INDEX_SENSOR_LIGHT_UNDER);
if(lightSensor>100){
//cout<<"Accepted light value: "<<lightSensor<<endl;
lightTotal = lightTotal + lightSensor;
numberSamples++;
}
}
lightTotal = lightTotal/numberSamples;
VALUE_BLACK_TAPE_MAX = lightTotal*0.6;
VALUE_BLACK_TAPE_MIN = VALUE_BLACK_TAPE_MAX*0.4;
}