 ros::NodeHandle  nh;
 
 std_msgs::String state_msg;
-ros::Publisher state_pub("launcher/state", &state_msg);
-
-void setMotorSpeed(int motor, double speed){
-  // Set the motor specified to the speed given.
-  bool is_set = false;
-  while (!is_set){
-    Inputa = curspeedA;
-    Inputb = curspeedB;
-    Inputc = curspeedC;
-
-    Setpointa = motorSpeedA;
-    Setpointb = motorSpeedB;
-    Setpointc = motorSpeedC;
-    // use hall effect and pid to tune
-    aPID.compute();
-    bPID.compute();
-    cPID.compute();
-
-	if ((curspeedA - oldspeedA) < 0.05) && ((curspeedB - oldspeedB) < 0.05) && ((curspeedC - oldspeedC) < 0.05){
-		is_set = true;
-	}
-  
-}
-
-void commute(motor, hall1, hall2, hall3, dir){
-
+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:
-		1top = A1top;
-		2top = A2top;
-		3top = A3top;
-		1bot = A1bot;
-		2bot = A2bot;
-		3bot = A3bot;
-		spd = Outputa;
+		top_1 = A1top;
+		top_2 = A2top;
+		top_3 = A3top;
+		bot_1 = A1bot;
+		bot_2 = A2bot;
+		bot_3 = A3bot;
+		spd = Outputa_temp;
 		break;
 	case 2:
-		1top = B1top;
-		2top = B2top;
-		3top = B3top;
-		1bot = B1bot;
-		2bot = B2bot;
-		3bot = B3bot;
-		spd = Outputb;
+		top_1 = B1top;
+		top_2 = B2top;
+		top_3 = B3top;
+		bot_1 = B1bot;
+		bot_2 = B2bot;
+		bot_3 = B3bot;
+		spd = Outputb_temp;
 		break;
 	case 3:
-		1top = C1top;
-		2top = C2top;
-		3top = C3top;
-		1bot = C1bot;
-		2bot = C2bot;
-		3bot = C3bot;
-		spd = Outputc;
+		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(1top, spd);
-			analogWrite(2top, 0);
-			analogWrite(3top, 0);
-			digitalWrite(1bot, HIGH);
-			digitalWrite(2bot, HIGH);
-			digitalWrite(3bot, 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 2
 		else if ((hall1 == HIGH) && (hall2 == LOW) && (hall3 == LOW)){
-			analogWrite(1top, spd);
-			analogWrite(2top, 0);
-			analogWrite(3top, 0);
-			digitalWrite(1bot, HIGH);
-			digitalWrite(2bot, LOW);
-			digitalWrite(3bot, 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 3
 		else if ((hall1 == HIGH) && (hall2 == HIGH) && (hall3 == LOW)){
-			analogWrite(1top, 0);
-			analogWrite(2top, spd);
-			analogWrite(3top, 0);
-			digitalWrite(1bot, LOW);
-			digitalWrite(2bot, HIGH);
-			digitalWrite(3bot, 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);
 		}
 		//Case 4
 		else if ((hall1 == LOW) && (hall2 == HIGH) && (hall3 == LOW)){
-			analogWrite(1top, 0);
-			analogWrite(2top, spd);
-			analogWrite(3top, 0);
-			digitalWrite(1bot, HIGH);
-			digitalWrite(2bot, HIGH);
-			digitalWrite(3bot, 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(1top, 0);
-			analogWrite(2top, 0);
-			analogWrite(3top, spd);
-			digitalWrite(1bot, HIGH);
-			digitalWrite(2bot, LOW);
-			digitalWrite(3bot, 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(1top, 0);
-			analogWrite(2top, 0);
-			analogWrite(3top, spd);
-			digitalWrite(1bot, LOW);
-			digitalWrite(2bot, HIGH);
-			digitalWrite(3bot, 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(1top, 0);
-			analogWrite(2top, spd);
-			analogWrite(3top, 0);
-			digitalWrite(1bot, HIGH);
-			digitalWrite(2bot, HIGH);
-			digitalWrite(3bot, 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 2
 		else if ((hall1 == HIGH) && (hall2 == LOW) && (hall3 == LOW)){
-			analogWrite(1top, spd);
-			analogWrite(2top, 0);
-			analogWrite(3top, 0);
-			digitalWrite(1bot, LOW);
-			digitalWrite(2bot, LOW);
-			digitalWrite(3bot, HIGH);
+			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(1top, 0);
-			analogWrite(2top, 0);
-			analogWrite(3top, spd);
-			digitalWrite(1bot, LOW);
-			digitalWrite(2bot, HIGH);
-			digitalWrite(3bot, 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);
 		}
 		//Case 4
 		else if ((hall1 == LOW) && (hall2 == HIGH) && (hall3 == LOW)){
-			analogWrite(1top, spd);
-			analogWrite(2top, 0);
-			analogWrite(3top, 0);
-			digitalWrite(1bot, HIGH);
-			digitalWrite(2bot, HIGH);
-			digitalWrite(3bot, 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(1top, spd);
-			analogWrite(2top, 0);
-			analogWrite(3top, 0);
-			digitalWrite(1bot, HIGH);
-			digitalWrite(2bot, LOW);
-			digitalWrite(3bot, 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(1top, 0);
-			analogWrite(2top, spd);
-			analogWrite(3top, 0);
-			digitalWrite(1bot, LOW);
-			digitalWrite(2bot, HIGH);
-			digitalWrite(3bot, 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);
 		}
 	}
 }

#endif

#include <Servo.h> 
#include <ros.h>
#include <std_msgs/UInt16.h>
#include <std_msgs/Float64.h>

	// publish command saying that we are adjusting
	std_msgs::String str;
	str.data = "adjusting";
	pub_state.publish(&str);

	// Get motor speed A, B, and C from the message and set
        Setpointa = cmd_msg.x;
        Setpointb = cmd_msg.y;
        Setpointc = cmd_msg.z;

	// verification done in loop, running constantly through PID library

	// publish command alerting the master system that we are ready
	//  std_msgs::String str;
	str.data = "ready";
	pub_state.publish(&str);
}

ros::Subscriber<geometry_msgs::Vector3> motor_sub("launcher/motor_vel/", motor_controls_cb);

void pid_controls_cb(const geometry_msgs::Vector3& pid_msg){

	// publish command saying that we are adjusting
	std_msgs::String str;
	str.data = "Reading PID";
	pub_state.publish(&str);

	// Get motor speed A, B, and C from the message
	Kp = pid_msg.x;
	Kd = pid_msg.y;
	Ki = pid_msg.z;

	// publish command alerting the master system that we are ready
	//  std_msgs::String str;
	str.data = "Updated PID";
	pub_state.publish(&str);
}

ros::Subscriber<geometry_msgs::Vector3> pid_sub("launcher/pid_val/", pid_controls_cb);

void setup(){
	//Pinmodes for the motor's PWM
