Merge pull request #3 from gbr1/dev

0.0.2

Author: gbr1

examples/imu/imu.ino

@@ -1,6 +1,9 @@
+#include "Wire.h"
 #include "motion_fx.h"
 #include "LSM6DSOSensor.h"
 
+TwoWire wire(PB7, PB8);
+
 #define ALGO_FREQ  100U /* Algorithm frequency 100Hz */
 #define ALGO_PERIOD  (1000U / ALGO_FREQ) /* Algorithm period [ms] */
 #define MOTION_FX_ENGINE_DELTATIME  0.01f
@@ -37,7 +40,7 @@ int32_t gyroscope[3];
 int32_t magnetometer[3];
 int32_t MagOffset[3];
 
-LSM6DSOSensor AccGyr(&Wire, LSM6DSO_I2C_ADD_L);
+LSM6DSOSensor AccGyr(&wire, LSM6DSO_I2C_ADD_L);
 
 HardwareTimer *MyTim;
 
@@ -54,8 +57,8 @@ void setup() {
   while (!Serial) yield();
 
   /* Initialize I2C bus */
-  Wire.begin();
-  Wire.setClock(400000);
+  wire.begin();
+  wire.setClock(400000);
 
   /* Start communication with IMU */
   AccGyr.begin();
@@ -94,7 +97,7 @@ void setup() {
   /* Get library version */
   LibVersionLen = (int)MotionFX_GetLibVersion(LibVersion);
 
-  MyTim = new HardwareTimer(TIM3);
+  MyTim = new HardwareTimer(TIM9);  //TIM3
   MyTim->setOverflow(ALGO_FREQ, HERTZ_FORMAT);
   MyTim->attachInterrupt(fusion_update);
   MyTim->resume();

examples/servo/servo.ino

@@ -0,0 +1,16 @@
+#include "Arduino_Robot_Firmware.h"
+
+Arduino_Robot_Firmware robot;
+
+void setup(){
+    robot.begin();
+}
+
+void loop(){
+    for(int i=0; i<180; i++){
+        robot.setServoA(i);
+        robot.setServoB(i);
+        delay(15);
+    }
+    delay(5000);
+}

examples/tof3/tof3.ino

@@ -0,0 +1,48 @@
+#include <Wire.h>
+#include <VL53L1X.h>
+#include <Wire.h>
+
+TwoWire wire(PB7, PB8);
+const uint8_t sensorCount = 3;
+
+const uint8_t xshutPins[sensorCount] = { PC5, PB0, PB1 }; //Right, Center, Left
+
+VL53L1X sensors[sensorCount];
+
+void setup(){
+  while (!Serial);
+  Serial.begin(115200);
+  wire.begin();
+  wire.setClock(400000);
+
+  for (uint8_t i = 0; i < sensorCount; i++){
+    pinMode(xshutPins[i], OUTPUT);
+    digitalWrite(xshutPins[i], LOW);
+  }
+
+  for (uint8_t i = 0; i < sensorCount; i++){
+    pinMode(xshutPins[i], INPUT);
+    delay(10);
+    sensors[i].setBus(&wire);
+    sensors[i].setTimeout(500);
+    if (!sensors[i].init()){
+      Serial.print("Failed to detect and initialize sensor ");
+      Serial.println(i);
+      while (1);
+    }
+
+    sensors[i].setAddress(0x2A + i);
+    sensors[i].startContinuous(50);
+  }
+}
+
+void loop(){
+  for (uint8_t i = 0; i < sensorCount; i++){
+    Serial.print(sensors[i].read());
+    if (sensors[i].timeoutOccurred()){
+        Serial.print(" TIMEOUT");
+    }
+    Serial.print('\t');
+  }
+  Serial.print("\n");
+}

examples/tofM/tofM.ino

@@ -0,0 +1,50 @@
+#include <Arduino.h>
+#include <Wire.h>
+#include <vl53l7cx_class.h>
+
+
+TwoWire wire(PB7, PB8);
+#define DEV_I2C wire
+
+#define SerialPort Serial
+
+#define LPN_PIN PB1
+#define I2C_RST_PIN PB0
+
+VL53L7CX sensor_vl53l7cx(&DEV_I2C, LPN_PIN, I2C_RST_PIN);
+
+
+uint8_t res = VL53L7CX_RESOLUTION_8X8; //VL53L7CX_RESOLUTION_4X4
+
+
+void setup(){
+  SerialPort.begin(115200);
+  DEV_I2C.begin();
+  sensor_vl53l7cx.begin();
+  sensor_vl53l7cx.init_sensor();
+  sensor_vl53l7cx.vl53l7cx_set_resolution(res);
+  sensor_vl53l7cx.vl53l7cx_start_ranging();
+}
+
+void loop(){
+  VL53L7CX_ResultsData Results;
+  uint8_t NewDataReady = 0;
+  uint8_t status;
+  do {
+    status = sensor_vl53l7cx.vl53l7cx_check_data_ready(&NewDataReady);
+  } while (!NewDataReady);
+
+  if ((!status) && (NewDataReady != 0)) {
+    status = sensor_vl53l7cx.vl53l7cx_get_ranging_data(&Results);
+    for (int y=0; y<8; y++){
+      for (int x=0; x<8; x++){
+        Serial.print((int)Results.distance_mm[x+y*8]);
+        Serial.print(" ");
+      }
+      Serial.println();
+    }
+    Serial.println();
+  }
+  delay(100);
+
+}

library.properties

@@ -1,5 +1,5 @@
 name=Arduino_Robot_Firmware
-version=0.0.1
+version=0.0.2
 author=Arduino, Giovanni di Dio Bruno, Lucio Rossi
 maintainer=Arduino <[email protected]>
 sentence=Library and firmware for Arduino Robot

src/Arduino_Robot_Firmware.cpp

@@ -21,6 +21,10 @@ Arduino_Robot_Firmware::Arduino_Robot_Firmware(){
 
     // color sensor
     apds9960 = new APDS9960(*wire,APDS_INT);
+
+    // servo
+    servo_A = new Servo();
+    servo_B = new Servo();
 }
 
 int Arduino_Robot_Firmware::begin(){
@@ -44,6 +48,9 @@ int Arduino_Robot_Firmware::begin(){
     wire->begin();
 
     beginAPDS();
+    beginServo();
+    beginI2Cselect();
+    connectExternalI2C();
 
     return 0;
 }
@@ -97,4 +104,42 @@ int Arduino_Robot_Firmware::getBlue(){
 
 int Arduino_Robot_Firmware::getProximity(){
     return bottom_proximity;
+}
+
+/******************************************************************************************************/
+/*                                        RC Servo A & B                                              */
+/******************************************************************************************************/
+
+int Arduino_Robot_Firmware::beginServo(){
+    servo_A->attach(SERVO_A);
+    servo_B->attach(SERVO_B);
+    return 0;
+}
+
+void Arduino_Robot_Firmware::setServoA(int position){
+    servo_A->write(position);
+}
+
+void Arduino_Robot_Firmware::setServoB(int position){
+    servo_B->write(position);
+}
+
+/******************************************************************************************************/
+/*                                        RC Servo A & B                                              */
+/******************************************************************************************************/
+
+int Arduino_Robot_Firmware::beginI2Cselect(){
+    pinMode(SELECT_I2C_BUS,OUTPUT);
+}
+
+void Arduino_Robot_Firmware::setExternalI2C(uint8_t state){
+    digitalWrite(SELECT_I2C_BUS,state);
+}
+
+void Arduino_Robot_Firmware::connectExternalI2C(){
+    setExternalI2C(LOW);
+}
+
+void Arduino_Robot_Firmware::disconnectExternalI2C(){
+    setExternalI2C(HIGH);
 }

src/Arduino_Robot_Firmware.h

@@ -8,20 +8,26 @@
 #include "dcmotor.h"
 #include "motor_control.h"
 #include "Arduino_APDS9960.h"
+#include <Servo.h>
 
 class Arduino_Robot_Firmware{
     private:
         APDS9960 * apds9960;
         int bottom_red, bottom_green, bottom_blue, bottom_clear, bottom_proximity;
 
+        Servo * servo_A;
+        Servo * servo_B;
+
+        TwoWire * wire;
+
+
     public:
         RGBled * led1;
         RGBled * led2;
         DCmotor * motor_left;
         DCmotor * motor_right;
         Encoder * encoder_left;
         Encoder * encoder_right;
-        TwoWire * wire;
 
 
         Arduino_Robot_Firmware();
@@ -42,6 +48,20 @@ class Arduino_Robot_Firmware{
         int getGreen();                             // green value 0-255
         int getBlue();                              // blue value 0-255
         int getProximity();                         // proximity value 0-127
+
+
+        // Servo
+        int beginServo();                           // initialize Servo interfaces
+        void setServoA(int position);               // 0°-180° servo position
+        void setServoB(int position);               // 0°-180° servo position
+
+
+        // I2C select
+        int beginI2Cselect();                       // initialize I2C bus selector
+        void setExternalI2C(uint8_t state);         // set A4,A5 connection on I2C bus 2
+        void connectExternalI2C();                  // allow A4,A5 on nano connector to be attached to I2C bus 2
+        void disconnectExternalI2C();               // disable the connection on A4,A5
+
 };
 
 #endif

src/pinout_definitions.h

@@ -44,8 +44,11 @@
 #define APDS_LED PB6
 #define APDS_INT PC10
 
+// I2C ports
 #define I2C_1_SDA PB7
 #define I2C_1_SCL PB8
+#define SELECT_I2C_BUS PB2
+