1 /*
2 * RobotPlayer.cpp
3 * toycars
4 *
5 * Created by Ruben Henner Zilibowitz on 5/08/06.
6 * Copyright 2006 Ruben Henner Zilibowitz. All rights reserved.
7 *
8 */
9
10 #include "RobotPlayer.h"
11 #include <cstdio>
12
RobotPlayer(ToyCar & inCar,list<Tuple> * path,int inPlayerNumber,FMOD::System * inFMOD_System)13 RobotPlayer::RobotPlayer(ToyCar& inCar, list<Tuple> *path, int inPlayerNumber, FMOD::System* inFMOD_System)
14 : Player(inCar, path, inPlayerNumber, inFMOD_System), backupDist(0)
15 {
16 // set debug data
17 unsticking = false;
18 playerType = kRobotPlayerType;
19 }
20
integrateKeys(double dt)21 void RobotPlayer::integrateKeys(double dt)
22 {
23 followPath(true, dt);
24 }
25
driveTowards(Tuple aim,double throttle)26 void RobotPlayer::driveTowards(Tuple aim, double throttle)
27 {
28 double angle = atan2(aim.y - car.getLocation_wc().y, aim.x - car.getLocation_wc().x);
29
30 angle -= car.getOrientation_wc();
31 angle = fmod(angle, kTwoPi);
32 if (angle > kPi)
33 angle -= kTwoPi;
34 else if (angle < -kPi)
35 angle += kTwoPi;
36 Tuple norm = aim - car.getLocation_wc();
37 if (fabs(angle) > kPiOn2) {
38 car.setThrottle(-throttle);
39 car.setSteerWheelAngle(car.getOrientation_wc() + kPi - atan2(norm.y,norm.x));
40 }
41 else {
42 car.setThrottle(throttle);
43 car.setSteerWheelAngle(atan2(norm.y,norm.x) - car.getOrientation_wc());
44 }
45 }
46
47 // Try to get unstuck by driving towards the path
backup(bool forwards,double dt)48 void RobotPlayer::backup(bool forwards, double dt)
49 {
50 // Try to get unstuck by driving towards the path
51 const Tuple aim = 0.5*pathClosestPoint + 0.5*(forwards ? *pathB : *pathA);
52 if ((car.getLocation_wc() - aim).Norm() < sqr(1))
53 backupDist = -1;
54 car.setBrake(0.0);
55 driveTowards(aim, 0.75);
56 backupDist -= car.getVelocity_wc().Length() * dt;
57 }
58
59 const double kMaxUnstickingBackupDist = 7.0;
60
followPath(bool forwards,double dt)61 void RobotPlayer::followPath(bool forwards, double dt)
62 {
63 if (playerIsStuck()) {
64 backupDist = kMaxUnstickingBackupDist;
65 }
66 if (backupDist > 0) {
67 backup(true, dt);
68
69 // set debug data
70 unsticking = true;
71 }
72 else if (car.getVelocity().x < -0.5) {
73 // stop the reversing
74 car.setThrottle(0.0);
75 car.setBrake(1.0);
76 }
77 else {
78 // set debug data
79 unsticking = false;
80
81 car.setBrake(0.0);
82
83 // Calculate steering angle
84
85 list<Tuple>::const_iterator pathC;
86 if (forwards) {
87 pathC = pathB;
88 pathC++;
89 if (pathC == pathEnd)
90 pathC = pathBegin;
91 }
92 else {
93 pathC = pathA;
94 if (pathC == pathBegin)
95 (pathC = pathEnd)--;
96 pathC--;
97 }
98
99 // Steer parallel to path
100 Tuple line = *pathC - (forwards ? *pathB : *pathA);
101 car.setSteerWheelAngle(atan2(line.y,line.x) - car.getOrientation_wc());
102 Tuple norm = pathClosestPoint - car.getLocation_wc();
103
104 // Correct to steer towards centre of path
105
106 if ((line^norm) < 0)
107 car.setSteerWheelAngle(car.getSteerWheelAngle() - norm.Length() * 0.1);
108 else
109 car.setSteerWheelAngle(car.getSteerWheelAngle() + norm.Length() * 0.1);
110
111 //car.mSteerWheelAngle += (line^norm) / line.Length() / kPi * 0.1;
112
113 list<Tuple>::const_iterator pathD = pathC;
114 if (forwards) {
115 pathD++;
116 if (pathD == pathEnd)
117 pathD = pathBegin;
118 }
119 else {
120 if (pathD == pathBegin)
121 (pathD = pathEnd)--;
122 else
123 pathD--;
124 }
125
126 // Look ahead and steer around corner slightly in advance
127 double pathAngle = line.Angle(*pathD - *pathC);
128 double segmentEndDistance = (car.getLocation_wc() - *pathC).Length();
129 car.setSteerWheelAngle(car.getSteerWheelAngle() + pathAngle / (segmentEndDistance + 0.5));
130
131 if (fabs(pathAngle) > kPi)
132 printf("yeah\n");
133
134 // Calculate throttle
135
136 double temp = 2*fabs(pathAngle) * car.getVelocity().x/(segmentEndDistance + 1.0);
137 if (temp > 1.0)
138 car.setThrottle(1.0 / temp);
139 else
140 car.setThrottle(1.0);
141 }
142
143 double slip = car.getWheel_BL().getLongtitudinalSlipVelocity() + car.getWheel_BR().getLongtitudinalSlipVelocity();
144 if (fabs(slip) > 5) {
145 car.setThrottle(5 / (-slip));
146 }
147 }
148
printDebugInfo() const149 void RobotPlayer::printDebugInfo() const
150 {
151 Player::printDebugInfo();
152 printf("unsticking: %d\n", unsticking);
153 }
154