Stronghold/Position.h at master · Techbrick/Stronghold · 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

/*
 * Position.h
 *
 *  Created on: Jan 26, 2016
 *      Author: Noah Zbozny
 */

#ifndef SRC_POSITION_H_
#define SRC_POSITION_H_

#include "WPILib.h"
#include "AHRS.h"
#include "Constants.h"

#include <math.h>

class Position {
	AHRS mxp; //navx mxp
	float angle;
	float pitch;
	float encoderTicks;
	float encoderTicksOffset;
	float xPos;
	float yPos;
	float wheelRadius = Constants::wheelRadius;
	float obstacleXPos [5] = {0, 0, 0, 0, 0};
	float obstacleYPos [5] = {0, 0, 0, 0, 0};

public:
	Position();
	void TrackX(bool movingForward, int encoderTickValue);
	void TrackY(bool movingForward, int encoderTickValue);
	void Update(bool movingForward, int encoderTickValue);
	void Calibrate();
	int NearestObstacle();
	float GetX();
	float GetY();
	float AngleToTower();
	float DistanceToTower();
	float GetAngle();
	float GetAngleDegrees();
	float GetPitch();
	bool IsTurning();
	void ZeroYaw();
};


#endif /* SRC_POSITION_H_ */