Merge pull request #13 from PenguinEmpire/dev

Dev to master
PenguinEmpire · Feb 20, 2018 · b949b01 · b949b01
2 parents 946f116 + 0fd9a62
commit b949b01
Show file tree

Hide file tree

Showing 2 changed files with 789 additions and 76 deletions.
diff --git a/FRC2018/src/PenguinEmpire.h b/FRC2018/src/PenguinEmpire.h
@@ -13,6 +13,31 @@
 #include "MyJoystick.h"
 #include "AHRS.h"
 
+class Lidar {
+public:
+	Lidar();
+	unsigned int AquireDistance(/*Timer**/);
+private:
+	enum Address {ADDRESS_DEFAULT=0x62}; // default I2C bus address for the LIDAR Lite v2
+	enum Register {COMMAND=0x00, STATUS=0x01, DISTANCE_1_2=0x8f};
+	enum Command {ACQUIRE_DC_CORRECT=0x04};
+	enum NumberOfRegistersToRead {READ_1_REGISTER=0x01, READ_2_REGISTERS=0x02};
+	enum NumberOfRegistersToWrite {WRITE_1_REGISTER=0x01};
+	I2C* I2CBus;
+
+	bool Busy()
+	{
+		unsigned char Status[Lidar::READ_1_REGISTER];
+		unsigned char statusRegister[Lidar::WRITE_1_REGISTER];
+		statusRegister[Lidar::WRITE_1_REGISTER-1] = Lidar::STATUS;
+
+		/**********read status**********/
+		if ( I2CBus->WriteBulk(statusRegister, Lidar::WRITE_1_REGISTER)) {printf ( "WriteBulk status failed! line %d\n", __LINE__ ); return true;}
+		if ( I2CBus->ReadOnly(Lidar::READ_1_REGISTER, Status) ) {printf ( "ReadOnly status failed! line %d\n", __LINE__ ); return true;}
+		//printf("Status at line %d %0x, bit0=%0x\n", __LINE__, Status[0], Status[0] & (unsigned char)0x01);
+		return (Status[0] & (unsigned char)0x01); // bit 0 is LIDAR Lite v2 busy bit
+	};
+};
 
 class Robot : public IterativeRobot {
 public:
@@ -26,8 +51,13 @@ class Robot : public IterativeRobot {
 	Compressor compressor;
 	DoubleSolenoid leftGearbox, rightGearbox, liftGearbox; // Gearbox shifters
 	DoubleSolenoid omniDropper;
-	Timer* timer;
-	DigitalInput* hallSensor;
+	Timer* mainTimer;
+	Timer* lidarTimer;
+	Lidar* lidar;
+	DigitalInput* bottomSensor;
+	DigitalInput* switchSensor;
+	DigitalInput* topSensor;
+	Encoder leftEnc, rightEnc;
 
 // Values and Structures
 	bool leftSwitch; // Is our color on the left side of the switch?
@@ -39,17 +69,13 @@ class Robot : public IterativeRobot {
 	float current;
 	int latestYaw;
 	bool turnSetup;
+	bool comboLift;
+	bool comboDrive;
 
 	bool gl90, gr90, gl180, gr180;
 
 	int testStep;
 
-	enum FieldPosition { // Used for autonomous
-		leftPos,
-		centerPos,
-		rightPos
-	} fpos;
-
 	enum StepType { // Used for autonomous
 		reset,
 		encoderMove,
@@ -79,8 +105,20 @@ class Robot : public IterativeRobot {
 //	};
 
 	std::vector<std::vector<double>> autosteps;
+	std::vector<std::vector<double>> ll, lr, cl, cr, rl, rr;
 	int numsteps, curstep;
 	bool stepSetup, stepComplete;
+	int fpos;
+	std::string mode;
+
+	int lastLiftState;
+	bool haltLifter;
+	bool goingPastSwitch;
+
+	std::shared_ptr<NetworkTable> contour;
+	std::vector<double> centerX, centerY, area, width;
+
+	int dist;
 
 // Stages
 
@@ -93,18 +131,18 @@ class Robot : public IterativeRobot {
 	void AutonomousInit();
 	void AutonomousPeriodic();
 	void CheckSide();
-	void CheckPos();
-	void LeftAuto();
-	void CenterAuto();
-	void RightAuto();
+	int CheckPos();
 	void RunSteps();
+	void ResetAll();
+	void StopMotors();
+	void AutoRunLifter(bool up, bool down);
 
 	// Teleoperated
 	void TeleopInit();
 	void TeleopPeriodic();
 	void SetLeftSpeed(double speed);
 	void SetRightSpeed(double speed);
-	void StopMotors();
+	void StopDriveMotors();
 	void TankDrive();
 	void Gyro90L(bool btn);
 	void Gyro90R(bool btn);