BetaTestingExperiment/CameraServo.cpp at development · MMRambotics/BetaTestingExperiment · 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
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
/*
 * @author:
 * 	@lead: Andrew Horsman
 * 	@other: Mike Noseworthy
 * @description: Controls camera servos.
 */

// Includes
#include "Headers/StateMachine.h"
#include "Headers/WPILibrary.h"

// State Machine constants.
static const int CAMERA_MOVEMENT = 1;
static const int LOCKED   = 1;
static const int UNLOCKED = 2;

// Joystick Buttons
static const int JOYSTICK_TOP_UP    = 3;
static const int JOYSTICK_TOP_DOWN  = 2;
static const int JOYSTICK_TOP_LEFT  = 4;
static const int JOYSTICK_TOP_RIGHT = 5;

// Defaults
static const float DEFAULT_POSITION_INCREMENT = 0.01;

class CameraServo {

	Servo pan;
	Servo tilt;

	float CAMERA_PAN_CENTER;
	float CAMERA_TILT_CENTER;

	float PAN_LEFT_MULTIPLIER;
	float PAN_RIGHT_MULTIPLIER;
	float TILT_UP_MULTIPLIER;
	float TILT_DOWN_MULTIPLIER;

	public:

		/*
		 * Creates a new CameraServo instance and configures constants.
		 *
		 * @param int PWM port the pan servo is on.
		 * @param int PWM port the tilt servo is on.
		 * @param float Value between 0.0 and 1.0 where the pan servo is
		 * centered.
		 * @param float Value between 0.0 and 1.0 where the tilt servo is
		 * centered.
		 * @param float Specify which value is the left direction (-1.0 or 1.0).
		 * @param float Specify which value is the up direction (-1.0 or 1.0).
		 */
		CameraServo(int panPort, int tiltPort, float panCenter, float tiltCenter, float panLeftDirection, float tiltUpDirection):
			pan(panPort),
			tilt(tiltPort)
		{
			CAMERA_PAN_CENTER  = panCenter;
			CAMERA_TILT_CENTER = tiltCenter;

			// Configurable directions.
			PAN_LEFT_MULTIPLIER    = panLeftDirection;
			PAN_RIGHT_MULTIPLIER   = panLeftDirection * -1;
			TILT_UP_MULTIPLIER     = tiltUpDirection;
			TILT_DOWN_MULTIPLIER   = tiltUpDirection * -1;

		}

		void JoystickControl(Joystick &joystick) {
			if (joystick.GetRawButton(JOYSTICK_TOP_UP))
				TiltUp();
			else if (joystick.GetRawButton(JOYSTICK_TOP_DOWN))
				TiltDown();

			if (joystick.GetRawButton(JOYSTICK_TOP_LEFT))
				PanLeft();
			else if (joystick.GetRawButton(JOYSTICK_TOP_RIGHT))
				PanRight();
		}

		/*
		 * Sets the camera to it's center position.
		 */
		void SetToCenter() {
			pan.Set(CAMERA_PAN_CENTER);
			tilt.Set(CAMERA_TILT_CENTER);
		}

		/*
		 * Move the camera to the left.
		 *
		 * @param float Value between 0.0 and 1.0 of the step the pan motor will
		 * take to the left.  Default is 0.05.  It would take 20 steps to go
		 * from 0.0 to 1.0.
		 */
		bool PanLeft(float positionOffset = DEFAULT_POSITION_INCREMENT) {
			float currentPosition = pan.Get();
			if (currentPosition == 0.0)
				return false;

			pan.Set(currentPosition + (positionOffset * PAN_LEFT_MULTIPLIER));
			return true;
		}

		/*
		 * Move the camera to the right.
		 *
		 * @param float Value between 0.0 and 1.0 of the step the pan motor will
		 * take to the right.  Default is 0.05.  It would take 20 steps to go
		 * from 0.0 to 1.0.
		 */
		bool PanRight(float positionOffset = DEFAULT_POSITION_INCREMENT) {
			float currentPosition = pan.Get();
			if (currentPosition == 1.0)
				return false;

			pan.Set(currentPosition + (positionOffset * PAN_RIGHT_MULTIPLIER));
			return true;
		}

		/*
		 * Tilt the camera up.
		 *
		 * @param float Value between 0.0 to 1.0 of the step the tilt motor will
		 * tilt up by.  Default is 0.05.  It would take 20 steps to go from 0.0
		 * to 1.0.
		 */
		bool TiltUp(float positionOffset = DEFAULT_POSITION_INCREMENT) {
			float currentPosition = tilt.Get();
			if (currentPosition == 0.1)
				return false;

			tilt.Set(currentPosition + (positionOffset * TILT_UP_MULTIPLIER));
			return true;
		}

		/*
		 * Tilt the camera down.
		 *
		 * @param float Value between 0.0 to 1.0 of the step the tilt motor will
		 * tilt down by.  Default is 0.05.  It would take 20 steps to go from
		 * 0.0 to 1.0.
		 */
		bool TiltDown(float positionOffset = DEFAULT_POSITION_INCREMENT) {
			float currentPosition = tilt.Get();
			if (currentPosition == 0.9)
				return true;

			tilt.Set(currentPosition + (positionOffset * TILT_DOWN_MULTIPLIER));
			return true;
		}

};