User | Revision | Line number | New contents of line |
sowmy87 |
0:637fe5c74b2d
|
1
|
#ifndef __ROBOT__C
|
sowmy87 |
0:637fe5c74b2d
|
2
|
#define __ROBOT__C
|
sowmy87 |
0:637fe5c74b2d
|
3
|
|
sowmy87 |
0:637fe5c74b2d
|
4
|
#include "Robot.h"
|
sowmy87 |
0:637fe5c74b2d
|
5
|
|
sowmy87 |
0:637fe5c74b2d
|
6
|
SRF05 sensor(p24,p25);
|
sowmy87 |
0:637fe5c74b2d
|
7
|
|
sowmy87 |
0:637fe5c74b2d
|
8
|
|
sowmy87 |
0:637fe5c74b2d
|
9
|
Serial pcr(USBTX, USBRX);
|
sowmy87 |
0:637fe5c74b2d
|
10
|
|
sowmy87 |
0:637fe5c74b2d
|
11
|
Robot::Robot() {
|
sowmy87 |
0:637fe5c74b2d
|
12
|
led1 = new DigitalOut(LED1);
|
sowmy87 |
0:637fe5c74b2d
|
13
|
led2 = new DigitalOut(LED2);
|
sowmy87 |
0:637fe5c74b2d
|
14
|
led3 = new DigitalOut(LED3);
|
sowmy87 |
0:637fe5c74b2d
|
15
|
|
sowmy87 |
0:637fe5c74b2d
|
16
|
compass=new CMPS03(p9,p10, CMPS03_DEFAULT_I2C_ADDRESS);
|
sowmy87 |
0:637fe5c74b2d
|
17
|
wait(1);
|
sowmy87 |
0:637fe5c74b2d
|
18
|
desiredHeading=compass->readBearing();
|
sowmy87 |
0:637fe5c74b2d
|
19
|
|
sowmy87 |
0:637fe5c74b2d
|
20
|
pid = new PID(1.0, 0.00, 0.01, PID_RATE);
|
sowmy87 |
0:637fe5c74b2d
|
21
|
pid->setInputLimits(-450,450);
|
sowmy87 |
0:637fe5c74b2d
|
22
|
pid->setOutputLimits(-MAX_ROBOT_SPEED,MAX_ROBOT_SPEED);
|
sowmy87 |
0:637fe5c74b2d
|
23
|
pid->setBias(0);
|
sowmy87 |
0:637fe5c74b2d
|
24
|
pid->setMode(AUTO_MODE);
|
sowmy87 |
0:637fe5c74b2d
|
25
|
pid->setSetPoint(0);
|
sowmy87 |
0:637fe5c74b2d
|
26
|
|
sowmy87 |
0:637fe5c74b2d
|
27
|
range= new float[55];
|
sowmy87 |
0:637fe5c74b2d
|
28
|
returnRange = new float[11];
|
sowmy87 |
0:637fe5c74b2d
|
29
|
// sensor1=new SRF05 (p22, p23);
|
sowmy87 |
0:637fe5c74b2d
|
30
|
// sensor=new SRF05 (p24, p25);
|
sowmy87 |
0:637fe5c74b2d
|
31
|
|
sowmy87 |
0:637fe5c74b2d
|
32
|
isFullScan=0;
|
sowmy87 |
0:637fe5c74b2d
|
33
|
readingNumber=0;
|
sowmy87 |
0:637fe5c74b2d
|
34
|
|
sowmy87 |
0:637fe5c74b2d
|
35
|
servo=new Servo (p21);
|
sowmy87 |
0:637fe5c74b2d
|
36
|
servo->calibrate(.0009, 180);
|
sowmy87 |
0:637fe5c74b2d
|
37
|
servoPosition=5;
|
sowmy87 |
0:637fe5c74b2d
|
38
|
servoDirection=1;
|
sowmy87 |
0:637fe5c74b2d
|
39
|
*servo=(float)servoPosition/10;
|
sowmy87 |
0:637fe5c74b2d
|
40
|
wait(1);
|
sowmy87 |
0:637fe5c74b2d
|
41
|
|
sowmy87 |
0:637fe5c74b2d
|
42
|
|
sowmy87 |
0:637fe5c74b2d
|
43
|
|
sowmy87 |
0:637fe5c74b2d
|
44
|
ticker1.attach(this, &Robot::TickTick, PID_RATE*2);
|
sowmy87 |
0:637fe5c74b2d
|
45
|
ticker2.attach(this, &Robot::TackTack, SERVO_RATE);
|
sowmy87 |
0:637fe5c74b2d
|
46
|
ticker3.attach(this, &Robot::FlashLEDs, 1);
|
sowmy87 |
0:637fe5c74b2d
|
47
|
}
|
sowmy87 |
0:637fe5c74b2d
|
48
|
Robot::~Robot() {
|
sowmy87 |
0:637fe5c74b2d
|
49
|
delete compass;
|
sowmy87 |
0:637fe5c74b2d
|
50
|
delete pid;
|
sowmy87 |
0:637fe5c74b2d
|
51
|
// delete sensor;
|
sowmy87 |
0:637fe5c74b2d
|
52
|
delete range;
|
sowmy87 |
0:637fe5c74b2d
|
53
|
delete returnRange;
|
sowmy87 |
0:637fe5c74b2d
|
54
|
delete servo;
|
sowmy87 |
0:637fe5c74b2d
|
55
|
}
|
sowmy87 |
0:637fe5c74b2d
|
56
|
|
sowmy87 |
0:637fe5c74b2d
|
57
|
int Robot::GetDesiredHeading() {
|
sowmy87 |
0:637fe5c74b2d
|
58
|
return desiredHeading/10;
|
sowmy87 |
0:637fe5c74b2d
|
59
|
}
|
sowmy87 |
0:637fe5c74b2d
|
60
|
int Robot::GetActualHeading() {
|
sowmy87 |
0:637fe5c74b2d
|
61
|
return actualHeading/10;
|
sowmy87 |
0:637fe5c74b2d
|
62
|
}
|
sowmy87 |
0:637fe5c74b2d
|
63
|
int Robot::SetDediredHeading(int deg) {
|
sowmy87 |
0:637fe5c74b2d
|
64
|
deg%=360;
|
sowmy87 |
0:637fe5c74b2d
|
65
|
if (deg<0)deg=deg+360;
|
sowmy87 |
0:637fe5c74b2d
|
66
|
desiredHeading=deg*10;
|
sowmy87 |
0:637fe5c74b2d
|
67
|
return deg;
|
sowmy87 |
0:637fe5c74b2d
|
68
|
}
|
sowmy87 |
0:637fe5c74b2d
|
69
|
|
sowmy87 |
0:637fe5c74b2d
|
70
|
void Robot::SetSpeed(int spd) {
|
sowmy87 |
0:637fe5c74b2d
|
71
|
int tmp=0;
|
sowmy87 |
0:637fe5c74b2d
|
72
|
desiredSpeed=spd%MAX_ROBOT_SPEED;
|
sowmy87 |
0:637fe5c74b2d
|
73
|
if (desiredSpeed>0)tmp=desiredSpeed+50;
|
sowmy87 |
0:637fe5c74b2d
|
74
|
if (desiredSpeed<0)tmp=desiredSpeed-50;
|
sowmy87 |
0:637fe5c74b2d
|
75
|
base.SetPWM(tmp);
|
sowmy87 |
0:637fe5c74b2d
|
76
|
}
|
sowmy87 |
0:637fe5c74b2d
|
77
|
|
sowmy87 |
0:637fe5c74b2d
|
78
|
void Robot::SetSpeed(int lSpd, int rSpd) {
|
sowmy87 |
0:637fe5c74b2d
|
79
|
lSpd%=MAX_ROBOT_SPEED;
|
sowmy87 |
0:637fe5c74b2d
|
80
|
rSpd%=MAX_ROBOT_SPEED;
|
sowmy87 |
0:637fe5c74b2d
|
81
|
if (lSpd>0)lSpd+=50;
|
sowmy87 |
0:637fe5c74b2d
|
82
|
if (lSpd<0)lSpd-=50;
|
sowmy87 |
0:637fe5c74b2d
|
83
|
if (rSpd>0)rSpd+=50;
|
sowmy87 |
0:637fe5c74b2d
|
84
|
if (rSpd<0)rSpd-=50;
|
sowmy87 |
0:637fe5c74b2d
|
85
|
base.SetPWM(lSpd,rSpd);
|
sowmy87 |
0:637fe5c74b2d
|
86
|
}
|
sowmy87 |
0:637fe5c74b2d
|
87
|
void Robot::SetLeftSpeed(int spd) {
|
sowmy87 |
0:637fe5c74b2d
|
88
|
spd%=MAX_ROBOT_SPEED;
|
sowmy87 |
0:637fe5c74b2d
|
89
|
if (spd>0)spd+=50;
|
sowmy87 |
0:637fe5c74b2d
|
90
|
if (spd<0)spd-=50;
|
sowmy87 |
0:637fe5c74b2d
|
91
|
base.SetLeftPWM(spd);
|
sowmy87 |
0:637fe5c74b2d
|
92
|
}
|
sowmy87 |
0:637fe5c74b2d
|
93
|
|
sowmy87 |
0:637fe5c74b2d
|
94
|
void Robot::SetRightSpeed(int spd) {
|
sowmy87 |
0:637fe5c74b2d
|
95
|
spd%=MAX_ROBOT_SPEED;
|
sowmy87 |
0:637fe5c74b2d
|
96
|
if (spd>0)spd+=50;
|
sowmy87 |
0:637fe5c74b2d
|
97
|
if (spd<0)spd-=50;
|
sowmy87 |
0:637fe5c74b2d
|
98
|
// pcr.printf("R:%i\t\t",spd);
|
sowmy87 |
0:637fe5c74b2d
|
99
|
base.SetRightPWM(spd);
|
sowmy87 |
0:637fe5c74b2d
|
100
|
}
|
sowmy87 |
0:637fe5c74b2d
|
101
|
|
sowmy87 |
0:637fe5c74b2d
|
102
|
float Robot::GetRange(int position) {
|
sowmy87 |
0:637fe5c74b2d
|
103
|
switch (position) {
|
sowmy87 |
0:637fe5c74b2d
|
104
|
case 0:
|
sowmy87 |
0:637fe5c74b2d
|
105
|
return range0/5;
|
sowmy87 |
0:637fe5c74b2d
|
106
|
case 1:
|
sowmy87 |
0:637fe5c74b2d
|
107
|
return range1/5;
|
sowmy87 |
0:637fe5c74b2d
|
108
|
case 2:
|
sowmy87 |
0:637fe5c74b2d
|
109
|
return range2/5;
|
sowmy87 |
0:637fe5c74b2d
|
110
|
case 3:
|
sowmy87 |
0:637fe5c74b2d
|
111
|
return range3/5;
|
sowmy87 |
0:637fe5c74b2d
|
112
|
case 4:
|
sowmy87 |
0:637fe5c74b2d
|
113
|
return range4/5;
|
sowmy87 |
0:637fe5c74b2d
|
114
|
case 5:
|
sowmy87 |
0:637fe5c74b2d
|
115
|
return range5/5;
|
sowmy87 |
0:637fe5c74b2d
|
116
|
case 6:
|
sowmy87 |
0:637fe5c74b2d
|
117
|
return range6/5;
|
sowmy87 |
0:637fe5c74b2d
|
118
|
case 7:
|
sowmy87 |
0:637fe5c74b2d
|
119
|
return range7/5;
|
sowmy87 |
0:637fe5c74b2d
|
120
|
case 8:
|
sowmy87 |
0:637fe5c74b2d
|
121
|
return range8/5;
|
sowmy87 |
0:637fe5c74b2d
|
122
|
case 9:
|
sowmy87 |
0:637fe5c74b2d
|
123
|
return range9/5;
|
sowmy87 |
0:637fe5c74b2d
|
124
|
case 10:
|
sowmy87 |
0:637fe5c74b2d
|
125
|
return range10/5;
|
sowmy87 |
0:637fe5c74b2d
|
126
|
default:
|
sowmy87 |
0:637fe5c74b2d
|
127
|
return -1.0;
|
sowmy87 |
0:637fe5c74b2d
|
128
|
}
|
sowmy87 |
0:637fe5c74b2d
|
129
|
}
|
sowmy87 |
0:637fe5c74b2d
|
130
|
|
sowmy87 |
0:637fe5c74b2d
|
131
|
|
sowmy87 |
0:637fe5c74b2d
|
132
|
void Robot::TickTick() {
|
sowmy87 |
0:637fe5c74b2d
|
133
|
actualHeading = (*compass).readBearing();
|
sowmy87 |
0:637fe5c74b2d
|
134
|
prevDeltaHeading=deltaHeading;
|
sowmy87 |
0:637fe5c74b2d
|
135
|
deltaHeading=(desiredHeading-actualHeading);
|
sowmy87 |
0:637fe5c74b2d
|
136
|
if (deltaHeading<-1800)deltaHeading+=3600;
|
sowmy87 |
0:637fe5c74b2d
|
137
|
pid->setProcessValue(deltaHeading);
|
sowmy87 |
0:637fe5c74b2d
|
138
|
float delta=pid->compute();//*((float)desiredSpeed/(MAX_ROBOT_SPEED));
|
sowmy87 |
0:637fe5c74b2d
|
139
|
// pcr.printf("Desired Heading = %i,\t Actual Heading = %i,\t Delta Heading = %i\n\r",desiredHeading/10, actualHeading/10, deltaHeading/10);
|
sowmy87 |
0:637fe5c74b2d
|
140
|
// pcr.printf("SPD= %i, Delta = %i, \tadjustment = %i\n\r",desiredSpeed, deltaHeading/10,(int)delta);
|
sowmy87 |
0:637fe5c74b2d
|
141
|
|
sowmy87 |
0:637fe5c74b2d
|
142
|
SetSpeed(desiredSpeed-(int)delta, desiredSpeed+(int)delta);
|
sowmy87 |
0:637fe5c74b2d
|
143
|
|
sowmy87 |
0:637fe5c74b2d
|
144
|
}
|
sowmy87 |
0:637fe5c74b2d
|
145
|
|
sowmy87 |
0:637fe5c74b2d
|
146
|
void Robot::TackTack() {
|
sowmy87 |
0:637fe5c74b2d
|
147
|
range[servoPosition*5+readingNumber] = sensor.read();
|
sowmy87 |
0:637fe5c74b2d
|
148
|
// pcr.printf("range= %f\n\r",range[servoPosition*5+readingNumber]);
|
sowmy87 |
0:637fe5c74b2d
|
149
|
range0=range[0+0]+range[0+1]+range[0+2]+range[0+3]+range[0+4];
|
sowmy87 |
0:637fe5c74b2d
|
150
|
range1=range[5+0]+range[5+1]+range[5+2]+range[5+3]+range[5+4];
|
sowmy87 |
0:637fe5c74b2d
|
151
|
range2=range[10+0]+range[10+1]+range[10+2]+range[10+3]+range[10+4];
|
sowmy87 |
0:637fe5c74b2d
|
152
|
range3=range[15+0]+range[15+1]+range[15+2]+range[15+3]+range[15+4];
|
sowmy87 |
0:637fe5c74b2d
|
153
|
range4=range[20+0]+range[20+1]+range[20+2]+range[20+3]+range[20+4];
|
sowmy87 |
0:637fe5c74b2d
|
154
|
range5=range[25+0]+range[25+1]+range[25+2]+range[25+3]+range[25+4];
|
sowmy87 |
0:637fe5c74b2d
|
155
|
range6=range[30+0]+range[30+1]+range[30+2]+range[30+3]+range[30+4];
|
sowmy87 |
0:637fe5c74b2d
|
156
|
range7=range[35+0]+range[35+1]+range[35+2]+range[35+3]+range[35+4];
|
sowmy87 |
0:637fe5c74b2d
|
157
|
range8=range[40+0]+range[40+1]+range[40+2]+range[40+3]+range[40+4];
|
sowmy87 |
0:637fe5c74b2d
|
158
|
range9=range[45+0]+range[45+1]+range[45+2]+range[45+3]+range[45+4];
|
sowmy87 |
0:637fe5c74b2d
|
159
|
range10=range[50+0]+range[50+1]+range[50+2]+range[50+3]+range[50+4];
|
sowmy87 |
0:637fe5c74b2d
|
160
|
|
sowmy87 |
0:637fe5c74b2d
|
161
|
wait(SERVO_RATE/2);
|
sowmy87 |
0:637fe5c74b2d
|
162
|
|
sowmy87 |
0:637fe5c74b2d
|
163
|
|
sowmy87 |
0:637fe5c74b2d
|
164
|
switch (isFullScan) {
|
sowmy87 |
0:637fe5c74b2d
|
165
|
case 0:
|
sowmy87 |
0:637fe5c74b2d
|
166
|
if (servoDirection==1&&servoPosition>=10) {
|
sowmy87 |
0:637fe5c74b2d
|
167
|
readingNumber++;
|
sowmy87 |
0:637fe5c74b2d
|
168
|
servoDirection=-1;
|
sowmy87 |
0:637fe5c74b2d
|
169
|
}
|
sowmy87 |
0:637fe5c74b2d
|
170
|
if (servoDirection==-1&&servoPosition<=0) {
|
sowmy87 |
0:637fe5c74b2d
|
171
|
readingNumber++;
|
sowmy87 |
0:637fe5c74b2d
|
172
|
servoDirection=1;
|
sowmy87 |
0:637fe5c74b2d
|
173
|
}
|
sowmy87 |
0:637fe5c74b2d
|
174
|
break;
|
sowmy87 |
0:637fe5c74b2d
|
175
|
case 1:
|
sowmy87 |
0:637fe5c74b2d
|
176
|
default:
|
sowmy87 |
0:637fe5c74b2d
|
177
|
if (servoDirection==1&&servoPosition>=8) {
|
sowmy87 |
0:637fe5c74b2d
|
178
|
readingNumber++;
|
sowmy87 |
0:637fe5c74b2d
|
179
|
servoDirection=-1;
|
sowmy87 |
0:637fe5c74b2d
|
180
|
}
|
sowmy87 |
0:637fe5c74b2d
|
181
|
if (servoDirection==-1&&servoPosition<=3) {
|
sowmy87 |
0:637fe5c74b2d
|
182
|
readingNumber++;
|
sowmy87 |
0:637fe5c74b2d
|
183
|
servoDirection=1;
|
sowmy87 |
0:637fe5c74b2d
|
184
|
}
|
sowmy87 |
0:637fe5c74b2d
|
185
|
}
|
sowmy87 |
0:637fe5c74b2d
|
186
|
servoPosition+=servoDirection;
|
sowmy87 |
0:637fe5c74b2d
|
187
|
// pcr.printf("range= %f\n\r",servoPosition);
|
sowmy87 |
0:637fe5c74b2d
|
188
|
*servo=(float)servoPosition/10;
|
sowmy87 |
0:637fe5c74b2d
|
189
|
readingNumber%=5;
|
sowmy87 |
0:637fe5c74b2d
|
190
|
|
sowmy87 |
0:637fe5c74b2d
|
191
|
}
|
sowmy87 |
0:637fe5c74b2d
|
192
|
|
sowmy87 |
0:637fe5c74b2d
|
193
|
void Robot::FlashLEDs() {
|
sowmy87 |
0:637fe5c74b2d
|
194
|
if (abs(deltaHeading)<100) *led2=1;
|
sowmy87 |
0:637fe5c74b2d
|
195
|
else *led2=0;
|
sowmy87 |
0:637fe5c74b2d
|
196
|
|
sowmy87 |
0:637fe5c74b2d
|
197
|
if (deltaHeading>50) *led1=1;
|
sowmy87 |
0:637fe5c74b2d
|
198
|
else *led1=0;
|
sowmy87 |
0:637fe5c74b2d
|
199
|
|
sowmy87 |
0:637fe5c74b2d
|
200
|
if (deltaHeading<-50) *led3=1;
|
sowmy87 |
0:637fe5c74b2d
|
201
|
else *led3=0;
|
sowmy87 |
0:637fe5c74b2d
|
202
|
}
|
sowmy87 |
0:637fe5c74b2d
|
203
|
|
sowmy87 |
0:637fe5c74b2d
|
204
|
|
sowmy87 |
0:637fe5c74b2d
|
205
|
#endif |