Kazushi Yamanobe
latest version 9/26
Revision 0:0d02a451e79d, committed 2021-10-10
- Comitter:
- Yamanobe
- Date:
- Sun Oct 10 04:47:14 2021 +0000
- Commit message:
- slave program (at 9/26)
Changed in this revision
diff -r 000000000000 -r 0d02a451e79d EC12E2420801.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/EC12E2420801.cpp Sun Oct 10 04:47:14 2021 +0000
@@ -0,0 +1,28 @@
+#include "EC12E2420801.h"
+
+Encoder::Encoder(PinName Apulse,PinName Bpulse) : Apulse(Apulse),Bpulse(Bpulse) {
+ Encoder::Apulse.fall(this,&Encoder::Apulse_Down);
+}
+
+void Encoder::Apulse_Down() {
+ if(Bpulse) {
+ count--;
+ } else {
+ count++;
+ }
+}
+
+bool Encoder::reset(int preset) {
+ count = 0;
+ return true;
+}
+
+/****値返す用の関数****/
+float Encoder::getData(short ch) {
+ switch(ch) {
+ case COUNT:
+ return count; //カウント
+ default:
+ return 0;
+ }
+}
\ No newline at end of file
diff -r 000000000000 -r 0d02a451e79d EC12E2420801.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/EC12E2420801.h Sun Oct 10 04:47:14 2021 +0000
@@ -0,0 +1,33 @@
+#ifndef ENCODER_H
+#define ENCODER_H
+
+#include "mbed.h"
+
+/****定数****/
+#define PI 3.1415f
+#define RADIAN 360
+#define MINUTE 60
+#define CALCULATE_PERIOD 0.01f
+
+enum DATA_CATEGORY{
+ COUNT,
+};
+
+
+class Encoder {
+ public:
+ Encoder(PinName Apulse,PinName Bpulse);
+ float getData(short ch);
+ bool reset(int preset = 0);
+
+ private:
+ InterruptIn Apulse;
+ InterruptIn Bpulse;
+
+ void Apulse_Down();
+
+ int preset;
+ float count;
+};
+
+#endif
\ No newline at end of file
diff -r 000000000000 -r 0d02a451e79d main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Sun Oct 10 04:47:14 2021 +0000
@@ -0,0 +1,98 @@
+#include "mbed.h"
+#include "EC12E2420801.h"
+#include "pid.h"
+
+#define SLAVE_ID 0x03
+
+#define I2C_SDA D4
+#define I2C_SCL D5
+#define MOTOR_PWM D9
+#define MOTOR_DIR D10
+#define MOTOR_EN D12
+#define SEN_HALL A0
+#define SEN_ENC_A A2
+#define SEN_ENC_B A4
+
+Serial pc(USBTX, USBRX, 115200);
+I2CSlave slave(I2C_SDA, I2C_SCL);
+PwmOut mot_pwm(MOTOR_PWM);
+DigitalOut mot_dir(MOTOR_DIR);
+DigitalOut mot_en(MOTOR_EN);
+AnalogIn hall(SEN_HALL);
+Encoder encoder(SEN_ENC_A, SEN_ENC_B);
+
+PositionPid _pid;
+Ticker tracer;
+Ticker debugger;
+
+void trace();
+void setup();
+void debug();
+
+int position = 0;
+
+namespace para {
+ const float PID_KP = 0.5f;
+ const float PID_KI = 0.0f;
+ const float PID_KD = 0.00001f;
+ const float PID_DT = 0.002f;
+}
+
+inline int getDIR(float duty){
+ return ((duty >= 0)? 1 : 0);
+}
+
+void trace() {
+ _pid.calculate(position, int(encoder.getData(COUNT)));
+ mot_dir = getDIR(_pid.duty());
+ mot_pwm = abs(_pid.duty());
+}
+void setup() {
+ slave.address(SLAVE_ID);
+ debugger.attach(&debug, 0.1);
+ _pid.setup(para::PID_KP, para::PID_KI, para::PID_KD, para::PID_DT);
+ mot_en = 0; // enable motor drive
+}
+void debug() {
+ pc.printf("%4.3f, ", hall.read());
+ pc.printf("%d, ", position);
+ pc.printf("%d\r\n", int(encoder.getData(COUNT)));
+}
+
+int main() {
+ setup();
+
+ while(1){
+ if(int(10 * hall.read()) != 0){
+ mot_dir = 1;
+ mot_pwm = 0.5;
+ wait_ms(1.0);
+ }else{
+ mot_pwm = 0.0;
+ encoder.reset();
+ break;
+ }
+ }
+
+ wait(2.0);
+ pc.printf("start\r\n");
+
+ tracer.attach(&trace, para::PID_DT);
+
+ while(1) {
+ char buf[10];
+
+ switch (slave.receive()) {
+ case I2CSlave::NoData:
+ break;
+ case I2CSlave::ReadAddressed:
+ break;
+ case I2CSlave::WriteGeneral:
+ break;
+ case I2CSlave::WriteAddressed:
+ slave.read(buf, 10);
+ position = int(buf[2]);
+ break;
+ }
+ }
+}
\ No newline at end of file
diff -r 000000000000 -r 0d02a451e79d mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Sun Oct 10 04:47:14 2021 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400 \ No newline at end of file
diff -r 000000000000 -r 0d02a451e79d pid.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/pid.cpp Sun Oct 10 04:47:14 2021 +0000
@@ -0,0 +1,54 @@
+#include "pid.h"
+
+void PositionPid::setup(float Kp, float Ki, float Kd, float dt){
+ kp = Kp;
+ ki = Ki;
+ kd = Kd;
+ time = dt;
+ frequency = 1/dt;
+}
+
+void PositionPid::calculate(float target, float nowValue){
+ old = now;
+ now = nowValue - target;
+ p = now;
+ i = i + (now + old) * 0.5f * time;
+ d = (now - old) * frequency;
+ result = kp*p + ki*i + kd*d;
+ if (result > 1.0f) {
+ result = 1.0f;
+ } else if (result < -1.0f) {
+ result = -1.0f;
+ }
+}
+
+float PositionPid::duty(){
+ return result;
+}
+
+void SpeedPid::setup(float Kp, float Ki, float Kd, float dt){
+ kp = Kp;
+ ki = Ki;
+ kd = Kd;
+ time = dt;
+ frequency = 1/dt;
+}
+
+void SpeedPid::calculate(float target, float nowValue){
+ e2 = e1;
+ e1 = e;
+ e = nowValue - target;
+ p = e - e1;
+ i = e*time;
+ d = (e-2*e1+e2)*frequency;
+ result = result + (kp*p+ki*i+kd*d);
+ if (result > 1.0f) {
+ result = 1.0f;
+ } else if (result < -1.0f) {
+ result = -1.0f;
+ }
+}
+
+float SpeedPid::duty(){
+ return result;
+}
diff -r 000000000000 -r 0d02a451e79d pid.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/pid.h Sun Oct 10 04:47:14 2021 +0000
@@ -0,0 +1,60 @@
+/*********************************************
+PID library
+
+PositionPid : 位置型
+SpeedPid : 速度型
+
+setup(float Kp, float Ki, float Kd, float dt)
+パラメータ、制御周期の設定
+Kp : Pゲイン
+Ki : Iゲイン
+Kd : Dゲイン
+dt : 制御周期[s]
+
+calculate(float target, float nowValue)
+PIDの計算をする
+target : 目標値
+nowValue : 現在値
+
+duty()
+計算結果を-1~1で返す
+*********************************************/
+
+#ifndef MBED_PID_H
+#define MBED_PID_H
+
+#include "mbed.h"
+
+class PositionPid
+{
+public :
+ void setup(float Kp, float Ki, float Kd, float dt);
+
+ void calculate(float target, float nowValue);
+
+ float duty();
+
+private :
+ float kp, ki, kd,
+ time, frequency,
+ old, now,
+ p, i, d, result;
+};
+
+class SpeedPid
+{
+public :
+ void setup(float Kp, float Ki, float Kd, float dt);
+
+ void calculate(float target, float nowValue);
+
+ float duty();
+
+private :
+ float kp, ki, kd,
+ time, frequency,
+ e, e1, e2,
+ p, i, d, result;
+};
+
+#endif
\ No newline at end of file