Something went wrong. Try again.
Forked pico-rectangle to make it compliant with https://github.com/CarVac/MeleeConchRuleset/blob/main/ruleset.md
Something went wrong. Try again.
4.1 kB · 122 lines
C++
at master
123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123#include "communication_protocols/joybus.hpp"
#include "global.hpp"
#include "hardware/gpio.h"
#include "hardware/pio.h"#include "my_pio.pio.h"
// PIO Shifts to the right by default// In: pushes batches of 8 shifted left, i.e we get [0x40, 0x03, rumble (the end bit is never pushed)]// Out: We push commands for a right shift with an enable pin, ie 5 (101) would be 0b11'10'11// So in doesn't need post processing but out doesvoid convertToPio(const uint8_t* command, const int len, uint32_t* result, int& resultLen) { if (len == 0) { resultLen = 0; return; } resultLen = len/2 + 1; int i; for (i = 0; i < resultLen; i++) { result[i] = 0; } for (i = 0; i < len; i++) { for (int j = 0; j < 8; j++) { result[i / 2] += 1 << (2 * (8 * (i % 2) + j) + 1); result[i / 2] += (!!(command[i] & (0x80u >> j))) << (2 * (8 * (i % 2) + j)); } } // End bit result[len / 2] += 3 << (2 * (8 * (len % 2)));}
namespace CommunicationProtocols{namespace Joybus{
void enterMode(int dataPin, std::function<GCReport()> func) { gpio_init(dataPin); gpio_set_dir(dataPin, GPIO_IN); gpio_pull_up(dataPin);
gpio_init(rumblePin); gpio_set_dir(rumblePin, GPIO_OUT);
sleep_us(100); // Stabilize voltages
PIO pio = pio0; pio_gpio_init(pio, dataPin); uint offset = pio_add_program(pio, &save_program);
pio_sm_config config = save_program_get_default_config(offset); sm_config_set_in_pins(&config, dataPin); sm_config_set_out_pins(&config, dataPin, 1); sm_config_set_set_pins(&config, dataPin, 1); sm_config_set_clkdiv(&config, 5); sm_config_set_out_shift(&config, true, false, 32); sm_config_set_in_shift(&config, false, true, 8); pio_sm_init(pio, 0, offset, &config); pio_sm_set_enabled(pio, 0, true); while (true) { uint8_t buffer[3]; buffer[0] = pio_sm_get_blocking(pio, 0);
if (buffer[0] == 0) { // Probe uint8_t probeResponse[3] = { 0x09, 0x00, 0x03 }; uint32_t result[2]; int resultLen; convertToPio(probeResponse, 3, result, resultLen); sleep_us(6); // 3.75us into the bit before end bit => 6.25 to wait if the end-bit is 5us long
pio_sm_set_enabled(pio, 0, false); pio_sm_init(pio, 0, offset+save_offset_outmode, &config); pio_sm_set_enabled(pio, 0, true);
for (int i = 0; i<resultLen; i++) pio_sm_put_blocking(pio, 0, result[i]); } else if (buffer[0] == 0x41) { // Origin (NOT 0x81) gpio_put(25, 1); uint8_t originResponse[10] = { 0x00, 0x80, 128, 128, 128, 128, 0, 0, 0, 0 }; uint32_t result[6]; int resultLen; convertToPio(originResponse, 10, result, resultLen); // Here we don't wait because convertToPio takes time
pio_sm_set_enabled(pio, 0, false); pio_sm_init(pio, 0, offset+save_offset_outmode, &config); pio_sm_set_enabled(pio, 0, true);
for (int i = 0; i<resultLen; i++) pio_sm_put_blocking(pio, 0, result[i]); } else if (buffer[0] == 0x40) { // Maybe poll //TODO Check later inputs... buffer[0] = pio_sm_get_blocking(pio, 0); buffer[0] = pio_sm_get_blocking(pio, 0); gpio_put(rumblePin, buffer[0] & 1);
GCReport gcReport = func();
uint32_t result[5]; int resultLen; convertToPio((uint8_t*)(&gcReport), 8, result, resultLen);
pio_sm_set_enabled(pio, 0, false); pio_sm_init(pio, 0, offset+save_offset_outmode, &config); pio_sm_set_enabled(pio, 0, true);
for (int i = 0; i<resultLen; i++) pio_sm_put_blocking(pio, 0, result[i]); } else { pio_sm_set_enabled(pio, 0, false); sleep_us(400); pio_sm_init(pio, 0, offset+save_offset_inmode, &config); pio_sm_set_enabled(pio, 0, true); } }}
}}