Diff: AS5045/AS5045.cpp

Revision:

5:97117a837d2c

Parent:

4:f6e22dd39313

Child:

12:406f75196a12

--- a/AS5045/AS5045.cpp	Sat Jul 18 14:59:04 2020 +0000
+++ b/AS5045/AS5045.cpp	Tue Jul 21 12:18:14 2020 +0000
@@ -7,20 +7,20 @@
  *	@note
  *		PinName CS is the digital output pin number
  */
- 
- 
- Timer timer_enc;
+
+
+Timer timer_enc;
 AS5045::AS5045(PinName CS) :
-  _spi(NC,D12, D13),	// MBED SPI init
-  _cs(CS)				// Digital output pin init
+    _spi(NC,D12, D13),	// MBED SPI init
+    _cs(CS)				// Digital output pin init
 {
-	// Set SPI bitwidth
-	_spi.format(9, 2);
+    // Set SPI bitwidth9
+    _spi.format(9, 2);
 
-	// Set SPI frequency
-	_spi.frequency(50000);//SPI_FREQ);
-	// Set the digital output high
-	_cs = 1;
+    // Set SPI frequency
+    _spi.frequency(500000);//SPI_FREQ);
+    // Set the digital output high
+    _cs = 1;
 }
 
 /** Read tick amount from encoder (position)
@@ -30,29 +30,42 @@
  */
 int AS5045::getPosition()
 {
-	unsigned int upper,		// Upper part of the tick amount integer
-				lower;		// Lower part of the tick amount integer
-
-	// Set the chip select pin low
-	_cs = 0;
-	timer_enc.reset();
-        timer_enc.start();
-	wait_ms(5);
-	// Read data from the encoder
- 	upper = (_spi.write(0x00)) ;
- 	lower = (_spi.write(0x00));
- 	
-// 	lower = lower >> 6;   
-//    upper = (upper >> 6)+lower;
-//   upper = upper & 0xffc0;
-   upper = upper >> 6;
-
-	// Set the chip select pin high
- 	_cs = 1;
-wait_ms(5);
- 	// Return full 9-bits tick amount
-	return upper; 
-	//return upper ;
+    unsigned int upper,		// Upper part of the tick amount integer
+             lower;		// Lower part of the tick amount integer
+//
+//    // Set the chip select pin low
+//    //timer_enc.reset();
+//    timer_enc.start();
+//    _cs = 0;
+//    
+//    wait_ms(1);
+//    // Read data from the encoder
+//    upper = (_spi.write(0x00)) ;
+//// 	lower = (_spi.write(0x00));
+//
+//// 	lower = lower >> 6;
+////    upper = (upper >> 6)+lower;
+////   upper = upper & 0xffc0;
+////    upper = upper >> 6;
+//
+//    // Set the chip select pin high
+//    _cs = 1;
+//    //wait_ms(5);
+    // Return full 9-bits tick amount
+   // return (upper>>5);
+    //return upper ;
+    
+    _cs = 0;
+   upper = _spi.write(0x00);
+   lower = _spi.write(0x00);
+  _cs = 1;
+ 
+ 
+ 	//upper &=~ 0xF0;//mask out the first 4 bits
+//     EncoderByteData  = upper << 8; //shift MSB to correct EncoderByteData in EncoderByteData message
+//     EncoderByteData  += lower; // add LSB to EncoderByteData message to complete message
+  return ((upper << 3)+(lower >> 6));
+  //return EncoderByteData; 
 }
 
 /** Convert position of the encoder to degrees
@@ -62,9 +75,9 @@
  */
 float AS5045::getRotation()
 {
-	// Get data from the encoder
- 	float value = (float)getPosition();
+    // Get data from the encoder
+    float value = (float)getPosition();
 
- 	// Return degrees of rotation of the encoder
- 	return value * RESOLUTION;
+    // Return degrees of rotation of the encoder
+    return value * RESOLUTION;
 }