lab robot with CPG, done sigle leg test

col-in-coding · col-in-coding · commit 88c41b951544 · 2020-03-06T19:50:33.000+08:00
diff --git a/.gitignore b/.gitignore
@@ -1,2 +1,4 @@
 .DS_Store
-.idea
+.idea
+
+.vscode
diff --git a/hardware_test/LabRobot/DogRobot/DogRobot.ino b/hardware_test/LabRobot/DogRobot/DogRobot.ino
@@ -17,33 +17,47 @@ void setup() {
   PTL("  0. switch off");
 }
 
-String inString = "";
+int inp = "";
+int code = 999;
 void loop() {
-if (Serial.available() > 0) {
+  if (Serial.available() > 0) {
     if (Serial.peek() != '\n') {
-      inString += (char)Serial.read();
+      inp = Serial.parseInt();
     } else {
       Serial.read();
       Serial.print("Instruction received: ");
-      
-      int code = inString.toInt();
-      Serial.println(code);
-      switch(code) {
-        case 1:
-          Robot.switch_on();
-          break;
-        case 2:
-          Robot.bot_stand();
-          break;
-        case 0:
-          Robot.switch_off();
-          break;
-        default:
-          PTL("wrong code");
-      }
+      Serial.println(inp);
 
-      inString = "";
+      code = inp;
+      Robot.lock = false; // unlock the robot
+      inp = "";
     }
   }
 
+  switch(code) {
+    case 1:
+      Robot.switch_on();
+      // lock the robot, unless the periodic motion code
+      Robot.lock = true;
+      break;
+    case 2:
+      Robot.bot_stand();
+      Robot.lock = true;
+      break;
+    case 3:
+      Robot.bot_walk();
+//      Robot.lock = true; // for test
+      break;
+    case 0:
+      Robot.switch_off();
+      Robot.lock = true;
+      break;
+    case 999:
+      // waiting for input ...
+      break;
+    default:
+      PTL("wrong code");
+      code = 999;
+  }
+
 }
diff --git a/hardware_test/LabRobot/DogRobot/QuadrupedRobot.cpp b/hardware_test/LabRobot/DogRobot/QuadrupedRobot.cpp
@@ -1,67 +1,158 @@
+
+#include <Arduino.h>
 #include "QuadrupedRobot.h"
-#include "Arduino.h"
 
 
-void QuadrupedRobot::setup_servos() {
+void QuadrupedRobot::setup_servos()
+{
   PTL("Setup Servos");
-  for (size_t i = 0; i < DOF; i++) {
-    if(servos[i] == nullptr) servos[i] = new Servo();
+  for (size_t i = 0; i < DOF; i++)
+  {
+    if (servos[i] == nullptr)
+      servos[i] = new Servo();
     servos[i]->attach(pins[i], SERVOMIN, SERVOMAX);
   }
 }
 
-void QuadrupedRobot::shut_servos() {
-  for (size_t i = 0; i < DOF; i++) {
-    if (servos[i] != nullptr) {
+void QuadrupedRobot::shut_servos()
+{
+  for (size_t i = 0; i < DOF; i++)
+  {
+    if (servos[i] != nullptr)
+    {
       servos[i]->detach();
     }
   }
 }
 
-void QuadrupedRobot::servo_write_angs(double angs[DOF]) {
-  PTL("Servo Write Angles");
-  double real_ang {};
-  double pulsewidth {};
-  for (size_t i = 0; i < DOF; i++) {
+void QuadrupedRobot::servo_write_angs(double angs[DOF], bool angs_in_rad=false)
+{
+//  PTL("Servo Write Angles");
+  double real_ang{};
+  double pulsewidth{};
+  for (size_t i = 0; i < DOF; i++)
+  {
+    if (angs_in_rad) {
+      angs[i] = angs[i] * 180 / M_PI;
+    }
     real_ang = init_angs[i] + dir[i] * angs[i];
     pulsewidth = 500 + real_ang / 90.0 * 1000;
-    if (!servos[i]->attached()) {
+    if (!servos[i]->attached())
+    {
       servos[i]->attach(pins[i], SERVOMIN, SERVOMAX);
     }
     servos[i]->writeMicroseconds(pulsewidth);
   }
-  delayMicroseconds(3000);
+  delay(5);
 }
 
-QuadrupedRobot::QuadrupedRobot() {
-
+QuadrupedRobot::QuadrupedRobot()
+{
+  // init data for CPG
+  omega_st = omega_sw * (1 - beta) / beta;
+  mu = Ah * Ah;
+  for (int i = 0; i < 4; i++) {
+    for (int j = 0; j < 4; j++) {
+      theta[i][j] = 2 * M_PI * (phi[i] - phi[j]);
+    }
+  }
 }
 
-QuadrupedRobot::~QuadrupedRobot() {
-  
+QuadrupedRobot::~QuadrupedRobot()
+{
 }
 
-void QuadrupedRobot::switch_on() {
+void QuadrupedRobot::switch_on()
+{
+  if(lock) return;
   PTL("Robot Switch On");
   setup_servos();
   bot_rest();
   delay(1000);
   shut_servos();
 }
 
-void QuadrupedRobot::switch_off() {
+void QuadrupedRobot::switch_off()
+{
+  if(lock) return;
   PTL("Robot Switch Off");
   shut_servos();
 }
 
-void QuadrupedRobot::bot_rest() {
+void QuadrupedRobot::bot_rest()
+{
+  if(lock) return;
   PTL("Robot Rest");
-  double rest_angs[12] {0, -55, 130, 0, -55, 130, 0, 55, -130, 0, 55, -130};
+  double rest_angs[12]{0, -55, 130, 0, -55, 130, 0, 55, -130, 0, 55, -130};
   servo_write_angs(rest_angs);
 }
 
-void QuadrupedRobot::bot_stand() {
+void QuadrupedRobot::bot_stand()
+{
+  if(lock) return;
   PTL("Robot Stand");
-  double stand_angs[12] {0, -30, 60, 0, -30, 60, 0, 30, -60, 0, 30, -60};
+  double stand_angs[12]{0, -30, 60, 0, -30, 60, 0, 30, -60, 0, 30, -60};
   servo_write_angs(stand_angs);
 }
+
+void QuadrupedRobot::bot_walk()
+{
+  if(lock) return;
+  double angs[12] {};
+  double stand_angs[12]{0, -30, 60, 0, -30, 60, 0, 30, -60, 0, 30, -60};
+  // dt = 0.0001s, dt * 100 = 0.01s
+  for (size_t itr = 0; itr < 100; itr++) {
+    for (int i = 0; i < 1; i++) {
+      r_square[i] = x[i] * x[i] + y[i] * y[i]; // 1
+   
+      // Frequency of the oscillator
+      omega[i] = omega_st / (exp(- a * y[i]) + 1) + omega_sw / (exp(a * y[i]) + 1); // 9.42
+      
+      // HOPF Oscillator
+      x_dot[i] = alpha * (mu - r_square[i]) * x[i] - omega[i] * y[i];
+      y_dot[i] = alpha * (mu - r_square[i]) * y[i] + omega[i] * x[i];
+      
+      /**
+       * coupling terms
+       * [
+       *   R11 R21 R31 R41
+       *   R12 R22 R32 R42
+       *   R13 R23 R33 R43
+       *   R14 R24 R34 R44
+       * ]
+       */
+  //    x_dot[i] += cos(theta[0][i]) * x[0] - sin(theta[0][i]) * y[0];
+  //    y_dot[i] += sin(theta[0][i]) * x[0] + cos(theta[0][i]) * y[0];
+  //    x_dot[i] += cos(theta[1][i]) * x[1] - sin(theta[1][i]) * y[1];
+  //    y_dot[i] += sin(theta[1][i]) * x[1] + cos(theta[1][i]) * y[1];
+  //    x_dot[i] += cos(theta[2][i]) * x[2] - sin(theta[2][i]) * y[2];
+  //    y_dot[i] += sin(theta[2][i]) * x[2] + cos(theta[2][i]) * y[2];
+  //    x_dot[i] += cos(theta[3][i]) * x[3] - sin(theta[3][i]) * y[3];
+  //    y_dot[i] += sin(theta[3][i]) * x[3] + cos(theta[3][i]) * y[3];
+  
+      // update the signal values
+      x[i] += x_dot[i] * dt;
+      y[i] += y_dot[i] * dt;
+      
+      // Hip Joint Angels
+      angs[i] = 0 + stand_angs[i];
+      angs[i + 1] = x[i] * 180 / M_PI + stand_angs[i + 1];
+
+      // Knee Joint Angel
+      if (y[i] > 0) {
+        angs[i + 2] = stand_angs[i + 2];
+      } else {
+        angs[i + 2] = - y[i] * Ak * 180 / (M_PI * Ah) + stand_angs[i + 2];
+      }
+    }
+  }
+
+//  PTL("signals: ");
+//  PTL(x[0]);
+//  PTL(y[0]);
+//  PTL("Angles: ");
+//  PTL(angs[1]);
+//  PTL(angs[2]);
+
+  servo_write_angs(angs);
+}
diff --git a/hardware_test/LabRobot/DogRobot/QuadrupedRobot.h b/hardware_test/LabRobot/DogRobot/QuadrupedRobot.h
@@ -1,12 +1,12 @@
 #ifndef QUADRUPED_ROBOT_H
 #define QUADRUPED_ROBOT_H
 
-#include"Arduino.h"
+#include "Arduino.h"
 #include <Servo.h>
 
-#define PT(s)  Serial.print(s)
+#define PT(s) Serial.print(s)
 #define PTL(s) Serial.println(s)
-#define PTF(s)  Serial.print(F(s)) //trade flash memory for dynamic memory with F() function
+#define PTF(s) Serial.print(F(s)) //trade flash memory for dynamic memory with F() function
 #define PTLF(s) Serial.println(F(s))
 
 // Servo Pin configs
@@ -26,45 +26,71 @@
 //servo constants
 #define SERVOMIN 500
 #define SERVOMAX 2500
-#define PWM_RANGE (SERVOMAX - SERVOMIN)
 
 #define DOF 12
 
-
 class QuadrupedRobot
 {
-  private:
-    Servo *servos[DOF] = {
+private:
+  Servo *servos[DOF] = {
       nullptr, nullptr, nullptr,
       nullptr, nullptr, nullptr,
       nullptr, nullptr, nullptr,
-      nullptr, nullptr, nullptr
-    };
-    byte pins[DOF] = {     
+      nullptr, nullptr, nullptr};
+  byte pins[DOF]{
       rFL_PIN, sFL_PIN, kFL_PIN,
       rFR_PIN, sFR_PIN, kFR_PIN,
       rHR_PIN, sHR_PIN, kHR_PIN,
-      rHL_PIN, sHL_PIN, kHL_PIN
-    };
+      rHL_PIN, sHL_PIN, kHL_PIN};
+
+  // Servo rotation configs
+  uint8_t init_angs[12]{45, 90, 180, 130, 83, 0, 45, 90, 180, 130, 83, 0};
+  int8_t dir[12]{1, -1, -1, -1, 1, 1, -1, 1, 1, 1, -1, -1};
+
+  /**
+   * CPG Gait Configs
+   */
+  uint8_t alpha{1000};
+  double beta{0.5};
+  double Ah{0.2};
+  double Ak{0.5};
+  uint8_t a{10};
+  // LF, RF, LH, RH
+  double phi[4]{0, 0.5, 0.5, 0};
+  double omega_sw{5*M_PI};
+
+  // oscillator signals
+  double x[4] {1, 0, 0, 0};
+  double y[4] {0, 0, 0, 0};
+  // x_dot = dx / dt
+  double x_dot[4] {};
+  double y_dot[4] {};
+  // r_square = x^2 + y^2
+  double r_square[4] {};
 
-    char* duty_angles = NULL;
+  double omega_st {};
+  double mu {};
+  double omega[4] {};
+  double theta[4][4] {};
+  double dt {0.0001};
 
-    // Servo rotation configs
-    uint8_t init_angs[12] = {45, 90, 180, 130, 83, 0, 45, 90, 180, 130, 83, 0};
-    int8_t dir[12] = {1, -1, -1, -1, 1, 1, -1, 1, 1, 1, -1, -1};
+  double rad2deg(double rad);
+  void setup_servos();
+  void shut_servos();
+  void servo_write_angs(double angs[12], bool angs_in_rad=false);
 
-    void setup_servos();
-    void shut_servos();
-    void servo_write_angs(double angs[12]);
+public:
+  QuadrupedRobot();
+  ~QuadrupedRobot();
 
-  public:
-    QuadrupedRobot();
-    ~QuadrupedRobot();
+  // lock any robot motion
+  bool lock{false};
 
-    void switch_on();
-    void switch_off();
-    void bot_rest();
-    void bot_stand();
+  void switch_on();
+  void switch_off();
+  void bot_rest();
+  void bot_stand();
+  void bot_walk();
 };
 
 #endif

-Original file line number
+Diff line change
@@ @@ -1,2 +1,4 @@ @@
 .DS_Store
 -.idea
 +.idea
++
 +.vscode