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

--- /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

--- /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

--- /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

--- /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

--- /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;
+}

--- /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