pca9685 test on Lab Robot

motor shake, maybe the battery cap is not enough

Author: col-in-coding

hardware_test/LabRobot/StableGait/test/Actuator.cpp

@@ -0,0 +1,90 @@
+#include <Arduino.h>
+#include "Actuator.h"
+#include "Adafruit_PWMServoDriver.h"
+
+
+/**
+ * 
+ *         ^ Y
+ *         |
+ * back    |---> X   forward
+ *       Z
+ * 
+ */
+Actuator::Actuator()
+{
+  // init servo driver, default address 0x40
+  pwm = new Adafruit_PWMServoDriver();
+
+}
+
+
+void Actuator::init()
+{
+  Serial.println("init pwm driver");
+  pwm->begin();
+  pwm->setPWMFreq(freq);
+  Serial.println("init legs");
+  legFL = new Leg();
+  legFR = new Leg();
+  legRR = new Leg();
+  legRL = new Leg();
+  LegPtr legs[4] {legFL, legFR, legRR, legRL};
+  // 1 the same with axis, -1 the opposite
+  const int8_t servo_dir[12]{1, -1, -1, 1, 1, 1, -1, 1, 1, -1, -1, -1};
+  const float init_ang[12]{35, 90, 180, 125, 83, 5, 40, 85, 180, 127, 83, 0};
+  for (uint8_t i = 0; i < 4; i++)
+  {
+    legs[i]->sRoll = new Servo(i * 3, servo_dir[i * 3], init_ang[i * 3]);
+    legs[i]->sPitch = new Servo(i * 3 + 1, servo_dir[i * 3 + 1], init_ang[i * 3 + 1]);
+    legs[i]->sKnee = new Servo(i * 3 + 2, servo_dir[i * 3 + 2], init_ang[i * 3 + 2]);
+  }
+}
+
+
+void Actuator::set_servo_pulse(ServoPtr servo, double pulse)
+{
+  double pulseleng;
+  pulseleng = 1000000; // 1,000,000 per second
+  pulseleng /= freq;
+  pulseleng /= resolution;
+  pulse *= 1000; // convert to us
+  pulse /= pulseleng;
+  pwm->setPWM(servo->id, 0, pulse);
+}
+
+
+void Actuator::write_servo_ang(ServoPtr servo , float angle)
+{
+  double pulse;
+  pulse = 0.5 + angle / 90.0;
+  set_servo_pulse(servo, pulse);
+  servo->angle = angle;
+}
+
+
+void Actuator::adjust() {
+  // adjust front left leg
+  write_servo_ang(legFL->sRoll, legFL->sRoll->init_ang);
+  write_servo_ang(legFL->sPitch, legFL->sPitch->init_ang);
+  write_servo_ang(legFL->sKnee, legFL->sKnee->init_ang);
+
+  write_servo_ang(legFR->sRoll, legFR->sRoll->init_ang);
+  write_servo_ang(legFR->sPitch, legFR->sPitch->init_ang);
+  write_servo_ang(legFR->sKnee, legFR->sKnee->init_ang);
+
+  write_servo_ang(legRR->sRoll, legRR->sRoll->init_ang);
+  write_servo_ang(legRR->sPitch, legRR->sPitch->init_ang);
+  write_servo_ang(legRR->sKnee, legRR->sKnee->init_ang);
+  
+  write_servo_ang(legRL->sRoll, legRL->sRoll->init_ang);
+  write_servo_ang(legRL->sPitch, legRL->sPitch->init_ang);
+  write_servo_ang(legRL->sKnee, legRL->sKnee->init_ang);
+}
+
+
+Actuator::~Actuator()
+{
+  delete pwm;
+  pwm = nullptr;
+}

hardware_test/LabRobot/StableGait/test/Actuator.h

@@ -0,0 +1,57 @@
+#ifndef ACTUATOR_H
+#define ACTUATOR_H
+
+#include <Arduino.h>
+#include "Actuator.h"
+#include "Adafruit_PWMServoDriver.h"
+
+
+struct Servo
+{
+  uint8_t id;
+  int8_t dir;
+  float init_ang; // in degrees
+  float angle; // current angle in degrees
+  Servo(uint8_t _id, int8_t _dir, float _init_ang) {
+    id = _id;
+    dir = _dir;
+    init_ang = _init_ang;
+    angle = _init_ang;
+  }
+};
+typedef Servo* ServoPtr;
+
+
+struct Leg
+{
+  ServoPtr sRoll;
+  ServoPtr sPitch;
+  ServoPtr sKnee;
+};
+typedef Leg* LegPtr;
+
+
+class Actuator
+{
+private:
+  const uint8_t dof{12};
+  const uint8_t freq{50}; // Hz
+  const uint16_t resolution{4096}; // 12 bits
+  Adafruit_PWMServoDriver *pwm;
+  LegPtr legFL;
+  LegPtr legFR;
+  LegPtr legRR;
+  LegPtr legRL;
+
+  void set_servo_pulse(ServoPtr servo, double pulse);
+  void write_servo_ang(ServoPtr servo, float ang);
+
+public:
+  Actuator(/* args */);
+  ~Actuator();
+  void init();
+  void adjust();
+
+};
+
+#endif

hardware_test/LabRobot/StableGait/test/test.ino

@@ -0,0 +1,42 @@
+#include "Actuator.h"
+
+Actuator Act;
+
+void setup() {
+  // put your setup code here, to run once:
+  Serial.begin(9600);
+  Serial.setTimeout(10);
+  while (!Serial);
+  // wait for ready
+  while (Serial.available() && Serial.read()); // empty buffer
+  delay(1000);
+
+  Act.init();
+
+  Serial.print(
+    "Please select from menu: \n"
+    "  1. adjust\n"
+    "  2. stand\n"
+  );
+}
+
+void loop() {
+  // put your main code here, to run repeatedly:
+  String serial_in_str {""};
+  while(Serial.available() > 0) {
+    char inChar = Serial.read();
+    serial_in_str += (char)inChar;
+    // wait for input buffer read
+    delay(10);
+  }
+  if(serial_in_str != "")
+  {
+    Serial.print("Command received: ");
+    Serial.println(serial_in_str);
+    int command_code = serial_in_str.toInt();
+
+    Act.adjust();
+        
+    
+  }
+}