Skip to content

Commit 802ce4a

Browse files
wfviningwfvining
committed
Merge remote-tracking branch 'origin/master' into hotfix-reset-manual-waypoints
2 parents fef7447 + 91d1cd6 commit 802ce4a

File tree

16 files changed

+273
-118
lines changed

16 files changed

+273
-118
lines changed

‎misc/deploy.sh‎

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -50,7 +50,7 @@ PullGit_Pack()
5050
{
5151
cd -;
5252
echo 'Packing up the repository... ';
53-
tar czf $dirName.tgz $dirPath;
53+
tar czhf $dirName.tgz $dirPath;
5454
exit 1;
5555
}
5656
read stuff;
@@ -75,7 +75,7 @@ Pack()
7575
{
7676
cd ~;
7777
echo 'Packing up the repository... ';
78-
tar czf $dirName.tgz $dirName;
78+
tar czhf $dirName.tgz $dirName;
7979
exit 0;
8080
}
8181
read stuff
@@ -297,9 +297,11 @@ if [ "$2" == "-S" ]; then
297297
sleep 10
298298
else
299299
#Transfer/Unpack/Run
300+
cd ~
300301
Transfer
301302
Unpack_Run
302303
sleep 10
304+
cd -
303305
fi
304306

305307
#if not on the network

‎src/behaviours/src/DriveController.cpp‎

Lines changed: 59 additions & 74 deletions
Original file line numberDiff line numberDiff line change
@@ -103,6 +103,11 @@ Result DriveController::DoWork()
103103
else
104104
{
105105
stateMachineState = STATE_MACHINE_ROTATE;
106+
waypoints.back().theta = atan2(waypoints.back().y - currentLocation.y, waypoints.back().x - currentLocation.x);
107+
result.pd.setPointYaw = waypoints.back().theta;
108+
109+
//cout << "**************************************************************************" << endl;
110+
//cout << "Waypoint x : " << waypoints.back().x << " y : " << waypoints.back().y << endl;
106111
//fall through on purpose
107112
}
108113
}
@@ -119,11 +124,10 @@ Result DriveController::DoWork()
119124
// Calculate the diffrence between current and desired heading in radians.
120125
float errorYaw = angles::shortest_angular_distance(currentLocation.theta, waypoints.back().theta);
121126

122-
cout << "ROTATING, ERROR: " << errorYaw << endl;
127+
//cout << "ROTATE Error yaw: " << errorYaw << " target heading : " << waypoints.back().theta << " current heading : " << currentLocation.theta << endl;
128+
//cout << "Waypoint x : " << waypoints.back().x << " y : " << waypoints.back().y << " currentLoc x : " << currentLocation.x << " y : " << currentLocation.y << endl;
123129

124130
result.pd.setPointVel = 0.0;
125-
result.pd.setPointYaw = waypoints.back().theta;
126-
127131
//Calculate absolute value of angle
128132

129133
float abs_error = fabs(angles::shortest_angular_distance(currentLocation.theta, waypoints.back().theta));
@@ -135,39 +139,12 @@ Result DriveController::DoWork()
135139
if (result.PIDMode == FAST_PID)
136140
{
137141
fastPID(0.0, errorYaw, result.pd.setPointVel, result.pd.setPointYaw);
138-
139-
const int MIN_TURN_VALUE = 80;
140-
141-
//If wheels get a value less than 80, will cause robot to sit in place
142-
if(fabs(left) < MIN_TURN_VALUE && fabs(right) < MIN_TURN_VALUE)
143-
{
144-
//increase left and right values to minimum value, checking signs for negative or positive value
145-
if(this->left < 0)
146-
{
147-
this->left = MIN_TURN_VALUE * -1;
148-
}
149-
else
150-
{
151-
this->left = MIN_TURN_VALUE;
152-
}
153-
154-
if(this->right < 0)
155-
{
156-
this->right = MIN_TURN_VALUE * -1;
157-
}
158-
else
159-
{
160-
this->right = MIN_TURN_VALUE;
161-
}
162-
}
163142
}
164143

165-
166144
break;
167145
}
168146
else
169147
{
170-
171148
//move to differential drive step
172149
stateMachineState = STATE_MACHINE_SKID_STEER;
173150

@@ -182,30 +159,24 @@ Result DriveController::DoWork()
182159
// Stay in this state until angle is at least PI/2
183160

184161
// calculate the distance between current and desired heading in radians
162+
waypoints.back().theta = atan2(waypoints.back().y - currentLocation.y, waypoints.back().x - currentLocation.x);
185163
float errorYaw = angles::shortest_angular_distance(currentLocation.theta, waypoints.back().theta);
164+
float distance = hypot(waypoints.back().x - currentLocation.x, waypoints.back().y - currentLocation.y);
165+
166+
//cout << "Skid steer, Error yaw: " << errorYaw << " target heading : " << waypoints.back().theta << " current heading : " << currentLocation.theta << " error distance : " << distance << endl;
167+
//cout << "Waypoint x : " << waypoints.back().x << " y : " << waypoints.back().y << " currentLoc x : " << currentLocation.x << " y : " << currentLocation.y << endl;
168+
186169

187-
result.pd.setPointYaw = waypoints.back().theta;
188170

189171
// goal not yet reached drive while maintaining proper heading.
190-
if (fabs(angles::shortest_angular_distance(currentLocation.theta, atan2(waypoints.back().y - currentLocation.y, waypoints.back().x - currentLocation.x))) < M_PI_2
191-
&& hypot(waypoints.back().x - currentLocation.x, waypoints.back().y - currentLocation.y) > waypointTolerance)
172+
if (fabs(errorYaw) < M_PI_2 && distance > waypointTolerance)
192173
{
193174
// drive and turn simultaniously
194175
result.pd.setPointVel = searchVelocity;
195176
if (result.PIDMode == FAST_PID)
196177
{
197-
fastPID(searchVelocity - linearVelocity,errorYaw, result.pd.setPointVel, result.pd.setPointYaw);
198-
}
199-
}
200-
// goal is reached but desired heading is still wrong turn only
201-
else if (fabs(angles::shortest_angular_distance(currentLocation.theta, waypoints.back().theta)) > finalRotationTolerance
202-
&& hypot(waypoints.back().x - currentLocation.x, waypoints.back().y - currentLocation.y) > waypointTolerance)
203-
{
204-
// rotate but dont drive
205-
result.pd.setPointVel = 0.0;
206-
if (result.PIDMode == FAST_PID)
207-
{
208-
fastPID(0.0,errorYaw, result.pd.setPointVel, result.pd.setPointYaw);
178+
//cout << "linear velocity: " << linearVelocity << endl;
179+
fastPID((searchVelocity-linearVelocity) ,errorYaw, result.pd.setPointVel, result.pd.setPointYaw);
209180
}
210181
}
211182
else {
@@ -239,7 +210,6 @@ Result DriveController::DoWork()
239210

240211
bool DriveController::ShouldInterrupt()
241212
{
242-
243213
if (interupt)
244214
{
245215
interupt = false;
@@ -273,20 +243,26 @@ void DriveController::ProcessData()
273243
else if (result.type == precisionDriving)
274244
{
275245

276-
if (result.PIDMode == FAST_PID){
246+
if (result.PIDMode == FAST_PID)
247+
{
277248
float vel = result.pd.cmdVel -linearVelocity;
278249
float setVel = result.pd.cmdVel;
279250
fastPID(vel,result.pd.cmdAngularError, setVel, result.pd.setPointYaw);
280251
}
281-
else if (result.PIDMode == SLOW_PID) {
252+
else if (result.PIDMode == SLOW_PID)
253+
{
282254
//will take longer to reach the setPoint but has less chanse of an overshoot
283255
float vel = result.pd.cmdVel -linearVelocity;
284256
float setVel = result.pd.cmdVel;
285257
slowPID(vel,result.pd.cmdAngularError, setVel, result.pd.setPointYaw);
286258
}
287-
else if (result.PIDMode == CONST_PID) {
259+
else if (result.PIDMode == CONST_PID)
260+
{
288261
float vel = result.pd.cmdVel - linearVelocity;
289262
float angular = result.pd.cmdAngular - angularVelocity;
263+
264+
//cout << "Ang. Vel. " << angularVelocity << " Ang. Error" << angular << endl;
265+
290266
constPID(vel, angular ,result.pd.setPointVel, result.pd.setPointYaw);
291267
}
292268
}
@@ -296,13 +272,15 @@ void DriveController::ProcessData()
296272
void DriveController::fastPID(float errorVel, float errorYaw , float setPointVel, float setPointYaw)
297273
{
298274

275+
// cout << "PID FAST" << endl;
276+
299277
float velOut = fastVelPID.PIDOut(errorVel, setPointVel);
300278
float yawOut = fastYawPID.PIDOut(errorYaw, setPointYaw);
301279

302280
int left = velOut - yawOut;
303281
int right = velOut + yawOut;
304282

305-
int sat = 255;
283+
int sat = 180;
306284
if (left > sat) {left = sat;}
307285
if (left < -sat) {left = -sat;}
308286
if (right > sat) {right = sat;}
@@ -312,15 +290,17 @@ void DriveController::fastPID(float errorVel, float errorYaw , float setPointVel
312290
this->right = right;
313291
}
314292

315-
void DriveController::slowPID(float errorVel,float errorYaw, float setPointVel, float setPointYaw) {
293+
void DriveController::slowPID(float errorVel,float errorYaw, float setPointVel, float setPointYaw)
294+
{
295+
//cout << "PID SLOW" << endl;
316296

317297
float velOut = slowVelPID.PIDOut(errorVel, setPointVel);
318298
float yawOut = slowYawPID.PIDOut(errorYaw, setPointYaw);
319299

320300
int left = velOut - yawOut;
321301
int right = velOut + yawOut;
322302

323-
int sat = 255;
303+
int sat = 180;
324304
if (left > sat) {left = sat;}
325305
if (left < -sat) {left = -sat;}
326306
if (right > sat) {right = sat;}
@@ -330,15 +310,18 @@ void DriveController::slowPID(float errorVel,float errorYaw, float setPointVel,
330310
this->right = right;
331311
}
332312

333-
void DriveController::constPID(float erroVel,float constAngularError, float setPointVel, float setPointYaw) {
313+
void DriveController::constPID(float erroVel,float constAngularError, float setPointVel, float setPointYaw)
314+
{
315+
316+
//cout << "PID CONST" << endl;
334317

335-
float velOut = fastVelPID.PIDOut(erroVel, setPointVel);
336-
float yawOut = fastYawPID.PIDOut(constAngularError, setPointYaw);
318+
float velOut = constVelPID.PIDOut(erroVel, setPointVel);
319+
float yawOut = constYawPID.PIDOut(constAngularError, setPointYaw);
337320

338321
int left = velOut - yawOut;
339322
int right = velOut + yawOut;
340323

341-
int sat = 255;
324+
int sat = 180;
342325
if (left > sat) {left = sat;}
343326
if (left < -sat) {left = -sat;}
344327
if (right > sat) {right = sat;}
@@ -349,27 +332,29 @@ void DriveController::constPID(float erroVel,float constAngularError, float setP
349332
}
350333

351334

352-
void DriveController::SetVelocityData(float linearVelocity,float angularVelocity) {
335+
void DriveController::SetVelocityData(float linearVelocity,float angularVelocity)
336+
{
353337
this->linearVelocity = linearVelocity;
354338
this->angularVelocity = angularVelocity;
355339
}
356340

357341

358342

359343

360-
PIDConfig DriveController::fastVelConfig() {
344+
PIDConfig DriveController::fastVelConfig()
345+
{
361346
PIDConfig config;
362347

363-
config.Kp = 140;
348+
config.Kp = 60;
364349
config.Ki = 10;
365-
config.Kd = 0.8;
350+
config.Kd = 2;
366351
config.satUpper = 255;
367352
config.satLower = -255;
368353
config.antiWindup = config.satUpper;
369354
config.errorHistLength = 4;
370355
config.alwaysIntegral = true;
371356
config.resetOnSetpoint = true;
372-
config.feedForwardMultiplier = 320; //gives 127 pwm at 0.4 commandedspeed
357+
config.feedForwardMultiplier = 610; //gives 127 pwm at 0.4 commandedspeed ORIG:320
373358
config.integralDeadZone = 0.01;
374359
config.integralErrorHistoryLength = 10000;
375360
config.integralMax = config.satUpper/2;
@@ -382,19 +367,19 @@ PIDConfig DriveController::fastVelConfig() {
382367
PIDConfig DriveController::fastYawConfig() {
383368
PIDConfig config;
384369

385-
config.Kp = 100;
386-
config.Ki = 10;
387-
config.Kd = 14;
370+
config.Kp = 60;
371+
config.Ki = 15;
372+
config.Kd = 5;
388373
config.satUpper = 255;
389374
config.satLower = -255;
390-
config.antiWindup = config.satUpper/2;
375+
config.antiWindup = config.satUpper/6;
391376
config.errorHistLength = 4;
392377
config.alwaysIntegral = false;
393378
config.resetOnSetpoint = true;
394379
config.feedForwardMultiplier = 0;
395380
config.integralDeadZone = 0.01;
396381
config.integralErrorHistoryLength = 10000;
397-
config.integralMax = config.satUpper/2;
382+
config.integralMax = config.satUpper/3;
398383
config.derivativeAlpha = 0.7;
399384

400385
return config;
@@ -448,16 +433,16 @@ PIDConfig DriveController::slowYawConfig() {
448433
PIDConfig DriveController::constVelConfig() {
449434
PIDConfig config;
450435

451-
config.Kp = 140;
436+
config.Kp = 60;
452437
config.Ki = 10;
453-
config.Kd = 0.8;
438+
config.Kd = 2;
454439
config.satUpper = 255;
455440
config.satLower = -255;
456-
config.antiWindup = config.satUpper/2;
441+
config.antiWindup = config.satUpper;
457442
config.errorHistLength = 4;
458443
config.alwaysIntegral = true;
459444
config.resetOnSetpoint = true;
460-
config.feedForwardMultiplier = 320; //gives 127 pwm at 0.4 commandedspeed
445+
config.feedForwardMultiplier = 610; //gives 127 pwm at 0.4 commandedspeed ORIG:320
461446
config.integralDeadZone = 0.01;
462447
config.integralErrorHistoryLength = 10000;
463448
config.integralMax = config.satUpper/2;
@@ -470,19 +455,19 @@ PIDConfig DriveController::constVelConfig() {
470455
PIDConfig DriveController::constYawConfig() {
471456
PIDConfig config;
472457

473-
config.Kp = 100;
458+
config.Kp = 5;
474459
config.Ki = 5;
475-
config.Kd = 1.2;
460+
config.Kd = 0;
476461
config.satUpper = 255;
477462
config.satLower = -255;
478463
config.antiWindup = config.satUpper/4;
479464
config.errorHistLength = 4;
480465
config.alwaysIntegral = true;
481466
config.resetOnSetpoint = true;
482-
config.feedForwardMultiplier = 120;
467+
config.feedForwardMultiplier = 0;
483468
config.integralDeadZone = 0.01;
484469
config.integralErrorHistoryLength = 10000;
485-
config.integralMax = config.satUpper/2;
470+
config.integralMax = config.satUpper;
486471
config.derivativeAlpha = 0.6;
487472

488473
return config;

‎src/behaviours/src/DriveController.h‎

Lines changed: 4 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -29,11 +29,12 @@ class DriveController : virtual Controller
2929

3030
bool interupt = false;
3131

32-
float rotateOnlyAngleTolerance = 0.15;
33-
float finalRotationTolerance = 0.2;
32+
float rotateOnlyAngleTolerance = 0.05; //May be too low?
33+
float finalRotationTolerance = 0.1;
3434
const float waypointTolerance = 0.15; //15 cm tolerance.
3535

36-
float searchVelocity = 0.5; // meters/second
36+
//0.65 MAX value
37+
float searchVelocity = 0.35; // meters/second
3738

3839
float linearVelocity = 0;
3940
float angularVelocity = 0;

‎src/behaviours/src/ObstacleController.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -121,7 +121,7 @@ void ObstacleController::ProcessData() {
121121
}
122122
}
123123
else {
124-
if (center < 0.12) {
124+
if (center < 3.0) {
125125
result.wristAngle = 0.7;
126126
}
127127
else {

0 commit comments

Comments
 (0)