実装
mbed 用の実装 と Arduino用のインターフェイスを混ぜたような実装を書いて実験しました。
正確に動かすためには正確に制御方法を理解する方法があるので、結局ある程度自分でコードを試行錯誤して理解する必要がある気がします。
キャリブレーション方法が特に難しいのですが、以下のような方法で行いました。ロジックがあっていることが大前提ですが「動かしてみてあってるか確認する」のが難しいので、コードの検証はしっかりやらないとダメです。
- 電源を切った状態で複数の座標にアームを動かしてみて、そのときの各サーボモータの角度を記録する
- 実際に座標を計算して、サーボモータへ入力すべき角度との差分や係数を求める
#include <Arduino.h>
#include <Servo.h>
#include "math.h"
#include <Servo.h>
#define DEBUG 0
constexpr float DEG2RADIAN(float deg) {
return deg * M_PI / 180;
}
constexpr float RADIAN2DEG(float rad) {
return rad * 180 / M_PI;
}
class MeArm {
private:
Servo base, shoulder, elbow, claw;
float baseServoMin, baseServoMax, baseCoefficient, baseOffset;
float shoulderServoMin, shoulderServoMax, shoulderCoefficient, shoulderOffset;
float elbowServoMin, elbowServoMax, elbowCoefficient, elbowOffset;
float clawServoMin, clawServoMax, clawCoefficient, clawOffset;
float currentX, currentY, currentZ;
public:
static const int16_t LENGTH_SEGMENT0 = 18;
static const int16_t LENGTH_SEGMENT1 = 80;
static const int16_t LENGTH_SEGMENT2 = 80;
static const int16_t LENGTH_SEGMENT3 = 55;
static const int16_t LENGTH_BASE_HEIGHT = 60;
static const int16_t MAX_REACH = 220;
static constexpr float lawOfCosines(float adj1, float adj2, float opp) {
return acos((adj1*adj1 + adj2*adj2 - opp*opp) / (2*adj1*adj2));
}
MeArm() {
setServoCoefficient(
0, 180, 1, 0,
0, 180, -1, 0,
0, 180, -1, 0,
0, 180, 1, 0
);
}
void begin(int pinBase, int pinShoulder, int pinElbow, int pinClaw) {
base.attach(pinBase);
shoulder.attach(pinShoulder);
elbow.attach(pinElbow);
claw.attach(pinClaw);
base.write(90);
shoulder.write(90);
elbow.write(90);
claw.write(90);
}
void setServoCoefficient(
float _baseServoMin, float _baseServoMax, float _baseCoefficient, float _baseOffset,
float _shoulderServoMin, float _shoulderServoMax, float _shoulderCoefficient, float _shoulderOffset,
float _elbowServoMin, float _elbowServoMax, float _elbowCoefficient, float _elbowOffset,
float _clawServoMin, float _clawServoMax, float _clawCoefficient, float _clawOffset
) {
baseServoMin = _baseServoMin;
baseServoMax = _baseServoMax;
baseCoefficient = _baseCoefficient;
baseOffset = _baseOffset;
shoulderServoMin = _shoulderServoMin;
shoulderServoMax = _shoulderServoMax;
shoulderCoefficient = _shoulderCoefficient;
shoulderOffset = _shoulderOffset;
elbowServoMin = _elbowServoMin;
elbowServoMax = _elbowServoMax;
elbowCoefficient = _elbowCoefficient;
elbowOffset = _elbowOffset;
clawServoMin = _clawServoMin;
clawServoMax = _clawServoMax;
clawCoefficient = _clawCoefficient;
clawOffset = _clawOffset;
}
bool solveInverseKinematics(
float x, float y, float z,
float& thetaBase, float& thetaShoulder, float& thetaElbow
) const {
float target = sqrt(x * x + y * y);
if (target > MAX_REACH) return false;
float wrist = target - LENGTH_SEGMENT3;
float shoulder = LENGTH_SEGMENT0;
float s2w = sqrt( (wrist - shoulder) * (wrist - shoulder) + (z - LENGTH_BASE_HEIGHT) * (z - LENGTH_BASE_HEIGHT) );
if (s2w == 0.0) {
return false;
}
if (DEBUG) {
Serial.print("x = "); Serial.print(x); Serial.print(" ");
Serial.print("y = "); Serial.print(y); Serial.print(" ");
Serial.print("z = "); Serial.print(z); Serial.print(" ");
Serial.print("target = "); Serial.print(target); Serial.print(" ");
Serial.print("wrist = "); Serial.print(wrist); Serial.print(" ");
Serial.print("shoulder = "); Serial.print(shoulder); Serial.print(" ");
Serial.print("s2w = "); Serial.print(s2w); Serial.print(" ");
}
float thetaSw = acos( (wrist - shoulder) / s2w );
if (isnan(thetaSw)) {
Serial.println("invalid thetaSw");
return false;
}
if (z < LENGTH_BASE_HEIGHT) {
thetaSw *= -1.0;
}
float thetaElbow0 = lawOfCosines(LENGTH_SEGMENT1, LENGTH_SEGMENT2, s2w);
if (isnan(thetaElbow0)) {
Serial.println("thetaElbow0 is NaN");
return false;
}
float thetaShoulder0 = lawOfCosines(s2w, LENGTH_SEGMENT1, LENGTH_SEGMENT2);
if (isnan(thetaShoulder0)) {
Serial.println("thetaShoulder0 is NaN");
return false;
}
thetaShoulder = thetaSw + thetaShoulder0;
thetaElbow = thetaElbow0;
thetaBase = atan2(y, x);
return true;
}
bool convertIKAnglesToServoAngles(
float thetaBase, float thetaShoulder, float thetaElbow,
float& thetaBaseServo, float& thetaShoulderServo, float& thetaElbowServo
) {
thetaElbow = M_PI - thetaShoulder - thetaElbow;
if (DEBUG) {
Serial.print(" beta="); Serial.print(RADIAN2DEG(thetaElbow)); Serial.print(" ");
}
thetaBaseServo = RADIAN2DEG(thetaBase) * baseCoefficient + baseOffset;
if (thetaBaseServo < baseServoMin) thetaBaseServo = baseServoMin;
if (baseServoMax < thetaBaseServo) thetaBaseServo = baseServoMax;
thetaShoulderServo = RADIAN2DEG(thetaShoulder) * shoulderCoefficient + shoulderOffset;
if (thetaShoulderServo < shoulderServoMin) thetaShoulderServo = shoulderServoMin;
if (shoulderServoMax < thetaShoulderServo) thetaShoulderServo = shoulderServoMax;
thetaElbowServo = RADIAN2DEG(thetaElbow) * elbowCoefficient + elbowOffset;
if (thetaElbowServo < elbowServoMin) thetaElbowServo = elbowServoMin;
if (elbowServoMax < thetaElbowServo) thetaElbowServo = elbowServoMax;
return true;
}
bool goDirectlyTo(float x, float y, float z) {
float thetaBase, thetaShoulder, thetaElbow;
bool ok = solveInverseKinematics(x, y, z, thetaBase, thetaShoulder, thetaElbow);
if (DEBUG) {
Serial.print("goDirectlyTo "); Serial.print(ok); Serial.print(" ");
Serial.print(RADIAN2DEG(thetaBase)); Serial.print(" ");
Serial.print(RADIAN2DEG(thetaShoulder)); Serial.print(" ");
Serial.print(RADIAN2DEG(thetaElbow)); Serial.print(" ");
}
if (ok) {
convertIKAnglesToServoAngles(
thetaBase, thetaShoulder, thetaElbow,
thetaBase, thetaShoulder, thetaElbow
);
if (DEBUG) {
Serial.print("converted: ");
Serial.print(thetaBase); Serial.print(" ");
Serial.print(thetaShoulder); Serial.print(" ");
Serial.print(thetaElbow); Serial.print(" ");
}
base.write(thetaBase);
shoulder.write(thetaShoulder);
elbow.write(thetaElbow);
currentX = x;
currentY = y;
currentZ = z;
if (DEBUG) Serial.println("");
return true;
} else {
if (DEBUG) Serial.println("");
return false;
}
}
bool gotoPoint(float x, float y, float z, float step=5, float wait=20) {
float x0 = currentX;
float y0 = currentY;
float z0 = currentZ;
float distance = sqrt((x0-x)*(x0-x)+(y0-y)*(y0-y)+(z0-z)*(z0-z));
for (float i = 0; i < distance; i+= step) {
bool ok = goDirectlyTo(x0 + (x-x0)*i/distance, y0 + (y-y0) * i/distance, z0 + (z-z0) * i/distance);
if (!ok) return false;
delay(wait);
}
bool ok = goDirectlyTo(x, y, z);
if (!ok) return false;
delay(wait);
return true;
}
void openGripper() {
moveGripper(M_PI/2);
}
void closeGripper() {
moveGripper(0);
}
void moveGripper(float thetaClaw) {
float thetaClawServo = RADIAN2DEG(thetaClaw) * clawCoefficient + clawOffset;
if (thetaClawServo < clawServoMin) thetaClawServo = clawServoMin;
if (clawServoMax < thetaClawServo) thetaClawServo = clawServoMax;
claw.write(thetaClawServo);
}
};
MeArm arm;
void setup() {
Serial.begin(9600);
Serial.println("start");
if (0) {
Serial.println("debug");
Servo middle, left, right, claw ;
middle.attach(2);
left.attach(3);
right.attach(4);
claw.attach(5);
middle.write(90);
left.write(90);
right.write(90);
claw.write(120);
for (;;);
uint16_t prev = 0;
for (;;) {
uint16_t read = static_cast<uint32_t>(analogRead(0)) * 180 / 512;
if (read != prev) {
Serial.print("read=");
Serial.println(read);
left.write(read);
prev = read;
delay(250);
}
}
for (;;) {
Serial.println("0");
left.write(0);
right.write(0);
delay(2000);
Serial.println("90");
left.write(90);
right.write(90);
delay(2000);
Serial.println("180");
left.write(180);
right.write(180);
delay(2000);
}
}
arm.begin(2, 4, 3, 5);
arm.setServoCoefficient(
0, 180, 1, 0,
20, 170, -1, 170,
20, 130, -1, 106,
75, 115, -0.5, 115
);
arm.goDirectlyTo(0, 100, 50);
}
void loop() {
Serial.println("loop");
arm.openGripper();
arm.gotoPoint(0,110,60); arm.openGripper(); delay(250);arm.closeGripper(); delay(250);
arm.gotoPoint(100,110,60); arm.openGripper(); delay(250);arm.closeGripper(); delay(250);
arm.gotoPoint(100,160,60); arm.openGripper(); delay(250);arm.closeGripper(); delay(250);
arm.gotoPoint( 0,160,60); arm.openGripper(); delay(250);arm.closeGripper(); delay(250);
arm.gotoPoint(-100,160,60); arm.openGripper(); delay(250);arm.closeGripper(); delay(250);
arm.gotoPoint(-100,110,60); arm.openGripper(); delay(250);arm.closeGripper(); delay(250);
arm.gotoPoint( 0,110,60); arm.openGripper(); delay(250);arm.closeGripper(); delay(250);
arm.closeGripper();
delay(1000);
arm.openGripper();
arm.gotoPoint(0,110,20); arm.openGripper(); delay(250);arm.closeGripper(); delay(250);
arm.gotoPoint(100,110,20); arm.openGripper(); delay(250);arm.closeGripper(); delay(250);
arm.gotoPoint(100,160,20); arm.openGripper(); delay(250);arm.closeGripper(); delay(250);
arm.gotoPoint( 0,160,20); arm.openGripper(); delay(250);arm.closeGripper(); delay(250);
arm.gotoPoint(-100,160,20); arm.openGripper(); delay(250);arm.closeGripper(); delay(250);
arm.gotoPoint(-100,110,20); arm.openGripper(); delay(250);arm.closeGripper(); delay(250);
arm.gotoPoint( 0,110,20); arm.openGripper(); delay(250);arm.closeGripper(); delay(250);
arm.closeGripper();
delay(1000);
arm.openGripper();
arm.gotoPoint(0,110,110); arm.openGripper(); delay(250);arm.closeGripper(); delay(250);
arm.gotoPoint(100,110,110); arm.openGripper(); delay(250);arm.closeGripper(); delay(250);
arm.gotoPoint(100,160,110); arm.openGripper(); delay(250);arm.closeGripper(); delay(250);
arm.gotoPoint( 0,160,110); arm.openGripper(); delay(250);arm.closeGripper(); delay(250);
arm.gotoPoint(-100,160,110); arm.openGripper(); delay(250);arm.closeGripper(); delay(250);
arm.gotoPoint(-100,110,110); arm.openGripper(); delay(250);arm.closeGripper(); delay(250);
arm.gotoPoint( 0,110,110); arm.openGripper(); delay(250);arm.closeGripper(); delay(250);
arm.closeGripper();
delay(1000);
} 
