Diff: SPI_Encoder.cpp

Revision:

1:7826ec4d9405

Parent:

0:21ae825df645

Child:

2:8154b75f836f

--- a/SPI_Encoder.cpp	Mon Jul 22 17:45:28 2019 +0000
+++ b/SPI_Encoder.cpp	Fri Aug 30 03:06:00 2019 +0000
@@ -2,53 +2,71 @@
 #include "SPI_Encoder.h"
 
 
-SPI_Encoder::SPI_Encoder(PinName mosi, PinName miso, PinName sclk, PinName _cs0, PinName _cs1, PinName _cs2, PinName _cs3) : encoder(mosi, miso, sclk), cs0(_cs0), cs1(_cs1), cs2(_cs2), cs3(_cs3)
+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,0);
-    encoder.frequency(8000000);
+    encoder.format(8,1);
+    encoder.frequency(10000000);
+    delta_t = t;
+    for(int i = 0; i < encoder_num; i++)
+        direction[i] = 0;
+    cs0 = 1;
+    cs1 = 1;
+    cs2 = 1;
+    cs3 = 1;
+    wait(0.1); //encoder init
 };
 
+void SPI_Encoder::inverse(int num)
+{
+    direction[num] = 1;
+}
+
 void SPI_Encoder::getPosition(int num)
 {
-    if (EncoderByteData[num] != Encoderposition_last[num]) { //if nothing has changed dont wast time sending position
-        Encoderposition_last[num] = EncoderByteData[num] ; //set last position to current position
-    } else {
-        uint8_t recieved = 0xA5;//just a temp vairable
-        EncoderByteData[num]  = 0;//reset position vairable
-        switching(num, 0);
-        _SPI_T(num, 0x10);//issue read command
-        wait_ms(50);//give time to read. Timmig is critical
- 
-        while (recieved != 0x10) { //loop while encoder is not ready to send
-            recieved = _SPI_T(num, 0x00); //cleck again if encoder is still working
-            wait_ms(1); //again,give time to read. Timmig is critical
-        }
- 
-        temp[0][num] = _SPI_T(num, 0x00); //Recieve MSB
-        temp[1][num] = _SPI_T(num, 0x00); // recieve LSB
-        switching(num, 1);
- 
-        temp[0][num] &=~ 0xF0;//mask out the first 4 bits
-        EncoderByteData[num]  = temp[0][num] << 8; //shift MSB to correct EncoderByteData in EncoderByteData message
-        EncoderByteData[num]  += temp[1][num]; // add LSB to EncoderByteData message to complete message
-        wait_ms(1);//again,give time to read. Timmig is critical
+    float pre_angle = angle[num];
+    float _angle = 0;
+    
+    uint8_t received = _SPI_T(num, 0x10);   //read command
+    while (received != 0x10) {              //loop while encoder is not ready to send
+        received = _SPI_T(num, 0x00);
     }
-    Steps[num] = EncoderByteData[num]/16;
+    temp[0][num] = _SPI_T(num, 0x00);       //Recieve MSB
+    temp[1][num] = _SPI_T(num, 0x00);       //recieve LSB
+    temp[0][num] &=~ 0xF0;                  //mask out the first 4 bits
+    EncoderByteData[num]  = temp[0][num] << 8;
+    EncoderByteData[num]  += temp[1][num];
+    
+    if(!direction[num]) _angle = EncoderByteData[num] / 4096.0f * 2.0f * PI;    //normal
+    else _angle = abs((EncoderByteData[num] / 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;
 }
 
-uint8_t SPI_Encoder::_SPI_T (int num, uint8_t SPITransmit)//Repetive SPI transmit sequence
+bool SPI_Encoder::setZero(int num)
 {
-    uint8_t SPI_temp = 0;  //vairable to hold recieved data
-    switching(num, 0);     //select spi device
-    SPI_temp = encoder.write(SPITransmit);//send and recieve
-    switching(num, 1);//deselect spi device
-    return(SPI_temp);      //return recieved byte
+    uint8_t received = _SPI_T(num, 0x70);   //read command
+    while (received != 0x80) {              //loop while encoder is not ready to send
+        received = _SPI_T(num, 0x00);
+    }
+    return true;
 }
 
+/* switch MSB or LSB */
+uint8_t SPI_Encoder::_SPI_T (int num, uint8_t SPITransmit)
+{
+    _switching(num, 0);
+    uint8_t SPI_temp = encoder.write(SPITransmit);
+    _switching(num, 1);
+    wait_us(10);            //Timmig is critical
+    return (SPI_temp);
+}
+
+/* switch read encoder */
 void SPI_Encoder::_switching(int num, int value)
 {
-    switch(num)
-    {
+    switch (num) {
         case 0:
             cs0 = value;
             break;