 radiopacket_t recv_packet;
 radiopacket_t send_packet;
 
-// util function to set the sender addresss
-void setSenderAddress(uint8_t* dest, uint8_t src);
-
 // setup a move roomba command packet
 // speed = -500 --> 500 mm/sec
 // deg = -2000 --> 2000 mm/sec
-void moveCommand(int spd,int deg);
+void moveCommand(radiopacket_t* packet,int spd,int deg);
 
 // setup a roomba ir_command packet
 // sending the specified uint8_t byte
-void fireGun(uint8_t b);
+void fireGun(radiopacket_t* packet,uint8_t b);
+
+typedef struct _input_t {
+  uint16_t button;
+  int16_t x_value;
+  int16_t y_value;
+} input_t;
+input_t input;
 
 // read in input, setup a firePacket
 void check_button(){
   // read the state of the switch into a local variable:
-  int val = analogRead(button_pin);
-  if( val < 50 ){
-    fireGun('B');
-  }
-}
-
-int16_t old_x = -1;
-int16_t old_y = -1;
+  input.button = analogRead(button_pin);  
+  if( input.button < 50 ){
+    if( FIREGUN >= txflag){
+      txflag = FIREGUN;      
+    }
+  }
+}
 
 int16_t mapSpeedValue(int16_t val){
- return map(val,0,4,-200,200); 
+  return map(val,0,4,-500,500); 
 }
 
 int16_t mapRotValue(int16_t val){
- return map(val,0,4,-200,200);
+  return map(val,0,4,-200,200);
 }
 
 // read in input. setup a move packet
 void handle_servo(){ 
-   uint16_t x_value = map(analogRead(joystick_x_pin), 0, 1023, 0, 5);            // reads the value of the potentiometer (value between 0 and 1023) 
-   uint16_t y_value = map(analogRead(joystick_y_pin), 0, 1023, 0, 5);            // reads the value of the potentiometer (value between 0 and 1023) 
-
-//  if(old_x == x_value && old_y == y_value){
-//    return;
-//  }
-//  old_x = x_value;
-//  old_y = y_value;
-
-  // map the x and y values to the correct ranges 
-   if( x_value == 2 && y_value == 2)
-   {
-    // stop  
-      moveCommand(0,(int16_t)0x8000);
+  // reads the value of the potentiometer (value between 0 and 1023) 
+  input.x_value = map(analogRead(joystick_x_pin), 0, 1023, 0, 5);
+  input.y_value = map(analogRead(joystick_y_pin), 0, 1023, 0, 5);
+
+  if( MOVECOMMAND >= txflag){
+    txflag = MOVECOMMAND;
+  }  
+}
+
+
+// recv_packet code
+void recv_packet_task(){
+  if(rxflag == 0){return;}
+  
+  // remember always to read the packet out of the radio, even
+  // if you don't use the data.
+  if (Radio_Receive(&recv_packet) != RADIO_RX_MORE_PACKETS){
+    // if there are no more packets on the radio, clear the receive flag;
+    // otherwise, we want to handle the next packet on the next loop iteration.
+    rxflag = 0;
+  }
+
+  txflag = DONTSEND;
+  if( recv_packet.type == SENSOR_DATA){
+    Serial.println("SENSOR_DATA");         
+  }else if(recv_packet.type == IR_DATA){
+    Serial.println("IR_DATA:");
+    Serial.println(recv_packet.payload.ir_data.roomba_number);
+    Serial.println(recv_packet.payload.ir_data.ir_data);
+  }else{
+    Serial.println("Unknown recv packet"); 
+  }  
+}
+
+// send packet code
+void send_packet_task(){
+  if( txflag == DONTSEND){return;}
+  Serial.println("sending packet");
+
+  if(txflag == FIREGUN){
+    // send a fire gun packet
+    fireGun(&send_packet,'A');
+  }else if(txflag == MOVECOMMAND){
+    // send a move packet
+    // map the x and y values to the correct ranges 
+    uint16_t x_value = input.x_value;
+    uint16_t y_value = input.y_value;
+    if( x_value == 2 && y_value == 2){
+      // stop  
+      moveCommand(&send_packet,0,(int16_t)0x8000);
     } else if( x_value == 2){
-    // move straight
+      // move straight
       y_value = -mapSpeedValue(y_value);
-      moveCommand(y_value,(int16_t)0x8000);
+      moveCommand(&send_packet,y_value,(int16_t)0x8000);
     } else if( y_value == 2){
-    // turn in place
+      // turn in place
       uint16_t deg = -1;
       if( x_value < 2 ){
         deg = 1;
       }
-      moveCommand(200,deg);
+      moveCommand(&send_packet,200,deg);
     }else{
       x_value = -mapRotValue(x_value); 
       y_value = -mapSpeedValue(y_value);      
-      moveCommand(y_value,x_value);
+      moveCommand(&send_packet,y_value,x_value);
     }
-
-  
-}
-
-
-// recv_packet code
-void recv_packet_task(){
-  if (rxflag)
-  {
-          // remember always to read the packet out of the radio, even
-         // if you don't use the data.
-     if (Radio_Receive(&recv_packet) != RADIO_RX_MORE_PACKETS)
-     {
-            // if there are no more packets on the radio, clear the receive flag;
-            // otherwise, we want to handle the next packet on the next loop iteration.
-      rxflag = 0;
-    }
-    
-    if( recv_packet.type == SENSOR_DATA){
-      Serial.println("SENSOR_DATA");         
-    }else if(recv_packet.type == IR_DATA){
-      Serial.println("IR_DATA:");
-      Serial.println(recv_packet.payload.ir_data.roomba_number);
-      Serial.println(recv_packet.payload.ir_data.ir_data);
-    }else{
-      Serial.println("Unknown recv packet"); 
-    }    
-  }
-}
-
-// send packet code
-void send_packet_task(){
-  if( txflag == 0){return;}
-  Serial.println("sending packet");
-  Radio_Transmit(&send_packet, RADIO_WAIT_FOR_TX);
-  txflag = 0;
+  }
+
+  // transmit the send packet
+  Radio_Transmit(&send_packet, RADIO_WAIT_FOR_TX);  
 }
 
 void setup() {

  Radio_Configure(RADIO_2MBPS, RADIO_HIGHEST_POWER);
  
  Radio_Set_Tx_Addr(ROOMBA_ADDRESSES[roomba_num]);
  
  // Thanks Neil!  
  Scheduler_Init();
  Scheduler_StartTask(0,20,check_button);
  Scheduler_StartTask(8,20,handle_servo);
  Scheduler_StartTask(5,20,recv_packet_task);
  Scheduler_StartTask(1,200,send_packet_task);
}

void idle(uint32_t idle_period){
  delay(idle_period); 
}

void radio_rxhandler(uint8_t pipe_number)
{
 rxflag = 1;
}

void loop() {
  uint32_t idle_period = Scheduler_Dispatch();
  if( idle_period){
   idle(idle_period); 
 }
}

void setSenderAddress(uint8_t* dest, uint8_t *src,int len){
  for(int i = 0;i < len ;++i){
    dest[i] = src[i];
  }
}

void fireGun(uint8_t b){
  if( txflag == 1){
    if (send_packet.type = IR_COMMAND){
      return;
    }
  }
  send_packet.type = IR_COMMAND;
  
  pf_ir_command_t* cmd = &(send_packet.payload.ir_command);
  setSenderAddress(cmd->sender_address,my_addr,5);  
  cmd->ir_command = SEND_BYTE;
  cmd->ir_data = 0;
  cmd->servo_angle = 0;
  txflag = 1;
}

void moveCommand(int16_t spd, int16_t deg){
  if(txflag == 1){return;}    
  
  // clear_the_packet(&send_packet);
  
  send_packet.type = COMMAND;
  
  pf_command_t* cmd = &(send_packet.payload.command);
  setSenderAddress(cmd->sender_address,my_addr,5);
  
  cmd->command = 137; // DRIVE op code
  cmd->num_arg_bytes = 4;
  
  // clear the arguments array to zero;
//  memset(cmd->arguments,0,sizeof(uint8_t)*16);
  
  // set the  speed
  cmd->arguments[0] = HIGH_BYTE(spd);

  // set the amount of rotation
  cmd->arguments[2] = HIGH_BYTE(deg);
  cmd->arguments[3] = LOW_BYTE(deg);
}