 void timeOut2Handler() {
   if (micros() - last_time2 > 5000) motor_speed2 = 0;
 }
-
 // ========================================= don't see above section =======================================//
 
-<<<<<<< HEAD
-
-void setup() {
-  Serial.begin(115200);
-=======
 void receive_info() {
-   String command1 =  Serial.readStringUntil('#');
-   Serial.print("after somdthing");
-   char command[30];
-   command1.toCharArray(command, 30);
-   char *parseChar1; // For state
-   char *parseChar2; // For turning_speed
-   char *parseChar3; // For catapult
-   char *parseChar4; // For the last null
-   parseChar1 = strtok(command, " ");
-   parseChar2 = strtok(NULL, " ");
-   parseChar3 = strtok(NULL, " ");
-   parseChar4 = strtok(NULL, " ");
-   /* Integrety checking */
-   int cur_state = atoi(parseChar1) - 1;
-   int cur_speed = atoi(parseChar2);
-   int cur_cata = atoi(parseChar3) - 1;
-   
-   if (cur_state < 0 || cur_state > (right_turn + 1)) {
-    //Serial.println("Fail state");
+  int raw_command = Serial.read() - 49;
+  // Integrity checking
+  if (raw_command < idle || raw_command > right_turn) {
+    Serial.print("Fail state");
     return;
-   }
-   if (cur_cata < 0 || cur_cata > 1) {
-    //Serial.println("Fail cata");
-    return;
-   }
-   if (!parseChar2 || !parseChar3 || parseChar4) {
-    //Serial.println("Fail token number");
-    return;
-   }
-   
-   gesture_state = (state)cur_state;
-   turning_speed = cur_speed;
-   should_catapult = (cur_cata > 0);
-   //Serial.println(command1);
-   //Serial.println(gesture_state);
-   //Serial.println(turning_speed);
-   //Serial.println(should_catapult);
-   
-    
-
-   
-   //int int_command1 = command1.toInt();
-   //String command2 = Serial.readStringUntil(' ');
-   //int int_command2 = command2.toInt();
-   //String command3 = Serial.readStringUntil('#');
-   //int int_command3 = command3.toInt();
-   //Serial.println("Command: ");
-   //Serial.println(int_command1);
-   //Serial.println(int_command2);
-   //Serial.println(int_command3);
-   
-  
-}
+  }
+  command = (state)raw_command;
+  Serial.print(command);
+}
+
 void setup() {
   Serial.begin(115200);
   digitalWrite(8,LOW);
->>>>>>> 206c86daf8a79e890468a8966aa9a831e7c4b4e7
   pinMode(21, OUTPUT);
   pinMode(12, OUTPUT);
   pinMode(13, OUTPUT);

    Serial.println();
  }

  // =========================================== 3. angle pid control ============================================== //
  robot_angle = kalAngleX;
  error = robot_angle - target_angle;
  delta_error = error - last_error;

  if (control_decision > 255) {control_decision = 255;integrated_error -= error;}
  else if (control_decision< -255) {control_decision = -255;integrated_error -= error;}  
  if (control_decision > 0){
    motorControl(control_decision * factor1, control_decision * factor2,1,1);
  } else {
    motorControl(-control_decision * factor1, -control_decision * factor2, 2,2);
  }
  last_error = error;
  if (0) { // set to 1 to activate
    Serial.print("  robot_angle=");Serial.println(robot_angle);
    Serial.print("  control decision=");Serial.println(control_decision);
  }
  //delay(2);
}
