Merge pull request linorobot#79 from hippo5329/pr-esp32

Add esp32 support

config/config.h

@@ -11,6 +11,8 @@
 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 // See the License for the specific language governing permissions and
 // limitations under the License.
+#ifndef CONFIG_H
+#define CONFIG_H
 
 #ifdef USE_VATTENKAR_CONFIG
     #include "custom/vattenkar_config.h"
@@ -20,6 +22,13 @@
     #include "custom/dev_config.h"
 #endif
 
-#if !defined (USE_VATTENKAR_CONFIG)
+#ifdef USE_ESP32_CONFIG
+    #include "custom/esp32_config.h"
+#endif
+
+// this should be the last one
+#ifndef LINO_BASE
     #include "lino_base_config.h"
-#endif
+#endif
+
+#endif

config/custom/esp32_config.h

@@ -0,0 +1,233 @@
+// Copyright (c) 2021 Juan Miguel Jimeno
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.esp32
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef ESP32_CONFIG_H
+#define ESP32_CONFIG_H
+
+#define LED_PIN 2 //used for debugging status
+
+//uncomment the base you're building
+#define LINO_BASE DIFFERENTIAL_DRIVE       // 2WD and Tracked robot w/ 2 motors
+// #define LINO_BASE SKID_STEER            // 4WD robot
+// #define LINO_BASE MECANUM               // Mecanum drive robot
+
+//uncomment the motor driver you're using
+// #define USE_GENERIC_2_IN_MOTOR_DRIVER      // Motor drivers with 2 Direction Pins(INA, INB) and 1 PWM(ENABLE) pin ie. L298, L293, VNH5019
+// #define USE_GENERIC_1_IN_MOTOR_DRIVER   // Motor drivers with 1 Direction Pin(INA) and 1 PWM(ENABLE) pin.
+#define USE_BTS7960_MOTOR_DRIVER        // BTS7970 Motor Driver using A4950 (<40V) module or DRV8833 (<10V)
+// #define USE_ESC_MOTOR_DRIVER            // Motor ESC for brushless motors
+
+//uncomment the IMU you're using
+// #define USE_GY85_IMU
+#define USE_MPU6050_IMU
+// #define USE_MPU9150_IMU
+// #define USE_MPU9250_IMU
+// #define USE_QMI8658_IMU
+// #define USE_HMC5883L_MAG
+// #define USE_AK8963_MAG
+// #define USE_AK8975_MAG
+// #define USE_AK09918_MAG
+// #define USE_QMC5883L_MAG
+// #define MAG_BIAS { 0, 0, 0 }
+
+#define K_P 0.6                             // P constant
+#define K_I 0.8                             // I constant
+#define K_D 0.5                             // D constant
+
+/*
+ROBOT ORIENTATION
+         FRONT
+    MOTOR1  MOTOR2  (2WD/ACKERMANN)
+    MOTOR3  MOTOR4  (4WD/MECANUM)
+         BACK
+*/
+
+//define your robot' specs here
+#define MOTOR_MAX_RPM 150                   // motor's max RPM
+#define MAX_RPM_RATIO 0.85                  // max RPM allowed for each MAX_RPM_ALLOWED = MOTOR_MAX_RPM * MAX_RPM_RATIO
+#define MOTOR_OPERATING_VOLTAGE 12          // motor's operating voltage (used to calculate max RPM)
+#define MOTOR_POWER_MAX_VOLTAGE 12          // max voltage of the motor's power source (used to calculate max RPM)
+#define MOTOR_POWER_MEASURED_VOLTAGE 12     // current voltage reading of the power connected to the motor (used for calibration)
+#define COUNTS_PER_REV1 550                 // wheel1 encoder's no of ticks per rev
+#define COUNTS_PER_REV2 550                 // wheel2 encoder's no of ticks per rev
+#define COUNTS_PER_REV3 550                 // wheel3 encoder's no of ticks per rev
+#define COUNTS_PER_REV4 550                 // wheel4 encoder's no of ticks per rev
+#define WHEEL_DIAMETER 0.0560               // wheel's diameter in meters
+#define LR_WHEELS_DISTANCE 0.224            // distance between left and right wheels
+#define PWM_BITS 10                         // PWM Resolution of the microcontroller
+#define PWM_FREQUENCY 20000                 // PWM Frequency
+
+// INVERT ENCODER COUNTS
+#define MOTOR1_ENCODER_INV false
+#define MOTOR2_ENCODER_INV false
+#define MOTOR3_ENCODER_INV false
+#define MOTOR4_ENCODER_INV false
+
+// INVERT MOTOR DIRECTIONS
+#define MOTOR1_INV false
+#define MOTOR2_INV false
+#define MOTOR3_INV false
+#define MOTOR4_INV false
+
+// ENCODER PINS
+#define MOTOR1_ENCODER_A 36
+#define MOTOR1_ENCODER_B 39
+
+#define MOTOR2_ENCODER_A 35
+#define MOTOR2_ENCODER_B 34
+
+#define MOTOR3_ENCODER_A 32
+#define MOTOR3_ENCODER_B 27
+
+#define MOTOR4_ENCODER_A 26
+#define MOTOR4_ENCODER_B 25
+
+// MOTOR PINS
+#ifdef USE_GENERIC_2_IN_MOTOR_DRIVER
+  #define MOTOR1_PWM 21 //Pin no 21 is not a PWM pin on Teensy 4.x, you can swap it with pin no 1 instead.
+  #define MOTOR1_IN_A 20
+  #define MOTOR1_IN_B 1
+
+  #define MOTOR2_PWM 5
+  #define MOTOR2_IN_A 6
+  #define MOTOR2_IN_B 8
+
+  #define MOTOR3_PWM 22
+  #define MOTOR3_IN_A 23
+  #define MOTOR3_IN_B 0
+
+  #define MOTOR4_PWM 4
+  #define MOTOR4_IN_A 3
+  #define MOTOR4_IN_B 2
+
+  #define PWM_MAX pow(2, PWM_BITS) - 1
+  #define PWM_MIN -PWM_MAX
+#endif
+
+#ifdef USE_GENERIC_1_IN_MOTOR_DRIVER
+  #define MOTOR1_PWM 21 //Pin no 21 is not a PWM pin on Teensy 4.x, you can use pin no 1 instead.
+  #define MOTOR1_IN_A 20
+  #define MOTOR1_IN_B -1 //DON'T TOUCH THIS! This is just a placeholder
+
+  #define MOTOR2_PWM 5
+  #define MOTOR2_IN_A 6
+  #define MOTOR2_IN_B -1 //DON'T TOUCH THIS! This is just a placeholder
+
+  #define MOTOR3_PWM 22
+  #define MOTOR3_IN_A 23
+  #define MOTOR3_IN_B -1 //DON'T TOUCH THIS! This is just a placeholder
+
+  #define MOTOR4_PWM 4
+  #define MOTOR4_IN_A 3
+  #define MOTOR4_IN_B -1 //DON'T TOUCH THIS! This is just a placeholder
+
+  #define PWM_MAX pow(2, PWM_BITS) - 1
+  #define PWM_MIN -PWM_MAX
+#endif
+
+#ifdef USE_BTS7960_MOTOR_DRIVER
+  #define MOTOR1_PWM -1 //DON'T TOUCH THIS! This is just a placeholder
+  #define MOTOR1_IN_A 19 // Pin no 21 is not a PWM pin on Teensy 4.x, you can use pin no 1 instead.
+  #define MOTOR1_IN_B 18 // Pin no 20 is not a PWM pin on Teensy 4.x, you can use pin no 0 instead.
+
+  #define MOTOR2_PWM -1 //DON'T TOUCH THIS! This is just a placeholder
+  #define MOTOR2_IN_A 16
+  #define MOTOR2_IN_B 17
+
+  #define MOTOR3_PWM -1 //DON'T TOUCH THIS! This is just a placeholder
+  #define MOTOR3_IN_A 13
+  #define MOTOR3_IN_B 12
+
+  #define MOTOR4_PWM -1 //DON'T TOUCH THIS! This is just a placeholder
+  #define MOTOR4_IN_A 4
+  #define MOTOR4_IN_B 23
+
+  #define PWM_MAX pow(2, PWM_BITS) - 1
+  #define PWM_MIN -PWM_MAX
+#endif
+
+#ifdef USE_ESC_MOTOR_DRIVER
+  #define MOTOR1_PWM 21 //Pin no 21 is not a PWM pin on Teensy 4.x. You can use pin no 1 instead.
+  #define MOTOR1_IN_A -1 //DON'T TOUCH THIS! This is just a placeholder
+  #define MOTOR1_IN_B -1 //DON'T TOUCH THIS! This is just a placeholder
+
+  #define MOTOR2_PWM 5
+  #define MOTOR2_IN_A -1 //DON'T TOUCH THIS! This is just a placeholder
+  #define MOTOR2_IN_B -1 //DON'T TOUCH THIS! This is just a placeholder
+
+  #define MOTOR3_PWM 22
+  #define MOTOR3_IN_A -1 //DON'T TOUCH THIS! This is just a placeholder
+  #define MOTOR3_IN_B -1 //DON'T TOUCH THIS! This is just a placeholder
+
+  #define MOTOR4_PWM 4
+  #define MOTOR4_IN_A -1 //DON'T TOUCH THIS! This is just a placeholder
+  #define MOTOR4_IN_B -1 //DON'T TOUCH THIS! This is just a placeholder
+
+  #define PWM_MAX 400
+  #define PWM_MIN -PWM_MAX
+#endif
+
+// #define USE_WIFI_TRANSPORT  // use micro ros wifi transport
+#define AGENT_IP { 192, 168, 1, 100 }  // eg IP of the desktop computer
+#define AGENT_PORT 8888
+// Enable WiFi with null terminated list of multiple APs SSID and password
+// #define WIFI_AP_LIST {{"WIFI_SSID", "WIFI_PASSWORD"}, {NULL}}
+#define WIFI_MONITOR 2 // min. period to send wifi signal strength to syslog
+// #define USE_ARDUINO_OTA
+// #define USE_SYSLOG
+#define SYSLOG_SERVER { 192, 168, 1, 100 }  // eg IP of the desktop computer
+#define SYSLOG_PORT 514
+#define DEVICE_HOSTNAME "esp32"
+#define APP_NAME "hardware"
+// #define USE_LIDAR_UDP
+#define LIDAR_RXD 14
+// #define LIDAR_PWM 15
+#define LIDAR_SERIAL 1 // uart number
+#define LIDAR_BAUDRATE 230400
+#define LIDAR_SERVER { 192, 168, 1, 100 }  // eg IP of the desktop computer
+#define LIDAR_PORT 8889
+#define BAUDRATE 115200
+// #define SDA_PIN 21 // specify I2C pins
+// #define SCL_PIN 22
+#define NODE_NAME "esp32"
+// #define TOPIC_PREFIX "esp32/"
+
+// battery voltage ADC pin
+#define BATTERY_PIN 33
+// 3.3V ref, 12 bits ADC, 33k + 10k voltage divider
+// #define USE_ADC_LUT
+#ifdef USE_ADC_LUT
+const int16_t ADC_LUT[4096] = { /* insert adc_calibrate data here */ };
+#define BATTERY_ADJUST(v) (ADC_LUT[v] * (3.3 / 4096 * (33 + 10) / 10 * 1.0))
+#else
+#define BATTERY_ADJUST(v) ((v) * (3.3 / 4096 * (33 + 10) / 10))
+#endif
+// #define USE_INA219
+// #define TRIG_PIN 31 // ultrasonic sensor HC-SR04
+// #define ECHO_PIN 32
+#define USE_SHORT_BRAKE // for shorter stopping distance
+// #define WDT_TIMEOUT 60 // Sec
+// #define BOARD_INIT sleep(5) // wait to begin IMU calibration
+
+#ifdef USE_SYSLOG
+#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){ \
+    syslog(LOG_ERR, "%s RCCHECK failed %d", __FUNCTION__, temp_rc); \
+    return false; }}
+#else
+#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){ \
+    flashLED(3); \
+    return false; }} // do not block
+#endif
+
+#endif

firmware/lib/encoder/encoder.cpp

@@ -1,10 +1,11 @@
 
 #include "encoder.h"
 
+#if !defined(ESP32)
 // Yes, all the code is in the header file, to provide the user
 // configure options with #define (before they include it), and
 // to facilitate some crafty optimizations!
 
 Encoder_internal_state_t * Encoder::interruptArgs[];
-
+#endif
 

firmware/lib/encoder/encoder.h

@@ -29,6 +29,57 @@
 #ifndef Encoder_h_
 #define Encoder_h_
 
+#ifdef ESP32
+#include "Arduino.h"
+#include <ESP32Encoder.h>
+class Encoder
+{
+private:
+	int counts_per_rev_ = -1;
+        ESP32Encoder encoder_;
+	unsigned long prev_update_time_;
+        int64_t prev_encoder_ticks_;
+public:
+	Encoder(int pin1, int pin2, int counts_per_rev, bool invert = false) {
+		int temp_pin = pin1;
+		if (pin1 < 0 || pin2 < 0) return; // unused encoder
+		if(invert)
+		{
+			pin1 = pin2;
+			pin2 = temp_pin;
+		}
+		counts_per_rev_ = counts_per_rev;
+		ESP32Encoder::useInternalWeakPullResistors = UP;
+		encoder_.attachHalfQuad(pin1, pin2);
+	}
+	float getRPM() {
+	        if (counts_per_rev_ < 0) return 0.0;
+		int64_t encoder_ticks = encoder_.getCount();
+		//this function calculates the motor's RPM based on encoder ticks and delta time
+		unsigned long current_time = micros();
+		unsigned long dt = current_time - prev_update_time_;
+
+		//convert the time from milliseconds to minutes
+		double dtm = (double)dt / 60000000;
+		int64_t delta_ticks = encoder_ticks - prev_encoder_ticks_;
+
+		//calculate wheel's speed (RPM)
+		prev_update_time_ = current_time;
+		prev_encoder_ticks_ = encoder_ticks;
+
+		return (((double) delta_ticks / counts_per_rev_) / dtm);
+	}
+	inline int32_t read() {
+	        if (counts_per_rev_ < 0) return 0;
+		return  encoder_.getCount();
+	}
+        inline void write(int32_t p) {
+	        if (counts_per_rev_ < 0) return;
+	        encoder_.setCount(p);
+	}
+};
+#else
+
 #if defined(ARDUINO) && ARDUINO >= 100
 #include "Arduino.h"
 #elif defined(WIRING)
@@ -1001,5 +1052,6 @@ ISR(INT7_vect) { Encoder::update(Encoder::interruptArgs[SCRAMBLE_INT_ORDER(7)]);
 #endif
 #endif // ENCODER_OPTIMIZE_INTERRUPTS
 
+#endif // ESP32
 
 #endif