fixed roll dir bug. Left turn test

Author: col-in-coding

hardware_test/LabRobot/StableGait/QuadrupedRobot.cpp

@@ -87,6 +87,12 @@ void QuadrupedRobot::parse_code_run()
     case 3:
       bot_walk();
       break;
+    case 4:
+      bot_turn_left();
+      break;
+    case 5:
+      bot_turn_right();
+      break;
     case 0:
       switch_off();
       break;
@@ -181,6 +187,9 @@ void QuadrupedRobot::body_xyz(float x, float y, float z)
   servo_move(joint_angs_new);
 }
 
+/**
+ * Moving Body from current position
+ */
 void QuadrupedRobot::body_move_xyz(float dx, float dy, float dz)
 {
   for (size_t i = 0; i < 4; i++)
@@ -193,6 +202,26 @@ void QuadrupedRobot::body_move_xyz(float dx, float dy, float dz)
   servo_move(joint_angs_new);
 }
 
+/**
+ * Turn body
+ */
+void QuadrupedRobot::body_turn_left()
+{
+  // FL pose
+  foot_pos[0] = - step_turn[0];
+  foot_pos[2] = - step_turn[2];
+  // RL pose
+  foot_pos[9] = - step_turn[9];
+  foot_pos[11] = - step_turn[11];
+  // FR pose
+  foot_pos[3] = 0;
+  foot_pos[5] = 0;
+  // RR pose
+  foot_pos[6] = 0;
+  foot_pos[8] = 0;
+  body_move_xyz(0, 0, 2 * toe_out0);
+}
+
 /**
  * Move ith foot with one step, in the same hight work plane
  */
@@ -203,6 +232,19 @@ void QuadrupedRobot::foot_step(int i, float x, float z)
   foot_move_xyz(i, 0, -50, 0); // leg placement
 }
 
+/**
+ * Move ith foot with a turn angle
+ */
+void QuadrupedRobot::foot_step_ang(int i)
+{
+  foot_move_xyz(i, 0, 50, 0); // leg lifting
+  foot_move_xyz(i, step_turn[3 * i], 0, step_turn[3 * i + 2]);
+  foot_move_xyz(i, 0, -50, 0); // leg placement
+}
+
+/**
+ * Moving foot from current position
+ */
 void QuadrupedRobot::foot_move_xyz(int i, float dx, float dy, float dz)
 {
   foot_pos[3 * i] += dx;
@@ -213,6 +255,32 @@ void QuadrupedRobot::foot_move_xyz(int i, float dx, float dy, float dz)
   delay(200);
 }
 
+/**
+ * Calculate step_turn
+ * foot sequence: FL FR RR RL
+ * index: ith (foot) * <0(x) | 1(y) | 2(z)>
+ */
+void QuadrupedRobot::turn_pose(float ang)
+{
+  step_turn[0] = body_radius * cos(body_phi0 + ang * M_PI / 180) - feet_dist_x / 2;
+  step_turn[2] = feet_dist_z / 2 - body_radius * sin(body_phi0 + ang * M_PI / 180);
+  step_turn[3] = body_radius * cos(body_phi0 - ang * M_PI / 180) - feet_dist_x / 2;
+  step_turn[5] = body_radius * sin(body_phi0 - ang * M_PI / 180) - feet_dist_z / 2;
+  step_turn[6] = feet_dist_x / 2 - body_radius * cos(body_phi0 + ang * M_PI / 180);
+  step_turn[8] = body_radius * sin(body_phi0 + ang * M_PI / 180) - feet_dist_z / 2;
+  step_turn[9] = feet_dist_x / 2 - body_radius * cos(body_phi0 - ang * M_PI / 180);
+  step_turn[11] = feet_dist_z / 2 - body_radius * sin(body_phi0 - ang * M_PI / 180);
+
+//  Serial.println(step_turn[0]);
+//  Serial.println(step_turn[2]);
+//  Serial.println(step_turn[3]);
+//  Serial.println(step_turn[5]);
+//  Serial.println(step_turn[6]);
+//  Serial.println(step_turn[8]);
+//  Serial.println(step_turn[9]);
+//  Serial.println(step_turn[11]);
+}
+
 
 /**
  * Calculate the joint angles by foot positions
@@ -361,18 +429,18 @@ void QuadrupedRobot::bot_rest()
 void QuadrupedRobot::bot_walk()
 {
   Serial.println("walk");
-  body_move_xyz(steplen / 4, 0, -toe_out0);
-  foot_step(3, steplen / 2, 0); // move RL
+  body_move_xyz(steplen / 4, 0, toe_out0);
+  foot_step(3, steplen / 2, 0); // move FL
   foot_step(0, steplen / 2, 0); // move FL
-  body_move_xyz(steplen / 4, 0, 2 * toe_out0);
+  body_move_xyz(steplen / 4, 0, -2 * toe_out0);
   while (command_code == 3)
   {
     foot_step(2, steplen, 0); // move RR
     foot_step(1, steplen, 0); // move FR
-    body_move_xyz(steplen / 2, 0, -2 * toe_out0 );
+    body_move_xyz(steplen / 2, 0, 2 * toe_out0 );
     foot_step(3, steplen, 0); // move RL
     foot_step(0, steplen, 0); // move FL
-    body_move_xyz(steplen / 2, 0, 2 * toe_out0);
+    body_move_xyz(steplen / 2, 0, -2 * toe_out0);
     interrupt();
   }
   foot_step(2, steplen / 2, 0);
@@ -381,6 +449,38 @@ void QuadrupedRobot::bot_walk()
   delay(500);  
 }
 
+void QuadrupedRobot::bot_turn_left()
+{
+  Serial.println("turn right");
+  turn_pose(turn_phi);
+  body_xyz(toe_out0 / 2, 0, -toe_out0); // Lean left
+  foot_step_ang(2); // move RR
+  foot_step_ang(1); // move FR
+  body_turn_left(); // move body
+  foot_step_ang(0); // move FL
+//  foot_step_ang(3); // move RL
+
+  pause();
+}
+
+void QuadrupedRobot::bot_turn_right()
+{
+  Serial.println("turn left");
+  while (command_code == 5)
+  {
+    ;
+  }
+  
+}
+
+void QuadrupedRobot::pause()
+{
+  while(!interrupt())
+  {
+    delay(1000);
+  }
+}
+
 QuadrupedRobot::~QuadrupedRobot()
 {
   for (size_t i = 0; i < DOF; i++)

hardware_test/LabRobot/StableGait/QuadrupedRobot.h

@@ -41,11 +41,19 @@ class QuadrupedRobot
    * Real servo coord to calibrated coord: X-roll, Y-yaw, Z-pitch
    */
   const float init_servo_deg[12]{35, 90, 180, 125, 83, 5, 40, 85, 180, 127, 83, 0};
-  const int8_t servo_dir[12]{-1, -1, -1, -1, 1, 1, 1, 1, 1, 1, -1, -1};
+  const int8_t servo_dir[12]{1, -1, -1, 1, 1, 1, -1, 1, 1, -1, -1, -1};
   const float toe_out0{30}; // outward distance of toe during stand, in mm
   const float dist_ag{30};  // distance between alfa and gamma axis, in mm
   const float thigh{107};
   const float calf{90};
+  /**
+   * Body configs
+   */
+  const float feet_dist_z{70}; // distance between feet in z axis, in mm
+  const float feet_dist_x{220}; // distance between feet in x axis, in mm
+  // distance from center to the foot
+  const float body_radius{sqrt(pow(feet_dist_z / 2, 2) + pow(feet_dist_x / 2, 2))};
+  const float body_phi0{atan(feet_dist_z / feet_dist_x)};
   /**
    * vleg_len: virtual leg length (composite with 2 limbs)
    * alfa, gamma, beta is conresponding to angles on HipZ, Knee, HipX axises
@@ -64,14 +72,15 @@ class QuadrupedRobot
   float joint_angs_new[12]{};
   float vlegs_len[4]{};
   float foot_pos[12] {};
+  float step_turn[12] {}; // foot step in case of turn
   /**
    * Configed States
    * Angles in callibrated coordinate
    */
   const float stand_angs[12]{0, -30, 60, 0, -30, 60, 0, 30, -60, 0, 30, -60};
   const float rest_angs[12]{0, -55, 130, 0, -55, 130, 0, 55, -130, 0, 55, -130};
-  const float adjust_angs[12] {};
-
+  const float adjust_angs[12]{};
+  const float turn_phi{20};
   // 
   // X points to forward, Z points to upward
 
@@ -84,8 +93,12 @@ class QuadrupedRobot
 
   void body_xyz(float x, float y, float z);
   void body_move_xyz(float dx, float dy, float dz);
+  void body_turn_left();
+  void body_turn_right();
   void foot_step(int i, float x, float z);
+  void foot_step_ang(int i);
   void foot_move_xyz(int i, float dx, float dy, float dz);
+  void turn_pose(float ang);
 
   void inverse_kinematics();
   float gamma_left(float dy, float dz);
@@ -97,6 +110,8 @@ class QuadrupedRobot
   float alfa_front(float dx, float beta, float vleg_len);
   float alfa_rear(float dx, float beta, float vleg_len);
 
+  void pause();
+
 public:
   ~QuadrupedRobot();
   bool interrupt();
@@ -109,6 +124,8 @@ class QuadrupedRobot
   void bot_rest();
   void bot_stand();
   void bot_walk();
+  void bot_turn_right();
+  void bot_turn_left();
 };
 
 #endif

hardware_test/LabRobot/StableGait/StableGait.ino

@@ -15,6 +15,8 @@ void setup() {
     "  1. switch on\n"
     "  2. stand\n"
     "  3. walk\n"
+    "  4. turn left\n"
+    "  5. turn right\n"
     "  0. switch off\n"
     "  998. adjust angles\n"
   );