 int C1bot = A1;
 int C2bot = A2;
 int C3bot = A3;
+
 //Hall effect sensors
 int ha1 = 2;
 int ha2 = A5;

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){

	switch (motor) {
	case 1:
		1top = A1top;
		2top = A2top;
		3top = A3top;
		1bot = A1bot;
		2bot = A2bot;
		3bot = A3bot;
		spd = Outputa;
		break;
	case 2:
		1top = B1top;
		2top = B2top;
		3top = B3top;
		1bot = B1bot;
		2bot = B2bot;
		3bot = B3bot;
		spd = Outputb;
		break;
	case 3:
		1top = C1top;
		2top = C2top;
		3top = C3top;
		1bot = C1bot;
		2bot = C2bot;
		3bot = C3bot;
		spd = Outputc;
		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);
		}
		//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);
		}
		//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);
		}
		//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);
		}
		//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);
		}
		//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);
		}
	}
	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);
		}
		//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);
		}
		//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);
		}
		//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);
		}
		//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);
		}
		//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);
		}
	}
}

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

#include <PID_v1.h>



//Pin definitions
//Motor stuff, enum class
int MotorA = 67;
int MotorB = 68;
int MotorC = 69;


int A1top = 3;
int A2top = 4;
int A3top = 5;
