 int hc3 = 31;
 
 //Hall effect sensor variables
-volatile int timeoldA = 0, timeoldB = 0, timeoldC = 0;
+volatile double curSpeedA = 0.0, curSpeedB = 0.0, curSpeedC = 0.0;
+volatile double oldSpeedA = 0.0, oldSpeedB = 0.0, oldSpeedC = 0.0;
+volatile int timeoldA = 0, timeoldB = 0, timeoldC = 0, time = 0;
 volatile int hallA1 = 0, hallA2 = 0, hallA3 = 0;
 volatile int hallB1 = 0, hallB2 = 0, hallB3 = 0;
 volatile int hallC1 = 0, hallC2 = 0, hallC3 = 0;
 
 //Commute variables
-int 1top, 2top, 3top;
-int 1bot, 2bot, 3bot;
-int dir
-char motor
+int top_1, top_2, top_3;
+int bot_1, bot_2, bot_3;
+int spd;
+int dir;
+char motr;
 
 //PID variables
 //char motorSelect = 0;
 double Setpointa = 0, Inputa;
 double Setpointb = 0, Inputb;
 double Setpointc = 0, Inputc;
-volatile double Outputa = 0, Outputb = 0, Outputc = 0;
+// The PID initializer was NOT liking the volatile qualifier
+
+double Outputa = 0, Outputb = 0, Outputc = 0;
+volatile double Outputa_temp = 0, Outputb_temp = 0, Outputc_temp = 0;
 
 double Kp = 1, Kd = 0.25, Ki = .05;
 

#include <std_msgs/String.h>
#include <std_msgs/Bool.h>

//#include <PID_v1.h>

//Pin definitions
//Motor stuff
int A1top = 3;
int A2top = 4;
int A3top = 5;

ros::NodeHandle  nh;

std_msgs::String state_msg;
ros::Publisher pub_state("launcher/state", &state_msg);

// Takes the current state from the interrupt service routine and 
//  decides on the next commute state
//
// Speed has to be volatile bc needs to be persistent through service
//   routines
//
// Each speed is actually output from pid
void commute(int motor, int hall1, int hall2, int hall3, int sdir){
        
	switch (motor) {
	case 1:
		top_1 = A1top;
		top_2 = A2top;
		top_3 = A3top;
		bot_1 = A1bot;
		bot_2 = A2bot;
		bot_3 = A3bot;
		spd = Outputa_temp;
		break;
	case 2:
		top_1 = B1top;
		top_2 = B2top;
		top_3 = B3top;
		bot_1 = B1bot;
		bot_2 = B2bot;
		bot_3 = B3bot;
		spd = Outputb_temp;
		break;
	case 3:
		top_1 = C1top;
		top_2 = C2top;
		top_3 = C3top;
		bot_1 = C1bot;
		bot_2 = C2bot;
		bot_3 = C3bot;
		spd = Outputc_temp;
		break;
	}

	if ( dir == 1 ){ //Forward
		//Case 1
		if ((hall1 == HIGH) && (hall2 == LOW) && (hall3 == HIGH)){
			analogWrite(top_1, spd);
			analogWrite(top_2, 0);
			analogWrite(top_3, 0);
			digitalWrite(bot_1, HIGH);
			digitalWrite(bot_2, HIGH);
			digitalWrite(bot_3, LOW);
		}
		//Case 2
		else if ((hall1 == HIGH) && (hall2 == LOW) && (hall3 == LOW)){
			analogWrite(top_1, spd);
			analogWrite(top_2, 0);
			analogWrite(top_3, 0);
			digitalWrite(bot_1, HIGH);
			digitalWrite(bot_2, LOW);
			digitalWrite(bot_3, HIGH);
		}
		//Case 3
		else if ((hall1 == HIGH) && (hall2 == HIGH) && (hall3 == LOW)){
			analogWrite(top_1, 0);
			analogWrite(top_2, spd);
			analogWrite(top_3, 0);
			digitalWrite(bot_1, LOW);
			digitalWrite(bot_2, HIGH);
			digitalWrite(bot_3, HIGH);
		}
		//Case 4
		else if ((hall1 == LOW) && (hall2 == HIGH) && (hall3 == LOW)){
			analogWrite(top_1, 0);
			analogWrite(top_2, spd);
			analogWrite(top_3, 0);
			digitalWrite(bot_1, HIGH);
			digitalWrite(bot_2, HIGH);
			digitalWrite(bot_3, LOW);
		}
		//Case 5
		else if ((hall1 == LOW) && (hall2 == HIGH) && (hall3 == HIGH)){
			analogWrite(top_1, 0);
			analogWrite(top_2, 0);
			analogWrite(top_3, spd);
			digitalWrite(bot_1, HIGH);
			digitalWrite(bot_2, LOW);
			digitalWrite(bot_3, HIGH);
		}
		//Case 6
		else if ((hall1 == LOW) && (hall2 == LOW) && (hall3 == HIGH)){
			analogWrite(top_1, 0);
			analogWrite(top_2, 0);
			analogWrite(top_3, spd);
			digitalWrite(bot_1, LOW);
			digitalWrite(bot_2, HIGH);
			digitalWrite(bot_3, HIGH);
		}
	}
	else{ //Reverse
		//Case 1
		if ((hall1 == HIGH) && (hall2 == LOW) && (hall3 == HIGH)){
			analogWrite(top_1, 0);
			analogWrite(top_2, spd);
			analogWrite(top_3, 0);
			digitalWrite(bot_1, HIGH);
			digitalWrite(bot_2, HIGH);
			digitalWrite(bot_3, LOW);
		}
		//Case 2
		else if ((hall1 == HIGH) && (hall2 == LOW) && (hall3 == LOW)){
			analogWrite(top_1, spd);
			analogWrite(top_2, 0);
			analogWrite(top_3, 0);
			digitalWrite(bot_1, LOW);
			digitalWrite(bot_2, LOW);
			digitalWrite(bot_3, HIGH);
		}
		//Case 3
		else if ((hall1 == HIGH) && (hall2 == HIGH) && (hall3 == LOW)){
			analogWrite(top_1, 0);
			analogWrite(top_2, 0);
			analogWrite(top_3, spd);
			digitalWrite(bot_1, LOW);
			digitalWrite(bot_2, HIGH);
			digitalWrite(bot_3, HIGH);
		}
		//Case 4
		else if ((hall1 == LOW) && (hall2 == HIGH) && (hall3 == LOW)){
			analogWrite(top_1, spd);
			analogWrite(top_2, 0);
			analogWrite(top_3, 0);
			digitalWrite(bot_1, HIGH);
			digitalWrite(bot_2, HIGH);
			digitalWrite(bot_3, LOW);
		}
		//Case 5
		else if ((hall1 == LOW) && (hall2 == HIGH) && (hall3 == HIGH)){
			analogWrite(top_1, spd);
			analogWrite(top_2, 0);
			analogWrite(top_3, 0);
			digitalWrite(bot_1, HIGH);
			digitalWrite(bot_2, LOW);
			digitalWrite(bot_3, HIGH);
		}
		//Case 6
		else if ((hall1 == LOW) && (hall2 == LOW) && (hall3 == HIGH)){
			analogWrite(top_1, 0);
			analogWrite(top_2, spd);
			analogWrite(top_3, 0);
			digitalWrite(bot_1, LOW);
			digitalWrite(bot_2, HIGH);
			digitalWrite(bot_3, HIGH);
		}
	}
}
