From 6e74f3f4eb2bba6f5b30c820859d03d4dc1d070b Mon Sep 17 00:00:00 2001 From: Thomas Chou Date: Mon, 11 Mar 2024 20:56:16 -0700 Subject: [PATCH] board: add pico suuport Add pico support as a custom configuration. Signed-off-by: Thomas Chou --- config/config.h | 4 + config/custom/pico_config.h | 240 +++++++++++++++++++++++++++++ firmware/lib/encoder/encoder.cpp | 2 +- firmware/lib/encoder/encoder.h | 50 +++++- firmware/lib/motor/default_motor.h | 17 +- firmware/platformio.ini | 19 +++ 6 files changed, 325 insertions(+), 7 deletions(-) create mode 100644 config/custom/pico_config.h diff --git a/config/config.h b/config/config.h index 89ac79f7..f0dccf7f 100644 --- a/config/config.h +++ b/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" diff --git a/config/custom/pico_config.h b/config/custom/pico_config.h new file mode 100644 index 00000000..868992fa --- /dev/null +++ b/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 diff --git a/firmware/lib/encoder/encoder.cpp b/firmware/lib/encoder/encoder.cpp index 1fc47c08..941f470f 100644 --- a/firmware/lib/encoder/encoder.cpp +++ b/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! diff --git a/firmware/lib/encoder/encoder.h b/firmware/lib/encoder/encoder.h index aa8f7a7b..e65922a6 100644 --- a/firmware/lib/encoder/encoder.h +++ b/firmware/lib/encoder/encoder.h @@ -28,9 +28,9 @@ #ifndef Encoder_h_ #define Encoder_h_ +#include #ifdef ESP32 -#include "Arduino.h" #include 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 + +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 diff --git a/firmware/lib/motor/default_motor.h b/firmware/lib/motor/default_motor.h index 8697a2da..26dd7577 100644 --- a/firmware/lib/motor/default_motor.h +++ b/firmware/lib/motor/default_motor.h @@ -18,14 +18,25 @@ #include #ifdef ESP32 #include +#else +#include +#endif +#include "config.h" +#ifdef ESP32 inline void analogWriteFrequency(uint8_t pin, double frequency) { analogWriteFrequency(frequency); } -#else -#include +#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" diff --git a/firmware/platformio.ini b/firmware/platformio.ini index dc41496f..510a235a 100644 --- a/firmware/platformio.ini +++ b/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 +