starting tof, wip, not working yet

Author: gbr1

.DS_Store

@@ -67,15 +67,12 @@ int Arduino_Robot_Firmware::begin(){
     connectExternalI2C();
     ext_wire->begin(ARDUINO_ROBOT_ADDRESS);
 
-
 
     if (beginAPDS()!=0){
         errorLed(ERROR_APDS);
     }
 
     beginServo();
-    //beginI2Cselect();
-    //connectExternalI2C();
 
     if (beginBMS()!=0){
         errorLed(ERROR_BMS);
@@ -85,7 +82,7 @@ int Arduino_Robot_Firmware::begin(){
         //errorLed(ERROR_TOUCH);
     }
 
-    if (beginImu()!=){
+    if (beginImu()!=0){
         errorLed(ERROR_IMU);
     }
 
@@ -268,7 +265,7 @@ void Arduino_Robot_Firmware::updateTouch(){
     touch_status = touch_sensor->getStatus();
 }
 
-bool Arduino_Robot_Firmware::getTouchPressed(){
+bool Arduino_Robot_Firmware::getAnyTouchPressed(){
     if (touch_sensor->touched(touch_status,TOUCH_PAD_GUARD)){
         return true;
     }
@@ -395,6 +392,7 @@ int Arduino_Robot_Firmware::beginImu(){
 void Arduino_Robot_Firmware::updateImu(){
     imu->Get_X_Axes(accelerometer);
     imu->Get_G_Axes(gyroscope);
+
 }
 
 

src/Arduino_Robot_Firmware.cpp

@@ -12,6 +12,8 @@
 #include "Arduino_MAX17332.h"
 #include "AT42QT2120.h"
 #include "LSM6DSOSensor.h"
+#include "motion_fx.h"
+
 
 class Arduino_Robot_Firmware{
     private:
@@ -38,6 +40,10 @@ class Arduino_Robot_Firmware{
         int32_t gyroscope[3];
 
 
+        MFX_input_t data_in;
+        MFX_output_t data_out;
+
+
     public:
 
         DCmotor * motor_left;
@@ -102,7 +108,7 @@ class Arduino_Robot_Firmware{
         // Touch
         int beginTouch();
         void updateTouch();
-        bool getTouchPressed();
+        bool getAnyTouchPressed();
         bool getTouchKey(const uint8_t key);
         bool getTouchUp();
         bool getTouchRight();

src/Arduino_Robot_Firmware.h

@@ -69,6 +69,11 @@
 #define EXT_A1 PA6 //MISO
 #define EXT_A2 PA5 //SCK
 #define EXT_A3 PA4 //CS
+#define EXT_GPIO0 PC3
+#define EXT_GPIO1 PC4
+#define EXT_GPIO2 PB0
+#define EXT_GPIO3 PB1
+
 
 
 // Uart

src/pinout_definitions.h

@@ -1,6 +1,7 @@
 #ifndef __ROBOT_DEFINITIONS_H__
 #define __ROBOT_DEFINITIONS_H__
 
+// Motor Control and mechanical parameters
 #define CONTROL_LIMIT 4096              // PWM resolution
 
 #define MOTOR_LIMIT 100.0               // Mechanical RPM limit speed of used motors
@@ -15,10 +16,18 @@ const float MOTOR_RATIO = MOTOR_CPR*MOTOR_GEAR_RATIO;
 #define MOTOR_CONTROL_PERIOD 0.01
 
 
-
+// Sensor fusioning parameters
 #define FROM_MG_TO_G  0.001f
 #define FROM_G_TO_MG  1000.0f
 #define FROM_MDPS_TO_DPS  0.001f
 #define FROM_DPS_TO_MDPS  1000.0f
+#define MOTION_FX_FREQ  100U
+const float MOTION_FX_PERIOD = (1000U / MOTION_FX_FREQ);
+#define MOTION_FX_ENGINE_DELTATIME  0.01f
+#define STATE_SIZE                      (size_t)(2432)
+#define SAMPLETODISCARD                 15
+#define GBIAS_ACC_TH_SC                 (2.0f*0.000765f)
+#define GBIAS_GYRO_TH_SC                (2.0f*0.002f)
+#define DECIMATION                      1U
 
 #endif

src/robot_definitions.h

@@ -0,0 +1,54 @@
+#ifndef __SENSOR_TOF_MATRIX_H__
+#define __SENSOR_TOF_MATRIX_H__
+
+#include "Arduino.h"
+#include "Wire.h"
+#include "vl53l7cx_class.h"
+
+
+class SensorTofMatrix{
+    private:
+        VL53L7CX * sensor;
+        TwoWire * wire;
+    public:
+        SensorTofMatrix(TwoWire * _wire, const uint8_t lpn_pin, const uint8_t i2c_rst_pin){
+            wire=_wire;
+            sensor = new VL53L7CX(wire,lpn_pin,i2c_rst_pin);
+        }
+
+        int begin(){
+            wire->begin();
+            sensor->begin();
+            sensor->init_sensor();
+            sensor->vl53l7cx_set_resolution(VL53L7CX_RESOLUTION_4X4);
+            sensor->vl53l7cx_start_ranging();
+        }
+
+        void print(){
+            int size=4;
+            VL53L7CX_ResultsData Results;
+            uint8_t NewDataReady = 0;
+            uint8_t status;
+            do {
+                status = sensor->vl53l7cx_check_data_ready(&NewDataReady);
+            } while (!NewDataReady);
+
+            if ((!status) && (NewDataReady != 0)) {
+                status = sensor->vl53l7cx_get_ranging_data(&Results);
+                for (int y=0; y<size; y++){
+                    for (int x=0; x<size; x++){
+                        Serial.print((int)Results.distance_mm[x+y*size]);
+                        Serial.print(" ");
+                    }  
+                    Serial.println();
+                }
+                Serial.println();
+            }
+        }
+
+        /*void setResolution(){
+            sensor->setRes
+        }*/
+};
+
+#endif