Diff: motor_controller.h

Revision:

0:c177452db984

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/motor_controller.h	Mon Nov 02 09:00:16 2020 +0000
@@ -0,0 +1,52 @@
+#include "mbed.h"
+#include "device.h"
+
+int addr[MOTOR_NUM] = {IIC_ADDR1, IIC_ADDR2, IIC_ADDR3, IIC_ADDR4};
+
+int controlMotor(int ch, int frequency)
+{    
+    int dir = COAST;
+    int size = 4;
+    if(ch < 0 || ch > 3)
+    {
+        //channel error
+        return 0;
+    }
+    else
+    {
+        // オドメトリ用に周波数を保存
+        my_odometry.wheel_frequency[ch] = frequency;
+        if(frequency > 0)
+        {
+            dir = CW;
+        }
+        else if(frequency < 0)
+        {
+            dir = CCW;
+            frequency = -frequency;
+        }
+        else
+        {
+            dir = BRAKE;
+        }
+        // 周波数制限 脱調を防ぐ
+        if(frequency > MaxFrequency) frequency = MaxFrequency;
+        //else if(frequency < MinFrequency) frequency = MinFrequency;
+        
+        master.writeSomeData(addr[ch], PWM_FREQUENCY, frequency, size);
+        master.writeSomeData(addr[ch], MOTOR_DIR, dir, size);
+        
+        return frequency;
+    }   
+}
+
+
+void coastAllMotor()
+{
+    for(int i = 0; i < MOTOR_NUM; i++)
+    {
+        // オドメトリ用に周波数を保存
+        my_odometry.wheel_frequency[i] = 0;
+        master.writeSomeData(addr[i], MOTOR_DIR, COAST, 4);
+    }
+}