Project-K9/PositionControl.h at master · MLenterman/Project-K9 · GitHub

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
#pragma once

#include "CoordSet.h"
#include "Direction.h"
#include "Logger.h"
#include "simpletools.h"
#include "abdrive.h"
#include <math.h>

#define TRAVEL_DISTANCE_PER_TICK 3.25F
#define DEFAULT_DRIVE_SPEED 32

class PositionControl{
public:
	PositionControl();
	~PositionControl();

	void moveForward();
	void moveBackward();
	void stop();
	void turn(Direction direction);

	void correct(float correctionLeft, float correctionRight);
	void correctAngle(int radials);

	bool getMoveForwardFlag();
	bool getMoveBackwardFlag();
	bool getStopFlag();

	CoordSet getCurrentPosition();
	void setCurrentPosition(CoordSet position);
	CoordSet getPrevPosition();
	void setPrevPosition(CoordSet position);
	CoordSet getTargetPosition();
	void setTargetPosition(CoordSet position);

	Direction getCurrentOrientation();
	void setCurrentOrientation(Direction orientation);
	Direction getTargetOrientation();
	void setTargetOrientation(Direction orientation);

	int getSpeed();
	void setSpeed(int speed);

	int getCurrentTicks();
	void setCurrentTicks(int ticks);
	int getPrevTicks();
	void setPrevTicks(int ticks);
	int getTargetTicks();
	void setTargetTicks(int ticks);

	bool isAtTargetPosition();
	int calcSpeedFromTicks();

	void update();

private:
	bool moveForwardFlag;
	bool moveBackwardFlag;
	bool stopFlag;

	bool actionInProgress;

	CoordSet currentPosition;
	CoordSet prevPosition;
	CoordSet targetPosition;

	Direction currentOrientation;
	Direction targetOrientation;

	int speed;

	int currentTicks;

	float	speedCorrectionLeft,
			speedCorrectionRight;

	int targetTicks;

	int startTicksLeft;
	int startTicksRight;

	int actionTicksLeft;
	int actionTicksRight;
protected:

};