   analogWrite(motor_pin, 0);
 }
 
-void load_ball(){
-  // While we haven't seen the switch be high, 
-  //   run the motor
-  
-  while(digitalRead(switch_pin) != HIGH){
-    run_motor(motor_speed); 
+//void stop_motor_interrupt(){
+//  if switc
+//}
 
 
-    delay(10);
-    break; // put this in here to test, don't want to kill the motor
-  }
-  
-  stop_motor();
-  
-}
+
+//void load_ball(){
+//  // While we haven't seen the switch be high, 
+////  //   run the motor
+////  
+////  while(digitalRead(switch_pin) != HIGH){
+////    run_motor(motor_speed); 
+////
+////
+////    delay(10); // run for .1 seconds.
+//////    break; // put this in here to test, don't want to kill the motor
+////  }
+////  
+////  stop_motor();
+//  
+//}
 
 void load_cmd_cb(const std_msgs::Bool& cmd_msg){
   

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

const int switch_pin = 5;
const int led_pin = 13;
const int motor_pin = 9;

const int motor_speed = 50;

//bool last_reading;
//long last_debounce_time=0;
//long debounce_delay=50;
//bool published = true;

void run_motor(int motor_speed){
  analogWrite(motor_pin, motor_speed);

    pub_state.publish(&state_msg);
    digitalWrite(led_pin, HIGH);
    
    run_motor(motor_speed);
//    load_ball();
    
//    delay(100);
    state_msg.data = "loaded";
    pub_state.publish(&state_msg);
    digitalWrite(led_pin,LOW);
    
  } else {
    stop_motor();
  } 
  
}

ros::Subscriber<std_msgs::Bool> load_sub("loader/load_cmd", load_cmd_cb);

void setup()
{
  nh.initNode();
  nh.advertise(pub_state);
  nh.subscribe(load_sub);
  
  //initialize an LED output pin for status purposes
  //init input pin for switch input and motor control pin
  pinMode(led_pin, OUTPUT);
  pinMode(switch_pin, INPUT);
  pinMode(motor_pin, OUTPUT);
  
  // set the motor to be initially off
  setPwmFrequency(motor_pin, 1); // highest pwm frequency
  stop_motor();
  
  // interrupt on the switch pin that 
  //  turns the motor off when the switch pin
  //   goes from low to high
  attachInterrupt(0, stop_motor, RISING);
 
}

