Diff: main.cpp

Revision:

0:0d02a451e79d

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