Diff: PMW3901/PMW3901.cpp

Revision:

6:c7bc001826ba

Parent:

5:1ef8b91a0318

--- a/PMW3901/PMW3901.cpp	Fri May 04 23:03:00 2018 +0000
+++ b/PMW3901/PMW3901.cpp	Tue May 08 13:35:42 2018 +0000
@@ -6,7 +6,26 @@
 }
     
 /** Initialize optical flow */
-int PMW3901::init()
+bool PMW3901::init()
+{
+    setup_spi();
+    if (test_spi())
+    {
+        setup_flow();
+        return true;
+    }
+    else
+    {
+        return false;
+    }
+}
+
+void PMW3901::read()
+{
+    read_flow();
+}
+
+void PMW3901::setup_spi()
 {
     // Deselect the device by seting chip select high
     cs = 1;
@@ -15,27 +34,31 @@
     spi.format(8,3);
     // Setup the spi frequency to 2MHz
     spi.frequency(2000000);
- 
+}
+
+bool PMW3901::test_spi()
+{
+    // 
+    char id;
+     
     // Select the device by seting chip select low
     cs = 0;
- 
-    // Send 0x00, the command to read the WHOAMI register
-    spi.write(0x00);
- 
-    // Send a dummy byte to receive the contents of the WHOAMI register
-    int whoami = spi.write(0x00);
- 
+    // Write the register to read sensor ID (set MSB to 0 for read)
+    spi.write(PRODUCT_ID & 0b01111111); 
+    // Write a dummy byte 0x00 to receive the chip ID
+    id = spi.write(0x00);
     // Deselect the device by seting chip select high
-    cs = 1;
-    
-    //setup_flow();
+    cs = 1; 
     
-    return whoami;
-}
-
-void PMW3901::read()
-{
-    read_flow();
+    //
+    if (id == 0x49)
+    {
+        return true;
+    }
+    else
+    {
+        return false;
+    }   
 }
 
 void PMW3901::setup_flow()
@@ -44,93 +67,93 @@
     cs = 0;
     
     //
-    spi.write(0x3A); spi.write(0x5A);
+    spi.write(0x3A | 0b10000000); spi.write(0x5A);
     
     // 
-    spi.write(0x02); spi.write(0x00);
+    spi.write(0x02 & 0b01111111); spi.write(0x00);
     // 
-    spi.write(0x03); spi.write(0x00);
-    spi.write(0x04); spi.write(0x00);
-    spi.write(0x05); spi.write(0x00);
-    spi.write(0x06); spi.write(0x00);
+    spi.write(0x03 & 0b01111111); spi.write(0x00);
+    spi.write(0x04 & 0b01111111); spi.write(0x00);
+    spi.write(0x05 & 0b01111111); spi.write(0x00);
+    spi.write(0x06 & 0b01111111); spi.write(0x00);
     
     //
-    spi.write(0x7F); spi.write(0x00);
-    spi.write(0x61); spi.write(0xAD);
-    spi.write(0x7F); spi.write(0x03);
-    spi.write(0x40); spi.write(0x00);
-    spi.write(0x7F); spi.write(0x05);
-    spi.write(0x41); spi.write(0xB3);
-    spi.write(0x43); spi.write(0xF1);
-    spi.write(0x45); spi.write(0x14);
-    spi.write(0x5B); spi.write(0x32);
-    spi.write(0x5F); spi.write(0x34);
-    spi.write(0x7B); spi.write(0x08);
-    spi.write(0x7F); spi.write(0x06);
-    spi.write(0x44); spi.write(0x1B);
-    spi.write(0x40); spi.write(0xBF);
-    spi.write(0x4E); spi.write(0x3F);
-    spi.write(0x7F); spi.write(0x08);
-    spi.write(0x65); spi.write(0x20);
-    spi.write(0x6A); spi.write(0x18);
-    spi.write(0x7F); spi.write(0x09);
-    spi.write(0x4F); spi.write(0xAF);
-    spi.write(0x5F); spi.write(0x40);
-    spi.write(0x48); spi.write(0x80);
-    spi.write(0x49); spi.write(0x80);
-    spi.write(0x57); spi.write(0x77);
-    spi.write(0x60); spi.write(0x78);
-    spi.write(0x61); spi.write(0x78);
-    spi.write(0x62); spi.write(0x08);
-    spi.write(0x63); spi.write(0x50);
-    spi.write(0x7F); spi.write(0x0A);
-    spi.write(0x45); spi.write(0x60);
-    spi.write(0x7F); spi.write(0x00);
-    spi.write(0x4D); spi.write(0x11);
-    spi.write(0x55); spi.write(0x80);
-    spi.write(0x74); spi.write(0x1F);
-    spi.write(0x75); spi.write(0x1F);
-    spi.write(0x4A); spi.write(0x78);
-    spi.write(0x4B); spi.write(0x78);
-    spi.write(0x44); spi.write(0x08);
-    spi.write(0x45); spi.write(0x50);
-    spi.write(0x64); spi.write(0xFF);
-    spi.write(0x65); spi.write(0x1F);
-    spi.write(0x7F); spi.write(0x14);
-    spi.write(0x65); spi.write(0x60);
-    spi.write(0x66); spi.write(0x08);
-    spi.write(0x63); spi.write(0x78);
-    spi.write(0x7F); spi.write(0x15);
-    spi.write(0x48); spi.write(0x58);
-    spi.write(0x7F); spi.write(0x07);
-    spi.write(0x41); spi.write(0x0D);
-    spi.write(0x43); spi.write(0x14);
-    spi.write(0x4B); spi.write(0x0E);
-    spi.write(0x45); spi.write(0x0F);
-    spi.write(0x44); spi.write(0x42);
-    spi.write(0x4C); spi.write(0x80);
-    spi.write(0x7F); spi.write(0x10);
-    spi.write(0x5B); spi.write(0x02);
-    spi.write(0x7F); spi.write(0x07);
-    spi.write(0x40); spi.write(0x41);
-    spi.write(0x70); spi.write(0x00);
+    spi.write(0x7F | 0b10000000); spi.write(0x00);
+    spi.write(0x61 | 0b10000000); spi.write(0xAD);
+    spi.write(0x7F | 0b10000000); spi.write(0x03);
+    spi.write(0x40 | 0b10000000); spi.write(0x00);
+    spi.write(0x7F | 0b10000000); spi.write(0x05);
+    spi.write(0x41 | 0b10000000); spi.write(0xB3);
+    spi.write(0x43 | 0b10000000); spi.write(0xF1);
+    spi.write(0x45 | 0b10000000); spi.write(0x14);
+    spi.write(0x5B | 0b10000000); spi.write(0x32);
+    spi.write(0x5F | 0b10000000); spi.write(0x34);
+    spi.write(0x7B | 0b10000000); spi.write(0x08);
+    spi.write(0x7F | 0b10000000); spi.write(0x06);
+    spi.write(0x44 | 0b10000000); spi.write(0x1B);
+    spi.write(0x40 | 0b10000000); spi.write(0xBF);
+    spi.write(0x4E | 0b10000000); spi.write(0x3F);
+    spi.write(0x7F | 0b10000000); spi.write(0x08);
+    spi.write(0x65 | 0b10000000); spi.write(0x20);
+    spi.write(0x6A | 0b10000000); spi.write(0x18);
+    spi.write(0x7F | 0b10000000); spi.write(0x09);
+    spi.write(0x4F | 0b10000000); spi.write(0xAF);
+    spi.write(0x5F | 0b10000000); spi.write(0x40);
+    spi.write(0x48 | 0b10000000); spi.write(0x80);
+    spi.write(0x49 | 0b10000000); spi.write(0x80);
+    spi.write(0x57 | 0b10000000); spi.write(0x77);
+    spi.write(0x60 | 0b10000000); spi.write(0x78);
+    spi.write(0x61 | 0b10000000); spi.write(0x78);
+    spi.write(0x62 | 0b10000000); spi.write(0x08);
+    spi.write(0x63 | 0b10000000); spi.write(0x50);
+    spi.write(0x7F | 0b10000000); spi.write(0x0A);
+    spi.write(0x45 | 0b10000000); spi.write(0x60);
+    spi.write(0x7F | 0b10000000); spi.write(0x00);
+    spi.write(0x4D | 0b10000000); spi.write(0x11);
+    spi.write(0x55 | 0b10000000); spi.write(0x80);
+    spi.write(0x74 | 0b10000000); spi.write(0x1F);
+    spi.write(0x75 | 0b10000000); spi.write(0x1F);
+    spi.write(0x4A | 0b10000000); spi.write(0x78);
+    spi.write(0x4B | 0b10000000); spi.write(0x78);
+    spi.write(0x44 | 0b10000000); spi.write(0x08);
+    spi.write(0x45 | 0b10000000); spi.write(0x50);
+    spi.write(0x64 | 0b10000000); spi.write(0xFF);
+    spi.write(0x65 | 0b10000000); spi.write(0x1F);
+    spi.write(0x7F | 0b10000000); spi.write(0x14);
+    spi.write(0x65 | 0b10000000); spi.write(0x60);
+    spi.write(0x66 | 0b10000000); spi.write(0x08);
+    spi.write(0x63 | 0b10000000); spi.write(0x78);
+    spi.write(0x7F | 0b10000000); spi.write(0x15);
+    spi.write(0x48 | 0b10000000); spi.write(0x58);
+    spi.write(0x7F | 0b10000000); spi.write(0x07);
+    spi.write(0x41 | 0b10000000); spi.write(0x0D);
+    spi.write(0x43 | 0b10000000); spi.write(0x14);
+    spi.write(0x4B | 0b10000000); spi.write(0x0E);
+    spi.write(0x45 | 0b10000000); spi.write(0x0F);
+    spi.write(0x44 | 0b10000000); spi.write(0x42);
+    spi.write(0x4C | 0b10000000); spi.write(0x80);
+    spi.write(0x7F | 0b10000000); spi.write(0x10);
+    spi.write(0x5B | 0b10000000); spi.write(0x02);
+    spi.write(0x7F | 0b10000000); spi.write(0x07);
+    spi.write(0x40 | 0b10000000); spi.write(0x41);
+    spi.write(0x70 | 0b10000000); spi.write(0x00);
     
     //
     wait(0.1);
-    spi.write(0x32); spi.write(0x44);
-    spi.write(0x7F); spi.write(0x07);
-    spi.write(0x40); spi.write(0x40);
-    spi.write(0x7F); spi.write(0x06);
-    spi.write(0x62); spi.write(0xf0);
-    spi.write(0x63); spi.write(0x00);
-    spi.write(0x7F); spi.write(0x0D);
-    spi.write(0x48); spi.write(0xC0);
-    spi.write(0x6F); spi.write(0xd5);
-    spi.write(0x7F); spi.write(0x00);
-    spi.write(0x5B); spi.write(0xa0);
-    spi.write(0x4E); spi.write(0xA8);
-    spi.write(0x5A); spi.write(0x50);
-    spi.write(0x40); spi.write(0x80);
+    spi.write(0x32 | 0b10000000); spi.write(0x44);
+    spi.write(0x7F | 0b10000000); spi.write(0x07);
+    spi.write(0x40 | 0b10000000); spi.write(0x40);
+    spi.write(0x7F | 0b10000000); spi.write(0x06);
+    spi.write(0x62 | 0b10000000); spi.write(0xf0);
+    spi.write(0x63 | 0b10000000); spi.write(0x00);
+    spi.write(0x7F | 0b10000000); spi.write(0x0D);
+    spi.write(0x48 | 0b10000000); spi.write(0xC0);
+    spi.write(0x6F | 0b10000000); spi.write(0xd5);
+    spi.write(0x7F | 0b10000000); spi.write(0x00);
+    spi.write(0x5B | 0b10000000); spi.write(0xa0);
+    spi.write(0x4E | 0b10000000); spi.write(0xA8);
+    spi.write(0x5A | 0b10000000); spi.write(0x50);
+    spi.write(0x40 | 0b10000000); spi.write(0x80);
     
     // Deselect the device by seting chip select high
     cs = 1;
@@ -143,24 +166,21 @@
     
     // Select the device by seting chip select low
     cs = 0; 
-    
+        
     //   
-    spi.write(0x02); wait(0.01); spi.write(0x00); wait(0.01);
-    // 
-    spi.write(0x03); wait(0.01); data[0] = spi.write(0x00); wait(0.01);
-    spi.write(0x04); wait(0.01); data[1] = spi.write(0x00); wait(0.01);
-    spi.write(0x05); wait(0.01); data[2] = spi.write(0x00); wait(0.01);
-    spi.write(0x06); wait(0.01); data[3] = spi.write(0x00); wait(0.01);
+    spi.write(MOTION & 0b01111111); spi.write(0x00); 
+    // Write the register to read sensor ID (set MSB to 0 for read) 
+    spi.write(DELTA_X_L & 0b01111111); data[0] = spi.write(0x00); 
+    spi.write(DELTA_X_H & 0b01111111); data[1] = spi.write(0x00); 
+    spi.write(DELTA_Y_L & 0b01111111); data[2] = spi.write(0x00); 
+    spi.write(DELTA_Y_H & 0b01111111); data[3] = spi.write(0x00); 
 
- /*   // Reassemble the data (two 8 bit data into one 16 bit data)
+    // Reassemble the data (two 8 bit data into one 16 bit data)
     int16_t x_raw = (data[1] << 8 ) | data[0];
     int16_t y_raw = (data[3] << 8 ) | data[2];
     // Convert to SI units [rad/s]
     x = x_raw * 1.0f;
-    y = y_raw * 1.0f; */
-    
-    x = (data[1] << 8 ) | data[0];
-    y = (data[3] << 8 ) | data[2];
+    y = y_raw * 1.0f; 
     
     // Deselect the device by seting chip select high
     cs = 1;