Monday Night

Author: Tom Lazar

Debug/src/IControl.o

@@ -20,6 +20,7 @@ Drive::Drive(Motor *motor, IXbox *xbox, ISensorControl *nav) {
 	state = nav->running;
 	requestedState = nav->running;
 	userControl = false;
+	direction = true;
 
 	left = new Encoder(2, 3);
 	right = new Encoder(4, 5);
@@ -59,6 +60,7 @@ void Drive::TeleopInit() {
 	SmartDashboard::PutNumber("Accel", 0.1); //Acceleration curve Value 0.1 - 0.05 is good
 
 	userControl = true; //Start with user control
+	direction = true;
 	state = nav->running;
 	requestedState = nav->running;
 }
@@ -69,12 +71,18 @@ void Drive::TeleopPeriodic() {
 	navSpeed = nav->UpdateMotorSpeeds(leftSpeed, rightSpeed); //nav returns speed it wants (corrected or wehen nav is controlling)
 	updateMotors();
 
-	if (Global::telemetry <= 1) { //Normal
+	if (Global::telemetry >= 1) { //Normal
 		SmartDashboard::PutNumber("Drive Left", leftSpeed);
 		SmartDashboard::PutNumber("Drive Right", rightSpeed);
-	} else if (Global::telemetry <= 2) { //debug
+		if (direction)
+			SmartDashboard::PutString("Direction", "Forward");
+		else
+			SmartDashboard::PutString("Direction", "Reverse");
+	} else if (Global::telemetry >= 2) { //debug
 
-	} else if (Global::telemetry <= 3) { //advanced debug
+	} else if (Global::telemetry >= 3) { //advanced debug
+		SmartDashboard::PutNumber("X Value", xbox->getAxisLeftX());
+		SmartDashboard::PutNumber("Y Value", xbox->getAxisLeftY());
 		SmartDashboard::PutNumber("Left Raw", left->Get());
 		SmartDashboard::PutNumber("Right RAW", right->Get());
 		SmartDashboard::PutNumber("Left2 Raw", left2->Get());
@@ -109,6 +117,10 @@ void Drive::readXboxArcadeT() { // Doesn't work with NAV right now
 void Drive::readXboxArcadeD() {
 	float x, y;
 
+	if (xbox->getYPressed()) {
+		direction = !direction;
+	}
+
 	if (requestedState == nav->running) { //When the state is running xbox controls other wise start to stop
 		x = xbox->getAxisLeftX();
 		y = xbox->getAxisLeftY();
@@ -117,8 +129,6 @@ void Drive::readXboxArcadeD() {
 		x = 0.0;
 		y = 0.0;
 	}
-	//SmartDashboard::PutNumber("X Value", x);
-	//SmartDashboard::PutNumber("Y Value", y);
 
 	//x*x + y*y is >= 1
 	if (y > 0)
@@ -143,13 +153,17 @@ void Drive::readXboxArcadeD() {
 	} else
 		state = nav->running;
 
-	if (state == nav->running || state == nav->stopping) { //If running or stopping update speeds here
-		leftSpeed = acceleration(y + x, leftSpeed);
-		rightSpeed = acceleration(y - x, rightSpeed);
+	if (direction) {
+		if (state == nav->running || state == nav->stopping) { //If running or stopping update speeds here
+			leftSpeed = acceleration(y + x, leftSpeed);
+			rightSpeed = acceleration(y - x, rightSpeed);
+		}
+	} else {
+		if (state == nav->running || state == nav->stopping) { //If running or stopping update speeds here
+			leftSpeed = acceleration2(-(y + x), leftSpeed);
+			rightSpeed = acceleration2(-(y - x), rightSpeed);
+		}
 	}
-
-	//SmartDashboard::PutNumber("RightOut Value", rightSpeed);
-	//SmartDashboard::PutNumber("LeftOut Value", leftSpeed);
 }
 //Compare requested speed with last speed, if difference greater than accel value only change by accel value
 float Drive::acceleration(float newS, float oldS) {
@@ -165,6 +179,19 @@ float Drive::acceleration(float newS, float oldS) {
 	return newS;
 }
 
+float Drive::acceleration2(float newS, float oldS) {
+	float accel = SmartDashboard::GetNumber("Accel", 0.0);
+
+	if (fabs(newS - oldS) < accel) {
+		if (oldS < newS)
+			return oldS + accel;
+		else
+			return oldS - accel;
+	}
+
+	return newS;
+}
+
 void Drive::updateMotors() {
 	if (requestedState == nav->stopped && state == nav->stopped) { //Update speeds here if nav is controlling
 		leftSpeed = navSpeed->leftMotorSpeed;

src/Drive/Drive.cpp

@@ -36,6 +36,7 @@ class Drive: public IDrive, public IControl {
 
 	//Acceleration for the drives
 	float acceleration(float newS, float oldS);
+	float acceleration2(float newS, float oldS);
 
 	Motor *motor;
 	IXbox *xbox;
@@ -52,6 +53,7 @@ class Drive: public IDrive, public IControl {
 	float leftSpeed, rightSpeed;
 
 	bool userControl; //True if using xbox or decelerating, false if under nav control
+	bool direction;
 };
 
 #endif /* SRC_DRIVE_H_ */

src/Drive/Drive.h

@@ -14,8 +14,6 @@ class Robot: public IterativeRobot {
 	Arm *arm;
 	CommandListMaker *clMaker;
 	RelayController* rc;
-	CustomUSBCamera* camera;
-
 
 	bool DEBUG = false;
 
@@ -35,8 +33,6 @@ class Robot: public IterativeRobot {
 		master->addNode(xbox, "Xbox");
 
 		if (robot.compare("PROTO") == 0) {
-			camera = new CustomUSBCamera();
-
 			//master->addNode(camera, "camera");
 
 		} else if (robot.compare("TIM") == 0) {
@@ -129,7 +125,7 @@ class Robot: public IterativeRobot {
 
 
 		// Turn???
-		NavxSensorControl::AutonomousPeriodic(turn);
+		//NavxSensorControl::AutonomousPeriodic(turn);
 
 	}
 

src/Robot.cpp

@@ -80,8 +80,7 @@ void NavxSensorControl::TargetingStateMachine() {
 	float motorSpeed = 0;
 	switch (targetState) {
 	case TargetingState::waitForButtonPress:
-		if ((xbox->getLeftTriggerPressed() && !comp)
-				|| (xbox->getLeftBumperPressed() && comp)) {
+		if ( (xbox->getLeftBumperPressed() && comp)) {
 			commandDriveState = DriveSystemState::stopped;
 			targetState = TargetingState::waitForStopped;
 		}