#define ST3STEP 0.2
#define ST4COUNT 10
+#define ST4SPEED 0.2 // aka STEP
#define ST4KP 1
#define ST4KI 0
#define ST4KD 0
float co = cos(angl - init->h());
//float si = sin(angl - init->h());
- float speed = 1; // desired
+ float speed = ST4SPEED; // desired
float gx = goal->x();
float gy = goal->y();
if (co < 0)