BetaTestingExperiment/MyRobot.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
153
154
155
156
157
158
159
160
161
162
/*
 * @author:
 * 	@lead: Andrew Horsman
 * 	@other: Mike Noseworthy
 * @description: Random beta experiments to complete task five.
 */

// Preprocessor configurations
#define DEBUG

// Includes
#include "Headers/Accelerometer.h"
#include "Headers/Camera.h"
#include "Headers/CameraServo.h"
#include "Headers/Gyro.h"
#include "Headers/HelperFunctions.h"
#include "Headers/LightSwitch.h"
#include "Headers/Potentiometer.h"
#include "Headers/Print.h"
#include "Headers/StateMachine.h"
#include "Headers/WPILibrary.h"
#include "Pneumatics.h"

// Motor ports configuration.
static const int PAN_MOTOR  = 9;
static const int TILT_MOTOR = 10;

// Camera movement configuration.
static const float CAMERA_PAN_CENTER  = 0.5;
static const float CAMERA_TILT_CENTER = 0.3;
static const float PAN_LEFT_DIRECTION = -1.0;
static const float TILT_UP_DIRECTION  = 1.0;

// Sensor port configuration.
static const int GYRO_PORT    = 1;
static const int POT_PORT     = 7;
static const int SWITCH_PORT  = 14;
static const int COMPASS_PORT = 1;
static const int ACCELX_PORT  = 2;
static const int ACCELY_PORT  = 3;

// Joystick port configuration
static const int JOYSTICK_LEFT  = 1;
static const int JOYSTICK_RIGHT = 2;

// Potentiometer voltage configuration.
static const float UPPER_BOUND_VOLTAGE = 4.956;
static const float LOWER_BOUND_VOLTAGE = 0.0005;

// Solenoid port configuration.
static const int IN_PORT  = 1;
static const int OUT_PORT = 2;

// Pneumatic compressor port configuration.
static const int PRESSURE_SWITCH_PORT  = 1;
static const int COMPRESSOR_RELAY_PORT = 1;

// Main class.
class MyRobot : public IterativeRobot {

	CameraServo     moveCamera;
	EasyGyro        gyro;
	Print           print;
	EasyCamera      camera;
	Joystick        joystickLeft;
	Joystick        joystickRight;
	LightSwitch     testSwitch;
	Potentiometer   pot;
	Helper          helper;
	EasyAccel 	    accel;
	StateMachine    robotState;
	PneumaticPiston piston;
	PneumaticCompressor compressor;

	public:

		MyRobot():
			moveCamera(PAN_MOTOR, TILT_MOTOR, CAMERA_PAN_CENTER, CAMERA_TILT_CENTER, PAN_LEFT_DIRECTION, TILT_UP_DIRECTION),
			gyro(GYRO_PORT),
			print(),
			camera(),
			joystickLeft(JOYSTICK_LEFT),
			joystickRight(JOYSTICK_RIGHT),
			testSwitch(SWITCH_PORT),
			pot(POT_PORT, LOWER_BOUND_VOLTAGE, UPPER_BOUND_VOLTAGE),
			helper(),
			accel(ACCELX_PORT, ACCELY_PORT),
			robotState(),
			piston(IN_PORT, OUT_PORT),
			compressor(PRESSURE_SWITCH_PORT, COMPRESSOR_RELAY_PORT)
		{
			DisableWatchdog();
		}

		~MyRobot() {}

		void TeleopInit() {
			DisableWatchdog();
			moveCamera.SetToCenter();

			EnableWatchdog();
			FeedWatchdog();
		}

		void TeleopPeriodic() {
			while (IsOperatorControl()) {
				FeedWatchdog();

				camera.GetImage();
				moveCamera.JoystickControl(joystickLeft);

				compressor.CheckAndEngageCompressor();
				piston.EngageIfTrigger(joystickLeft);

				#ifdef DEBUG
				Debug();
				#endif
			}
		}

	private:

		void EnableWatchdog() {
			GetWatchdog().SetEnabled(true);
			GetWatchdog().SetExpiration(0.5);
		}

		void DisableWatchdog() {
			GetWatchdog().SetEnabled(false);
		}

		void FeedWatchdog() {
			GetWatchdog().Feed();
		}

		void Debug() {
			print.ClearDisplay();

			char gyroHeading[40];
			sprintf(gyroHeading, "Gyro Value: %f", gyro.GetGyroHeading());
			print.PrintText(gyroHeading);

			if (testSwitch.IsOn()) {
				print.PrintText("Switch is on!");
			} else {
				print.PrintText("Switch is off!");
			}

			char potentiometerVoltage[40];
			sprintf(potentiometerVoltage, "Analog Voltage: %f", pot.GetRawVoltage());
			print.PrintText(potentiometerVoltage);

			char potentiometerPosition[40];
			float currentPosition = helper.closeBounds(pot.CalculatePosition());
			sprintf(potentiometerPosition, "Position: %.2f%%", currentPosition);
			print.PrintText(potentiometerPosition);
		}

};

// Start with BetaRobot class, do not change.
START_ROBOT_CLASS(MyRobot);