board: add pico suuport

Add pico support as a custom configuration.

Signed-off-by: Thomas Chou <[email protected]>

config/config.h

@@ -26,6 +26,10 @@
     #include "custom/esp32_config.h"
 #endif
 
+#ifdef USE_PICO_CONFIG
+    #include "custom/pico_config.h"
+#endif
+
 // this should be the last one
 #ifndef LINO_BASE
     #include "lino_base_config.h"

config/custom/pico_config.h

@@ -0,0 +1,240 @@
+// 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.
+// 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 PICO_CONFIG_H
+#define PICO_CONFIG_H
+
+#define LED_PIN LED_BUILTIN //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 900                 // wheel1 encoder's no of ticks per rev
+#define COUNTS_PER_REV2 900                 // wheel2 encoder's no of ticks per rev
+#define COUNTS_PER_REV3 900                 // wheel3 encoder's no of ticks per rev
+#define COUNTS_PER_REV4 900                 // 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
+// Note: encoder pins must be consecutive (e.g. 2 and 3, 10 and 11 etc.)
+#define MOTOR1_ENCODER_A  6
+#define MOTOR1_ENCODER_B  7
+
+#define MOTOR2_ENCODER_A 10
+#define MOTOR2_ENCODER_B 11
+
+#define MOTOR3_ENCODER_A 20
+#define MOTOR3_ENCODER_B 21
+
+#define MOTOR4_ENCODER_A 2
+#define MOTOR4_ENCODER_B 3
+
+// 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 12
+  #define MOTOR1_IN_B 13
+
+  #define MOTOR2_PWM -1 //DON'T TOUCH THIS! This is just a placeholder
+  #define MOTOR2_IN_A 14
+  #define MOTOR2_IN_B 15
+
+  #define MOTOR3_PWM -1 //DON'T TOUCH THIS! This is just a placeholder
+  #define MOTOR3_IN_A 16
+  #define MOTOR3_IN_B 17
+
+  #define MOTOR4_PWM -1 //DON'T TOUCH THIS! This is just a placeholder
+  #define MOTOR4_IN_A 18
+  #define MOTOR4_IN_B 19
+
+  #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 "pico"
+#define APP_NAME "hardware"
+// #define USE_LIDAR_UDP
+#define LIDAR_RXD 1
+// #define LIDAR_PWM 0
+#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 4 // specify I2C pins
+#define SCL_PIN 5
+#define NODE_NAME "pico"
+// #define TOPIC_PREFIX "pico/"
+
+// battery voltage ADC pin
+#define BATTERY_PIN 28
+// 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 { \
+    Wire.setSDA(SDA_PIN); \
+    Wire.setSCL(SCL_PIN); \
+    Wire.begin(); \
+    Wire.setClock(400000); \
+}
+// #define BOARD_INIT_LATE {}
+
+#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,7 +1,7 @@
 
 #include "encoder.h"
 
-#if !defined(ESP32)
+#if !defined(ESP32) && !defined(PICO)
 // 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!

firmware/lib/encoder/encoder.h

@@ -28,9 +28,9 @@
 
 #ifndef Encoder_h_
 #define Encoder_h_
+#include <Arduino.h>
 
 #ifdef ESP32
-#include "Arduino.h"
 #include <ESP32Encoder.h>
 class Encoder
 {
@@ -49,8 +49,7 @@ class Encoder
 			pin2 = temp_pin;
 		}
 		counts_per_rev_ = counts_per_rev;
-		ESP32Encoder::useInternalWeakPullResistors = UP;
-		encoder_.attachHalfQuad(pin1, pin2);
+				encoder_.attachHalfQuad(pin1, pin2);
 	}
 	float getRPM() {
 	        if (counts_per_rev_ < 0) return 0.0;
@@ -78,6 +77,51 @@ class Encoder
 	        encoder_.setCount(p);
 	}
 };
+#elif defined(PICO)
+#include <pio_encoder.h>
+
+class Encoder
+{
+private:
+	int counts_per_rev_ = -1;
+	unsigned long prev_update_time_;
+    int32_t prev_encoder_ticks_;
+    uint offset_;
+  	PioEncoder pioencoder_;
+public:
+	Encoder(int pin1, int pin2, int counts_per_rev, bool invert = false) : \
+		pioencoder_(pin1) {
+		if (pin1 < 0 || pin2 < 0) return; // unused encoder
+		pioencoder_.begin();
+		pioencoder_.flip(invert);
+		counts_per_rev_ = counts_per_rev;
+ 	}
+	float getRPM() {
+	        if (counts_per_rev_ < 0) return 0.0;
+		int32_t encoder_ticks = read();
+		//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 pioencoder_.getCount();
+	}
+    inline void write(int32_t p) {
+		if (counts_per_rev_ < 0) return;
+		pioencoder_.reset();
+	}
+};
 #else
 
 #if defined(ARDUINO) && ARDUINO >= 100

firmware/lib/motor/default_motor.h

@@ -18,14 +18,25 @@
 #include <Arduino.h>
 #ifdef ESP32
 #include <ESP32Servo.h>
+#else
+#include <Servo.h>
+#endif
+#include "config.h"
+#ifdef ESP32
 inline void analogWriteFrequency(uint8_t pin, double frequency)
 {
   analogWriteFrequency(frequency);
 }
-#else
-#include <Servo.h> 
+#elif defined(PICO)
+inline void analogWriteFrequency(double frequency)
+{
+  analogWriteFreq(frequency);
+}
+inline void analogWriteFrequency(uint8_t pin, double frequency)
+{
+  analogWriteFreq(frequency);
+}
 #endif
-#include "config.h"
 
 #include "motor_interface.h"
 

firmware/platformio.ini

@@ -81,3 +81,22 @@ build_flags =
     -I ../config
     -D __PGMSPACE_H_
     -D USE_ESP32_CONFIG
+
+[env:pico]
+platform = https://github.com/maxgerhardt/platform-raspberrypi.git
+board = rpipico
+monitor_speed = 115200
+monitor_port = /dev/ttyACM0
+upload_port = /dev/ttyACM0
+upload_protocol = picotool
+; upload_port = 192.168.1.101
+; board_microros_transport = wifi
+; board_microros_distro = humble
+lib_deps =
+    ${env.lib_deps}
+    https://github.com/gbr1/rp2040-encoder-library.git
+build_flags =
+    -I ../config
+    -D PICO
+    -D USE_PICO_CONFIG
+