Diff: SPI_Encoder.cpp

Revision:

4:2331b7fcfe8d

Parent:

3:d3694519bd73

Child:

5:1458a8f024dc

--- a/SPI_Encoder.cpp	Thu Sep 19 13:00:42 2019 +0000
+++ b/SPI_Encoder.cpp	Thu Sep 26 13:49:51 2019 +0000
@@ -5,7 +5,7 @@
 SPI_Encoder::SPI_Encoder(PinName mosi, PinName miso, PinName sclk, PinName _cs0, PinName _cs1, PinName _cs2, PinName _cs3, float t) : encoder(mosi, miso, sclk), cs0(_cs0), cs1(_cs1), cs2(_cs2), cs3(_cs3)
 {
     encoder.format(8,1);
-    encoder.frequency(2000000);
+    encoder.frequency(1000000);
     delta_t = t;
     for(int i = 0; i < encoder_num; i++)
         direction[i] = 0;
@@ -21,29 +21,46 @@
     direction[num] = 1;
 }
 
-void SPI_Encoder::getPosition(int num)
+bool SPI_Encoder::getPosition(int num)
 {
     uint8_t temp[2];
     uint16_t EncoderByteData;
     float pre_angle = angle[num];
     float _angle = 0;
+    int stack = 0;
+    bool e = false;
     
     uint8_t received = _SPI_T(num, 0x10);   //read command
     while (received != 0x10) {              //loop while encoder is not ready to send
+        if(stack > 100) {
+            e = true;
+            break;
+        }
         received = _SPI_T(num, 0x00);
+        stack++;
     }
-    temp[0] = _SPI_T(num, 0x00);       //Recieve MSB
-    temp[1] = _SPI_T(num, 0x00);       //recieve LSB
-    temp[0] &=~ 0xF0;                  //mask out the first 4 bits
-    EncoderByteData  = temp[0] << 8;
-    EncoderByteData  += temp[1];
-    
-    if(!direction[num]) _angle = EncoderByteData / 4096.0f * 2.0f * PI;    //normal
-    else _angle = abs((EncoderByteData / 4096.0f * 2.0f * PI) - 2*PI);     //inverse
-    if(_angle > PI) angle[num] = _angle - 2*PI;                                 //0~2π → -π~π
-    else angle[num] = _angle;
-    
-    velocity[num] = (angle[num] - pre_angle) / delta_t;
+    if(!e) {
+        temp[0] = _SPI_T(num, 0x00);       //Recieve MSB
+        temp[1] = _SPI_T(num, 0x00);       //recieve LSB
+        temp[0] &=~ 0xF0;                  //mask out the first 4 bits
+        EncoderByteData  = temp[0] << 8;
+        EncoderByteData  += temp[1];
+        
+        if(!direction[num]) _angle = EncoderByteData / 4096.0f * 2.0f * PI;    //normal
+        else _angle = abs((EncoderByteData / 4096.0f * 2.0f * PI) - 2.0f*PI);  //inverse
+        if(_angle > PI) angle[num] = _angle - 2*PI;                            //0~2π → -π~π
+        else angle[num] = _angle;
+        
+        velocity[num] = (angle[num] - pre_angle) / delta_t;
+        
+        return 0;
+    }
+    else {
+        angle[num] = pre_angle;
+        velocity[num] = 0;
+        
+        return 1;
+    }
 }
 
 bool SPI_Encoder::setZero(int num)