Diff: encoder.cpp

Revision:

0:2dbbe7d28fab

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/encoder.cpp	Sat Aug 04 05:55:50 2018 +0000
@@ -0,0 +1,56 @@
+#include "mbed.h"
+#include "encoder.h"
+
+DigitalOut encoder_cs(D9);
+//SPI spi_encoder(D11, D12, D13); // mosi, miso, sclk
+SPI spi_encoder(D4, D5, D3); // mosi, miso, sclk
+
+unsigned short encoder_value;
+unsigned short angle = 0;
+unsigned short angle_old;
+int angle_dif;
+int a_dif;
+int Angle = 0;
+unsigned short k = 0;
+
+void init_SPI_encoder()
+{
+    spi_encoder.format(16,3);
+    spi_encoder.frequency(1000000); // 1MHz clock rate
+}
+
+void init_encoder()
+{
+    encoder_cs = 1;  // high：disable the device
+}
+
+void angle_measure()
+{
+    encoder_cs = 0; // Select the device by seting chip select low
+    encoder_value = spi_encoder.write(0x00);
+    angle = encoder_value >> 3;
+    encoder_cs = 1; // Deselect the device
+
+    if (k == 0) {
+        Angle = 0;
+        angle_old = angle;
+        k++;
+    } else {
+        angle_dif = angle_count(angle, angle_old);
+        Angle = Angle + angle_dif;
+        angle_old = angle;
+    }
+}
+
+int angle_count(unsigned short now,unsigned short old)
+{
+    a_dif = now - old;
+    if (a_dif > 4096/2) {
+        a_dif = -(4096 - a_dif);
+    } else if (a_dif < -4096/2) {
+        a_dif = 4096 + a_dif;
+    } else {
+        a_dif = a_dif;
+    }
+    return a_dif;
+}