Diff: encoder.cpp

Revision:

1:2f2a74337b77

Parent:

0:f2657b94ea70

Child:

2:33062fc279ca

diff -r f2657b94ea70 -r 2f2a74337b77 encoder.cpp
--- a/encoder.cpp	Tue Jul 31 12:18:02 2018 +0000
+++ b/encoder.cpp	Tue Jul 31 13:24:56 2018 +0000
@@ -1,2 +1,63 @@
+#include "mbed.h"
 #include "encoder.h"
 
+DigitalOut encoder_cs(D9);
+SPI spi_encoder(D11, D12, D13); // mosi, miso, sclk
+
+unsigned short encoder_value = 0;
+unsigned short angle = 0;
+unsigned short angle_old;
+int angle_dif;
+int a_dif;
+int Angle;
+unsigned short k = 0;
+unsigned short angle_init;
+
+void init_encoder()
+{   
+    encoder_cs = 0; // Select the device by seting chip select low
+    encoder_value = spi_encoder.write(0x00);
+    angle_init = encoder_value >> 3;
+    encoder_cs = 1; // Deselect the device    
+}
+
+void init_SPI_encoder()
+{
+    spi_encoder.format(16,3);
+    spi_encoder.frequency(1000000); // 1MHz clock rate
+}
+
+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_dif = angle_count(angle, angle_init);
+        Angle = angle_init + angle_dif;
+        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