From e0a57b5fd958fb8f141d1012a0826ab8a9e59ade Mon Sep 17 00:00:00 2001
From: felipepsaraiva <felipepiressaraiva@gmail.com>
Date: Fri, 8 Nov 2019 20:21:07 -0300
Subject: [PATCH] Comandos de chutar e defender

---
 src/ServoManager.cpp      |  8 ++++----
 src/extras/compliance.cpp |  3 ---
 src/peripherals.cpp       | 27 ++++++++++++++++++---------
 3 files changed, 22 insertions(+), 16 deletions(-)

diff --git a/src/ServoManager.cpp b/src/ServoManager.cpp
index a7bf1d8..9d92f68 100644
--- a/src/ServoManager.cpp
+++ b/src/ServoManager.cpp
@@ -3,13 +3,13 @@
 ServoManager manager;
 
 ServoManager::ServoManager(ManagerState start_state)
-    : servos{{BodyServo(RIGHT_ANKLE_ROLL, RIGHT_ANKLE_ROLL_ID, 0, 10, false),
+    : servos{{BodyServo(RIGHT_ANKLE_ROLL, RIGHT_ANKLE_ROLL_ID, 0, -20, false),
               BodyServo(RIGHT_ANKLE_PITCH, RIGHT_ANKLE_PITCH_ID, 0, 0, true),
               BodyServo(RIGHT_KNEE, RIGHT_KNEE_ID, 0, -20, false),
               BodyServo(RIGHT_HIP_PITCH, RIGHT_HIP_PITCH_ID, 0, -5, false),
-              BodyServo(RIGHT_HIP_ROLL, RIGHT_HIP_ROLL_ID, 0, -30, false),
+              BodyServo(RIGHT_HIP_ROLL, RIGHT_HIP_ROLL_ID, 0, -90, false),
               BodyServo(RIGHT_HIP_YAW, RIGHT_HIP_YAW_ID, 0, 0, true),
-              BodyServo(LEFT_ANKLE_ROLL, LEFT_ANKLE_ROLL_ID, 0, -60, false),
+              BodyServo(LEFT_ANKLE_ROLL, LEFT_ANKLE_ROLL_ID, 0, -50, false),
               BodyServo(LEFT_ANKLE_PITCH, LEFT_ANKLE_PITCH_ID, 0, 10, true),
               BodyServo(LEFT_KNEE, LEFT_KNEE_ID, 0, 0, false),
               BodyServo(LEFT_HIP_PITCH, LEFT_HIP_PITCH_ID, 0, -40, false),
@@ -17,7 +17,7 @@ ServoManager::ServoManager(ManagerState start_state)
               BodyServo(LEFT_HIP_YAW, LEFT_HIP_YAW_ID, 0, 0, true),
               BodyServo(LEFT_ARM_PITCH, LEFT_ARM_PITCH_ID, 0, 0, false),
               BodyServo(LEFT_ARM_YAW, LEFT_ARM_YAW_ID, 0, 0, false),
-              BodyServo(LEFT_ARM_ROLL, LEFT_ARM_ROLL_ID, 0, 0, false),
+              BodyServo(LEFT_ARM_ROLL, LEFT_ARM_ROLL_ID, 0, -20, false),
               BodyServo(RIGHT_ARM_PITCH, RIGHT_ARM_PITCH_ID, 0, 0, false),
               BodyServo(RIGHT_ARM_YAW, RIGHT_ARM_YAW_ID, 0, 0, false),
               BodyServo(RIGHT_ARM_ROLL, RIGHT_ARM_ROLL_ID, 0, 0, false)}},
diff --git a/src/extras/compliance.cpp b/src/extras/compliance.cpp
index ebaaa33..4b2eba6 100644
--- a/src/extras/compliance.cpp
+++ b/src/extras/compliance.cpp
@@ -66,9 +66,6 @@ void setup() {
   SERIAL_SERVOS.setTimeout(20);
 
   delay(3000);
-
-  for (auto servo : servos)
-    servo.setPosition(1024 / 2, 1);
 }
 
 bool updateServo(XYZrobotServo& servo) {
diff --git a/src/peripherals.cpp b/src/peripherals.cpp
index 7cfbff2..0f51eb7 100644
--- a/src/peripherals.cpp
+++ b/src/peripherals.cpp
@@ -2,22 +2,31 @@
 
 time_t btn_last_press[] = {0, 0, 0, 0, 0};
 
-void run_button0_action() {
-  control.publish_command("reset");
-  manager.reset();
-}
+void run_button0_action() {}
+
+// void run_button1_action() {
+//   control.publish_command(control.status.is_mode_manual ? "set_mode_auto"
+//                                                         : "set_mode_manual");
+// }
+
+// void run_button2_action() {
+//   if (control.status.is_mode_manual)
+//     control.publish_command("walk");
+// }
 
 void run_button1_action() {
-  control.publish_command(control.status.is_mode_manual ? "set_mode_auto"
-                                                        : "set_mode_manual");
+  control.publish_command("move kick_left");
 }
 
 void run_button2_action() {
-  if (control.status.is_mode_manual)
-    control.publish_command("walk");
+  control.publish_command("defend");
 }
 
-void run_button3_action() {}
+void run_button3_action() {
+  // control.publish_command("move wave");
+  control.publish_command("reset");
+  manager.reset();
+}
 
 void run_button4_action() {
   if (manager.get_state() == ManagerState::WaitServo) {