Diff: encoder.cpp

Revision:

0:762ec6dc5696

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/encoder.cpp	Wed Aug 29 07:30:11 2018 +0000
@@ -0,0 +1,66 @@
+#include "mbed.h"
+#include "encoder.h"
+
+DigitalOut encoder_cs(D9);
+SPI spi_encoder(D11, D12, D13); // mosi, miso, sclk
+
+unsigned short encoder_value;
+unsigned short encoder_value_1;
+unsigned short encoder_value_2;
+unsigned short angle;
+unsigned short angle_old;
+int angle_dif;
+int a_dif;
+int Angle = 0;
+unsigned short k = 0;
+
+void init_SPI_encoder()
+{
+    spi_encoder.format(8,3);
+    spi_encoder.frequency(1200000); // 1.125MHz 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_1 = spi_encoder.write(0x00);
+    encoder_value_2 = spi_encoder.write(0x00);
+    encoder_value = (encoder_value_1 << 8) | encoder_value_2;
+    angle = encoder_value >> 3;
+    encoder_cs = 1; // Deselect the device
+//    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;
+}    
\ No newline at end of file