Skip to content

Instantly share code, notes, and snippets.

@idriszmy

idriszmy/SumoRobot500gRC_Pico.ino

Last active August 23, 2024 15:07
Show Gist options
  • Save idriszmy/900d8eefe6d6782d2e44897f877f3a53 to your computer and use it in GitHub Desktop.
Save idriszmy/900d8eefe6d6782d2e44897f877f3a53 to your computer and use it in GitHub Desktop.
Download ZIP
This example is for controlling sumo robot 500g RC using Raspberry Pi Pico and wireless USB joystick.
Raw
joystick_data.h
/*
This is a few types of wireless USB joystick data.
AUTHOR : Idris Zainal Abidin
COMPANY : Cytron Technologies Sdn Bhd
WEBSITE : https://my.cytron.io?tracking=idris
TELEGRAM : https://t.me/idriszmy
*/
#ifndef JOYSTICK_DATA_H
#define JOYSTICK_DATA_H
#ifdef JOYSTICK_1
#define DPAD_INDEX 2
#define AN_LX_INDEX 2
#define AN_LY_INDEX 4
#define AN_RX_INDEX 5
#define AN_RY_INDEX 6
#define R2_INDEX 1
#define R2 0x02
#endif
#ifdef JOYSTICK_2
#define DPAD_INDEX 2
#define AN_LX_INDEX 3
#define AN_LY_INDEX 4
#define AN_RX_INDEX 5
#define AN_RY_INDEX 6
#define R2_INDEX 0
#define R2 0x80
#endif
#ifdef JOYSTICK_3
#define DPAD_INDEX 5
#define AN_LX_INDEX 3
#define AN_LY_INDEX 4
#define AN_RX_INDEX 1
#define AN_RY_INDEX 2
#define R2_INDEX 6
#define R2 0x08
#endif
#define DPAD_UP 0x00
#define DPAD_UP_RIGHT 0x01
#define DPAD_RIGHT 0x02
#define DPAD_DOWN_RIGHT 0x03
#define DPAD_DOWN 0x04
#define DPAD_DOWN_LEFT 0x05
#define DPAD_LEFT 0x06
#define DPAD_UP_LEFT 0x07
#define DPAD_RELEASE 0x0F
#define AN_RELEASE 0x80
#endif
Raw
SumoRobot500gRC_Pico.ino
/*********************************************************************
Adafruit invests time and resources providing this open source code,
please support Adafruit and open-source hardware by purchasing
products from Adafruit!
MIT license, check LICENSE for more information
Copyright (c) 2019 Ha Thach for Adafruit Industries
All text above, and the splash screen below must be included in
any redistribution
*********************************************************************/
/*
This example is for controlling sumo robot 500g RC using wireless USB joystick.
SETTINGS
Board: Raspberry Pi Pico
CPU Speed: 120 MHz
USB Stack: Adafruit TinyUSB
AUTHOR : Idris Zainal Abidin
COMPANY : Cytron Technologies Sdn Bhd
WEBSITE : https://my.cytron.io?tracking=idris
TELEGRAM : https://t.me/idriszmy
*/
#define JOYSTICK_2
// USBHost is defined in usbh_helper.h
#include "usbh_helper.h"
#include "joystick_data.h"
#include <CytronMotorDriver.h>
// Configure the motor driver.
CytronMD motor1(PWM_PWM, 18, 19); // PWM 1A = Pin 3, PWM 1B = Pin 9
CytronMD motor2(PWM_PWM, 20, 21); // PWM 2A = Pin 10, PWM 2B = Pin 11
int soft_acceleration = 3; // 1 slower, 4 faster
uint8_t gamepad_data[30];
bool gamepad_pressed = false;
int LED = 25;
//------------- Core0 -------------//
void setup() {
pinMode(LED, OUTPUT);
digitalWrite(LED, HIGH);
motor1.setSpeed(0); // Motor 1 stop
motor2.setSpeed(0); // Motor 2 stop
Serial.begin(115200);
//while (!Serial) delay(10); // wait for native usb
Serial.println("TinyUSB Dual: HID Device Report Example");
delay(2000);
}
int MAX_SPEED = 255;
signed int speed = 0;
signed int speed_left = 0;
signed int speed_right = 0;
signed int speed_left_target = 0;
signed int speed_right_target = 0;
void loop() {
Serial.flush();
if (gamepad_data[R2_INDEX] & R2) {
speed = MAX_SPEED;
}
else {
speed = MAX_SPEED * 0.5;
}
if (gamepad_data[DPAD_INDEX] == DPAD_RELEASE) {
int speed_percent = map(gamepad_data[AN_LY_INDEX], 0, 255, 100, -100);
int steering_percent = map(gamepad_data[AN_RX_INDEX], 0, 255, -100, 100);
// Mix speed and steering together for left and right motor speed.
float speed_left_percent = (speed_percent + steering_percent) / 100.0f;
speed_left_target = (int)(speed_left_percent * speed);
speed_left_target = constrain(speed_left_target, -speed, speed);
float speed_right_percent = (speed_percent - steering_percent) / 100.0f;
speed_right_target = (int)(speed_right_percent * speed);
speed_right_target = constrain(speed_right_target, -speed, speed);
}
else if (gamepad_data[DPAD_INDEX] == DPAD_UP) {
speed_left_target = speed;
speed_right_target = speed;
}
else if (gamepad_data[DPAD_INDEX] == DPAD_UP_RIGHT) {
speed_left_target = speed;
speed_right_target = 0;
}
else if (gamepad_data[DPAD_INDEX] == DPAD_RIGHT) {
speed_left_target = speed;
speed_right_target = -speed;
}
else if (gamepad_data[DPAD_INDEX] == DPAD_DOWN_RIGHT) {
speed_left_target = -speed;
speed_right_target = 0;
}
else if (gamepad_data[DPAD_INDEX] == DPAD_DOWN) {
speed_left_target = -speed;
speed_right_target = -speed;
}
else if (gamepad_data[DPAD_INDEX] == DPAD_DOWN_LEFT) {
speed_left_target = 0;
speed_right_target = -speed;
}
else if (gamepad_data[DPAD_INDEX] == DPAD_LEFT) {
speed_left_target = -speed;
speed_right_target = speed;
}
else if (gamepad_data[DPAD_INDEX] == DPAD_UP_LEFT) {
speed_left_target = 0;
speed_right_target = speed;
}
if (speed_left < speed_left_target) {
speed_left += soft_acceleration;
if (speed_left > speed_left_target) {
speed_left = speed_left_target;
}
delay(1);
}
else if (speed_left > speed_left_target) {
speed_left -= soft_acceleration;
if (speed_left < speed_left_target) {
speed_left = speed_left_target;
}
delay(1);
}
if (speed_right < speed_right_target) {
speed_right += soft_acceleration;
if (speed_right > speed_right_target) {
speed_right = speed_right_target;
}
delay(1);
}
else if (speed_right > speed_right_target) {
speed_right -= soft_acceleration;
if (speed_right < speed_right_target) {
speed_right = speed_right_target;
}
delay(1);
}
//Serial.print(speed_left);
//Serial.print('\t');
//Serial.println(speed_right);
//delay(1);
motor1.setSpeed(speed_left); // Motor 1 move forward
motor2.setSpeed(speed_right); // Motor 2 move forward
if (speed_left || speed_right) {
digitalWrite(LED, HIGH);
}
else if (!speed_left && !speed_right) {
digitalWrite(LED, LOW);
}
}
//------------- Core1 -------------//
void setup1() {
// configure pio-usb: defined in usbh_helper.h
rp2040_configure_pio_usb();
// run host stack on controller (rhport) 1
// Note: For rp2040 pico-pio-usb, calling USBHost.begin() on core1 will have most of the
// host bit-banging processing works done in core1 to free up core0 for other works
USBHost.begin(1);
}
void loop1() {
USBHost.task();
}
extern "C" {
// Invoked when device with hid interface is mounted
// Report descriptor is also available for use.
// tuh_hid_parse_report_descriptor() can be used to parse common/simple enough
// descriptor. Note: if report descriptor length > CFG_TUH_ENUMERATION_BUFSIZE,
// it will be skipped therefore report_desc = NULL, desc_len = 0
void tuh_hid_mount_cb(uint8_t dev_addr, uint8_t instance, uint8_t const *desc_report, uint16_t desc_len) {
(void) desc_report;
(void) desc_len;
uint16_t vid, pid;
tuh_vid_pid_get(dev_addr, &vid, &pid);
Serial.printf("HID device address = %d, instance = %d is mounted\r\n", dev_addr, instance);
Serial.printf("VID = %04x, PID = %04x\r\n", vid, pid);
if (!tuh_hid_receive_report(dev_addr, instance)) {
Serial.printf("Error: cannot request to receive report\r\n");
}
}
// Invoked when device with hid interface is un-mounted
void tuh_hid_umount_cb(uint8_t dev_addr, uint8_t instance) {
Serial.printf("HID device address = %d, instance = %d is unmounted\r\n", dev_addr, instance);
}
// Invoked when received report from device via interrupt endpoint
void tuh_hid_report_received_cb(uint8_t dev_addr, uint8_t instance, uint8_t const *report, uint16_t len) {
if (len > 4) {
#if defined(JOYSTICK_1)
for (uint16_t i = 0; i < len; i++) {
if (report[i] != gamepad_data[i]) {
#elif defined(JOYSTICK_2)
for (uint16_t i = 0; i < len-20; i++) {
if (report[i] != gamepad_data[i]) {
#elif defined(JOYSTICK_3)
for (uint16_t i = 0; i < len; i++) {
if (report[0] == 1 && report[i] != gamepad_data[i]) {
#endif
gamepad_pressed = true;
break;
}
}
if (gamepad_pressed == true) {
for (uint8_t i = 0; i < len; i++) {
gamepad_data[i] = report[i];
//Serial.printf("0x%02X ", gamepad_data[i]);
}
//Serial.println();
gamepad_pressed = false;
}
}
// continue to request to receive report
if (!tuh_hid_receive_report(dev_addr, instance)) {
Serial.printf("Error: cannot request to receive report\r\n");
}
}
} // extern C
Raw
usbh_helper.h
/*********************************************************************
Adafruit invests time and resources providing this open source code,
please support Adafruit and open-source hardware by purchasing
products from Adafruit!
MIT license, check LICENSE for more information
Copyright (c) 2019 Ha Thach for Adafruit Industries
All text above, and the splash screen below must be included in
any redistribution
*********************************************************************/
#ifndef USBH_HELPER_H
#define USBH_HELPER_H
#ifdef ARDUINO_ARCH_RP2040
// pio-usb is required for rp2040 host
#include "pio_usb.h"
// Pin D+ for host, D- = D+ + 1
#ifndef PIN_USB_HOST_DP
#define PIN_USB_HOST_DP 2
#endif
// Pin for enabling Host VBUS. comment out if not used
#ifndef PIN_5V_EN
#define PIN_5V_EN 18
#endif
#ifndef PIN_5V_EN_STATE
#define PIN_5V_EN_STATE 1
#endif
#endif // ARDUINO_ARCH_RP2040
#include "Adafruit_TinyUSB.h"
#if defined(CFG_TUH_MAX3421) && CFG_TUH_MAX3421
// USB Host using MAX3421E: SPI, CS, INT
#include "SPI.h"
#if defined(ARDUINO_METRO_ESP32S2)
Adafruit_USBH_Host USBHost(&SPI, 15, 14);
#else
// Default use SPI and pin 10, 9 for CS and INT
Adafruit_USBH_Host USBHost(&SPI, 10, 9);
#endif
#else
// Native USB Host such as rp2040
Adafruit_USBH_Host USBHost;
#endif
//--------------------------------------------------------------------+
// Helper Functions
//--------------------------------------------------------------------+
#ifdef ARDUINO_ARCH_RP2040
static void rp2040_configure_pio_usb(void) {
//while ( !Serial ) delay(10); // wait for native usb
Serial.println("Core1 setup to run TinyUSB host with pio-usb");
// Check for CPU frequency, must be multiple of 120Mhz for bit-banging USB
uint32_t cpu_hz = clock_get_hz(clk_sys);
if (cpu_hz != 120000000UL && cpu_hz != 240000000UL) {
while (!Serial) {
delay(10); // wait for native usb
}
Serial.printf("Error: CPU Clock = %lu, PIO USB require CPU clock must be multiple of 120 Mhz\r\n", cpu_hz);
Serial.printf("Change your CPU Clock to either 120 or 240 Mhz in Menu->CPU Speed \r\n");
while (1) {
delay(1);
}
}
#ifdef PIN_5V_EN
pinMode(PIN_5V_EN, OUTPUT);
digitalWrite(PIN_5V_EN, PIN_5V_EN_STATE);
#endif
pio_usb_configuration_t pio_cfg = PIO_USB_DEFAULT_CONFIG;
pio_cfg.pin_dp = PIN_USB_HOST_DP;
#if defined(ARDUINO_RASPBERRY_PI_PICO_W)
// For pico-w, PIO is also used to communicate with cyw43
// Therefore we need to alternate the pio-usb configuration
// details https://github.com/sekigon-gonnoc/Pico-PIO-USB/issues/46
pio_cfg.sm_tx = 3;
pio_cfg.sm_rx = 2;
pio_cfg.sm_eop = 3;
pio_cfg.pio_rx_num = 0;
pio_cfg.pio_tx_num = 1;
pio_cfg.tx_ch = 9;
#endif
USBHost.configure_pio_usb(1, &pio_cfg);
}
#endif
#endif
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment