https://curiores.com/positioncontrol
上記のサイトのpart4を実現したいのですが,うまくできません.
現状,part1は試すことができて,エンコーダの動作は確認できています.
また,モータも電源を繋ぐと動作することを確認しています.
マイコンは,Arduino unoで,モータシールドはArduinoMotorShieldです.
使用しているモータは,下記のサイトのものです.
https://www.pololu.com/product/4756
配線の状態としては以下の通りです.
マイコンとモータシールドを物理的に接続して,USBからプログラムの書き込みを行っています.
モータの配線は,赤と黒の線をモータシールドのAにそれぞれ+と-を繋いでいます.黄色と白の線はモータシールドのDIGITALの2,3ピンに配線しています.青と緑は,それぞれモータシールドのGND,5Vです.
モータは12Vで駆動するため,直流安定化電源を使って,モータシールドのAの隣のGNDとVinに接続しています.
初学者ですので,配線が違う可能性もあります..
現在の動作は,実行してもなにも起こらないです.
シリアルモニタで値はプロットされます.
モータについている緑色のGNDを抜くとなぜかモータは動くのですが,よくわからない挙動をします.
プログラムは以下の通りです.
#include <util/atomic.h> // For the ATOMIC_BLOCK macro
#define ENCA 2 // YELLOW
#define ENCB 3 // WHITE
#define PWM 9 // Speed control for Motor A
#define IN1 12 // Direction control 1 for Motor A
#define IN2 13 // Direction control 2 for Motor A
volatile int posi = 0;
long prevT = 0;
float eprev = 0;
float eintegral = 0;
void setup() {
Serial.begin(9600);
pinMode(ENCA, INPUT);
pinMode(ENCB, INPUT);
attachInterrupt(digitalPinToInterrupt(ENCA), readEncoder, RISING);
pinMode(PWM, OUTPUT);
pinMode(IN1, OUTPUT);
pinMode(IN2, OUTPUT);
Serial.println("target pos");
}
void loop() {
int target = 250 * sin(prevT / 1e6);
//int target = 120;
int up =300;
int down =-300;
float kp = 1;
float kd = 0.0;
float ki = 0.0;
long currT = micros();
float deltaT = (float) (currT - prevT) / 1.0e6;
prevT = currT;
int pos;
ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
pos = posi;
}
int e = pos - target;
float dedt = (e - eprev) / deltaT;
eintegral += e * deltaT;
float u = kp * e + kd * dedt + ki * eintegral;
float pwr = fabs(u);
if (pwr > 255) pwr = 255;
int dir = (u < 0) ? -1 : 1;
setMotor(dir, pwr, PWM, IN1, IN2);
eprev = e;
Serial.print(target);
Serial.print(",");
Serial.println(pos);
Serial.print(",");
Serial.println(up);
Serial.print(",");
Serial.println(down);
}
void setMotor(int dir, float pwmVal, int pwm, int in1, int in2) {
analogWrite(pwm, (int)pwmVal);
if (dir == 1) {
digitalWrite(in1, HIGH);
digitalWrite(in2, LOW);
} else {
digitalWrite(in1, LOW);
digitalWrite(in2, HIGH);
}
}
void readEncoder() {
int b = digitalRead(ENCB);
if (b > 0) {
posi++;
} else {
posi--;
}
}
あなたの回答
tips
プレビュー