Diff: XNucleo6810.cpp

Revision:

2:04a5cf34e1a9

Child:

3:862286610dfd

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/XNucleo6810.cpp	Tue Nov 10 10:19:14 2020 +0000
@@ -0,0 +1,164 @@
+#include "XNucleo6810.h"
+
+#define theVL6180DevDefault   0x52   // sensor i2c addresss on powerup
+
+XNucleo53L1A1 *XNucleo53L1A1::_instance = NULL;
+
+
+XNucleo53L1A1 *XNucleo53L1A1::instance(vl6180_DevI2C *ext_i2c)
+{
+    if (_instance == NULL) {
+        _instance = new XNucleo53L1A1(ext_i2c);
+    } else {
+        printf("Failed to create XNucleo53L1A1 instance\n\r\r\n");
+    }
+    return _instance;
+}
+
+XNucleo53L1A1 *XNucleo53L1A1::instance(vl6180_DevI2C *ext_i2c,
+                                       PinName gpio1_centre,
+                                       PinName gpio1_left, PinName gpio1_right,PinName gpio1_bottom)
+{
+    if (_instance == NULL) {
+//        printf("instance %d z\n",ext_i2c);
+        _instance = new XNucleo53L1A1(ext_i2c, gpio1_centre, gpio1_left, gpio1_right,gpio1_bottom);
+    } else {
+        printf("Failed to create XNucleo53L0A1 instance\n\r");
+    }
+    return _instance;
+}
+
+
+int XNucleo53L1A1::init_board()
+{
+    printf("init_board \n");
+    int status, n_dev = 0;
+
+    sensor_centre->VL53L1_Off();
+    sensor_left->VL53L1_Off();
+    sensor_right->VL53L1_Off();
+    sensor_bottom->VL53L1_Off();
+   // int i = 0;
+    
+    // On startup or on the shutdown pin going high the VL53l1 devices have a i2c address of 0x52.
+    // To initialise then we have to bring up one device at a time This involves raising the shutdown pin for that device,
+    // initialising then setting the i2c address to a unique value. Once that is done the shutdown pins don't need to be touched.
+    
+    // set the shutdown pins to be outputs ( they are inputs on power up)
+    stmpe1600_exp0->set_gpio_dir(GPIO_0,OUTPUT);
+    stmpe1600_exp0->set_gpio_dir(GPIO_1,OUTPUT);
+    stmpe1600_exp0->set_gpio_dir(GPIO_2,OUTPUT);
+    stmpe1600_exp0->set_gpio_dir(GPIO_3,OUTPUT);
+    stmpe1600_exp0->set_gpio_dir(GPIO_4,OUTPUT);
+    stmpe1600_exp0->set_gpio_dir(GPIO_5,OUTPUT);
+    stmpe1600_exp0->set_gpio_dir(GPIO_6,OUTPUT);
+    stmpe1600_exp0->set_gpio_dir(GPIO_7,OUTPUT);
+    stmpe1600_exp0->set_gpio_dir(GPIO_8,OUTPUT);
+    stmpe1600_exp0->set_gpio_dir(GPIO_9,OUTPUT);
+    stmpe1600_exp0->set_gpio_dir(GPIO_10,OUTPUT);
+    stmpe1600_exp0->set_gpio_dir(GPIO_11,OUTPUT);
+ 
+    stmpe1600_exp0->set_gpio_dir(GPIO_12,OUTPUT);
+    stmpe1600_exp0->set_gpio_dir(GPIO_14,OUTPUT);
+    stmpe1600_exp0->set_gpio_dir(GPIO_15,OUTPUT);
+    stmpe1600_exp0->set_gpio_dir(GPIO_13,OUTPUT);
+    
+    // set the shutdown pins to low, this will reset the VL53l1s
+
+    stmpe1600_exp0->set_gpio(GPIO_0);
+    stmpe1600_exp0->set_gpio(GPIO_1);
+    stmpe1600_exp0->set_gpio(GPIO_2);
+    stmpe1600_exp0->set_gpio(GPIO_3);
+    stmpe1600_exp0->set_gpio(GPIO_4);
+    stmpe1600_exp0->set_gpio(GPIO_5);
+    stmpe1600_exp0->set_gpio(GPIO_6);
+    
+    stmpe1600_exp0->set_gpio(GPIO_7);
+    stmpe1600_exp0->set_gpio(GPIO_8);
+    stmpe1600_exp0->set_gpio(GPIO_9);
+    stmpe1600_exp0->set_gpio(GPIO_10);
+    stmpe1600_exp0->set_gpio(GPIO_11);    
+    
+    stmpe1600_exp0->clear_gpio(GPIO_12);
+    stmpe1600_exp0->clear_gpio(GPIO_14);
+    stmpe1600_exp0->clear_gpio(GPIO_15);
+    stmpe1600_exp0->clear_gpio(GPIO_13);
+
+// select the first VL53l1. It will have i2c address of 0x52. Set the i2c address to
+//NEW_SENSOR_CENTRE_ADDRESS
+    printf("Sensor centre init \n\r");
+    stmpe1600_exp0->set_gpio(GPIO_12);
+    sensor_centre->init_sensor(theVL6180DevDefault);//set id to default address
+    status = sensor_centre->InitSensor(NEW_SENSOR_CENTRE_ADDRESS); // now set id to unique address
+    if (status) {
+        delete sensor_centre;
+        delete xshutdown_centre;
+        sensor_centre = NULL;
+        xshutdown_centre = NULL;
+        printf("Sensor centre not present\n\r");
+    } else {
+        printf("Sensor centre present\n\r");
+        n_dev++;
+    }
+    wait_ms(50);  // NEEDS A DELAY BETWEEN SENSORS
+    // select the second VL53l1. It will have i2c address of 0x52. Set the i2c address to
+    //NEW_SENSOR_LEFT_ADDRESS
+    
+    stmpe1600_exp0->set_gpio(GPIO_14);
+        printf("Sensor left init \n\r");
+    sensor_left->init_sensor(theVL6180DevDefault);//set id to default address
+    status = sensor_left->InitSensor(NEW_SENSOR_LEFT_ADDRESS);  // now set id to unique address
+    if (status) {
+        delete sensor_left;
+        delete xshutdown_left;
+        sensor_left = NULL;
+        xshutdown_left = NULL;
+        printf("Sensor left not present\n\r");
+    } else {
+        printf("Sensor left present\n\r");
+        n_dev++;
+    }
+  wait_ms(50);  
+     // select the 3rd VL53l1. It will have i2c address of 0x52. Set the i2c address to
+    //NEW_SENSOR_RIGHT_ADDRESS 
+    printf("Sensor right init \n\r");
+    stmpe1600_exp0->set_gpio(GPIO_15);
+    sensor_right->init_sensor(theVL6180DevDefault);//set id to default address
+        printf("Sensor right init_sensor \n\r");
+    status = sensor_right->InitSensor(NEW_SENSOR_RIGHT_ADDRESS);
+        printf("Sensor right InitSensor \n\r");
+    if (status) {
+        delete sensor_right;
+        delete xshutdown_right;
+        sensor_right = NULL;
+        xshutdown_right = NULL;
+        printf("Sensor right not present\n\r");
+    } else {
+        printf("Sensor right present\n\r");
+        n_dev++;
+    }
+wait_ms(50);
+
+     // select the 4th VL53l1. It will have i2c address of 0x52. Set the i2c address to
+    //NEW_SENSOR_BOTTOM_ADDRESS 
+    printf("Sensor bottom init \n\r");
+    stmpe1600_exp0->set_gpio(GPIO_13);
+    sensor_bottom->init_sensor(theVL6180DevDefault);//set id to default address
+    status = sensor_bottom->InitSensor(NEW_SENSOR_BOTTOM_ADDRESS);
+    if (status) {
+        delete sensor_bottom;
+        delete xshutdown_bottom;
+        sensor_bottom = NULL;
+        xshutdown_bottom = NULL;
+        printf("Sensor bottom not present\n\r");
+    } else {
+        printf("Sensor bottom present\n\r");
+        n_dev++;
+    }
+wait_ms(50);
+    if (n_dev == 0) {
+        return 1;
+    } else {
+        return 0;
+    }
+}