Compare commits
No commits in common. 'master' and 'bluepill' have entirely different histories.
@ -1,9 +1 @@
|
||||
.pio/
|
||||
.pioenvs/
|
||||
.vscode/
|
||||
hoverserial.code-workspace
|
||||
.piolibdeps/
|
||||
hoverserial.ino
|
||||
build_opt.h
|
||||
.gcc-flags.json
|
||||
.clang_complete
|
||||
build/*
|
||||
@ -0,0 +1,7 @@
|
||||
{
|
||||
"board": "STM32:stm32:GenF1",
|
||||
"configuration": "pnum=BLUEPILL_F103C8,upload_method=swdMethod,xserial=generic,usb=CDCgen,xusb=FS,opt=osstd,rtlib=nano",
|
||||
"port": "/dev/ttyACM0",
|
||||
"sketch": "hoverserial.ino",
|
||||
"output": "./build"
|
||||
}
|
||||
@ -0,0 +1,20 @@
|
||||
{
|
||||
"configurations": [
|
||||
{
|
||||
"name": "Linux",
|
||||
"includePath": [
|
||||
"/home/jerome/Arduino/libraries/**",
|
||||
"/home/jerome/.arduino15/packages/STM32/tools/**",
|
||||
"/home/jerome/.arduino15/packages/STM32/hardware/stm32/1.8.0/**"
|
||||
],
|
||||
"forcedInclude": [
|
||||
"/home/jerome/.arduino15/packages/STM32/hardware/stm32/1.8.0/cores/arduino/Arduino.h"
|
||||
],
|
||||
"intelliSenseMode": "gcc-x64",
|
||||
"compilerPath": "/usr/bin/clang",
|
||||
"cStandard": "c11",
|
||||
"cppStandard": "c++17"
|
||||
}
|
||||
],
|
||||
"version": 4
|
||||
}
|
||||
@ -0,0 +1 @@
|
||||
-DENABLE_HWSERIAL1
|
||||
@ -0,0 +1,8 @@
|
||||
{
|
||||
"folders": [
|
||||
{
|
||||
"path": "."
|
||||
}
|
||||
],
|
||||
"settings": {}
|
||||
}
|
||||
@ -0,0 +1,369 @@
|
||||
#include <Arduino.h>
|
||||
#include "splash-h.h"
|
||||
#include <Adafruit_GFX.h>
|
||||
#include <Adafruit_SSD1306.h>
|
||||
#include <Adafruit_NeoPixel.h>
|
||||
#include "leds_config.h"
|
||||
|
||||
// Declare our NeoPixel strip object:
|
||||
Adafruit_NeoPixel strip(LED_STRIP *NB_STRIP, PIN, NEO_GRB + NEO_KHZ800);
|
||||
// Argument 1 = Number of pixels in NeoPixel strip
|
||||
// Argument 2 = Arduino pin number (most are valid)
|
||||
// Argument 3 = Pixel type flags, add together as needed:
|
||||
// NEO_KHZ800 800 KHz bitstream (most NeoPixel products w/WS2812 LEDs)
|
||||
// NEO_KHZ400 400 KHz (classic 'v1' (not v2) FLORA pixels, WS2811 drivers)
|
||||
// NEO_GRB Pixels are wired for GRB bitstream (most NeoPixel products)
|
||||
// NEO_RGB Pixels are wired for RGB bitstream (v1 FLORA pixels, not v2)
|
||||
// NEO_RGBW Pixels are wired for RGBW bitstream (NeoPixel RGBW products)
|
||||
|
||||
#include <NintendoExtensionCtrl.h> // https://github.com/dmadison/NintendoExtensionCtrl
|
||||
|
||||
#include <Wire.h>
|
||||
// second I2C on PB11 (SDA) PB10 (SCL)
|
||||
TwoWire Wire2(PB11, PB10);
|
||||
// Connect to a Nunchuk
|
||||
Nunchuk nchuk(Wire2);
|
||||
|
||||
// ########################## DEFINES ##########################
|
||||
#define LED_BUILTIN PC13
|
||||
#define TIME_SEND 100 // [ms] time interval
|
||||
#define HOVER_SERIAL_BAUD 38400 // [-] Baud rate for HoverSerial (used to communicate with the hoverboard)
|
||||
#define START_FRAME 0xAAAA // [-] Start frme definition for reliable serial communication
|
||||
#define SCREEN_WIDTH 128 // OLED display width, in pixels
|
||||
#define SCREEN_HEIGHT 64 // OLED display height, in pixels
|
||||
|
||||
// Declaration for an SSD1306 display connected to I2C2 (SDA, SCL pins)
|
||||
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);
|
||||
|
||||
// Global variables
|
||||
uint8_t idx = 0; // Index for new data pointer
|
||||
uint16_t bufStartFrame; // Buffer Start Frame
|
||||
byte *p; // Pointer declaration for the new received data
|
||||
byte incomingByte;
|
||||
byte incomingBytePrev;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
uint16_t start;
|
||||
int16_t steer;
|
||||
int16_t speed;
|
||||
uint16_t checksum;
|
||||
} SerialCommand;
|
||||
SerialCommand Command;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
uint16_t start;
|
||||
int16_t cmd1;
|
||||
int16_t cmd2;
|
||||
int16_t speedR;
|
||||
int16_t speedL;
|
||||
int16_t speedR_meas;
|
||||
int16_t speedL_meas;
|
||||
int16_t batVoltage;
|
||||
int16_t boardTemp;
|
||||
int16_t checksum;
|
||||
} SerialFeedback;
|
||||
SerialFeedback Feedback;
|
||||
SerialFeedback NewFeedback;
|
||||
|
||||
int cmd1; // normalized input values. -1000 to 1000
|
||||
int cmd2;
|
||||
int idx_ledR = 0, idx_ledL = 0;
|
||||
|
||||
// ########################## colorWipe ##########################
|
||||
// Fill strip pixels one after another with a color. Strip is NOT cleared
|
||||
// first; anything there will be covered pixel by pixel. Pass in color
|
||||
// (as a single 'packed' 32-bit value, which you can get by calling
|
||||
// strip.Color(red, green, blue) as shown in the loop() function above),
|
||||
// and a delay time (in milliseconds) between pixels.
|
||||
void colorWipe(uint32_t color, int wait)
|
||||
{
|
||||
for (int i = 0; i < strip.numPixels(); i++)
|
||||
{ // For each pixel in strip...
|
||||
strip.setPixelColor(i, color); // Set pixel's color (in RAM)
|
||||
strip.show(); // Update strip to match
|
||||
delay(wait); // Pause for a moment
|
||||
}
|
||||
}
|
||||
|
||||
// Serial1 PA_9 TX PA_10 RX
|
||||
// ########################## SETUP ##########################
|
||||
void setup()
|
||||
{
|
||||
Wire.setSCL(PB_8);
|
||||
Wire.setSDA(PB_9);
|
||||
// Wire2.setSCL(PB_10);
|
||||
// Wire2.setSDA(PB_11);
|
||||
|
||||
nchuk.begin();
|
||||
|
||||
display.begin(SSD1306_SWITCHCAPVCC, 0x3C); // initialize with the I2C addr 0x3C (for the 128x64)
|
||||
display.clearDisplay();
|
||||
display.drawBitmap((SCREEN_WIDTH - my_splash_width) / 2, (SCREEN_HEIGHT - my_splash_height) / 2,
|
||||
my_splash_data, my_splash_width, my_splash_height, 1);
|
||||
display.display();
|
||||
|
||||
Serial.begin(115200);
|
||||
Serial1.begin(HOVER_SERIAL_BAUD);
|
||||
|
||||
strip.begin(); // INITIALIZE NeoPixel strip object
|
||||
strip.clear();
|
||||
strip.show(); // Turn OFF all pixels ASAP
|
||||
strip.setBrightness(50); // Set BRIGHTNESS to about 1/5 (max = 255)
|
||||
|
||||
while (!Serial)
|
||||
;
|
||||
pinMode(LED_BUILTIN, OUTPUT);
|
||||
Serial.println("STM32 hoverboard control");
|
||||
display.clearDisplay();
|
||||
display.setTextSize(1); // Draw 1X-scale text
|
||||
display.setTextColor(SSD1306_WHITE);
|
||||
display.setCursor(int((display.width() -
|
||||
(strlen("Hoverboard Control") * 6)) /
|
||||
2),
|
||||
0);
|
||||
display.println("Hoverboard Control");
|
||||
display.setCursor(int((display.width() - (strlen("v STM32 0.2") * 6)) / 2), 8);
|
||||
display.println("v STM32 0.2");
|
||||
|
||||
// Fill along the length of the strip in various colors...
|
||||
|
||||
colorWipe(strip.Color(255, 0, 0), 20); // Red
|
||||
colorWipe(strip.Color(0, 255, 0), 20); // Green
|
||||
colorWipe(strip.Color(0, 0, 255), 20); // Blue
|
||||
colorWipe(strip.Color(0, 0, 0), 20); // Black
|
||||
}
|
||||
// ########################## SEND ##########################
|
||||
void Send(int16_t uSteer, int16_t uSpeed)
|
||||
{
|
||||
// Create command
|
||||
Command.start = (uint16_t)START_FRAME;
|
||||
Command.steer = (int16_t)uSteer;
|
||||
Command.speed = (int16_t)uSpeed;
|
||||
Command.checksum =
|
||||
(uint16_t)(Command.start ^ Command.steer ^ Command.speed);
|
||||
|
||||
// Write to Serial
|
||||
Serial1.write((uint8_t *)&Command, sizeof(Command));
|
||||
}
|
||||
// ########################## RECEIVE ##########################
|
||||
void Receive()
|
||||
|
||||
{
|
||||
int old_cursorX, old_cursorY;
|
||||
// Check for new data availability in the Serial buffer
|
||||
if (Serial1.available())
|
||||
{
|
||||
incomingByte = Serial1.read(); // Read the incoming byte
|
||||
bufStartFrame = ((uint16_t)(incomingBytePrev) << 8) + incomingByte; // Construct the start frame
|
||||
}
|
||||
else
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
// If DEBUG_RX is defined print all incoming bytes
|
||||
#ifdef DEBUG_RX
|
||||
Serial.print(incomingByte);
|
||||
return;
|
||||
#endif
|
||||
|
||||
// Copy received data
|
||||
if (bufStartFrame == START_FRAME)
|
||||
{ // Initialize if new data is detected
|
||||
p = (byte *)&NewFeedback;
|
||||
*p++ = incomingBytePrev;
|
||||
*p++ = incomingByte;
|
||||
idx = 2;
|
||||
}
|
||||
else if (idx >= 2 && idx < sizeof(SerialFeedback))
|
||||
{ // Save the new received data
|
||||
*p++ = incomingByte;
|
||||
idx++;
|
||||
}
|
||||
// Check if we reached the end of the package
|
||||
if (idx == sizeof(SerialFeedback))
|
||||
{
|
||||
uint16_t checksum;
|
||||
checksum =
|
||||
(uint16_t)(NewFeedback.start ^ NewFeedback.cmd1 ^ NewFeedback.cmd2 ^ NewFeedback.speedR ^ NewFeedback.speedL ^ NewFeedback.speedR_meas ^ NewFeedback.speedL_meas ^ NewFeedback.batVoltage ^ NewFeedback.boardTemp);
|
||||
|
||||
// Check validity of the new data
|
||||
if (NewFeedback.start == START_FRAME && checksum == NewFeedback.checksum)
|
||||
{
|
||||
// Copy the new data
|
||||
memcpy(&Feedback, &NewFeedback, sizeof(SerialFeedback));
|
||||
|
||||
// Print data to CDC Serial if available
|
||||
if (Serial)
|
||||
{
|
||||
Serial.print("1: ");
|
||||
Serial.print(Feedback.cmd1);
|
||||
Serial.print(" 2: ");
|
||||
Serial.print(Feedback.cmd2);
|
||||
Serial.print(" 3: ");
|
||||
Serial.print(Feedback.speedR);
|
||||
Serial.print(" 4: ");
|
||||
Serial.print(Feedback.speedL);
|
||||
Serial.print(" 5: ");
|
||||
Serial.print(Feedback.speedR_meas);
|
||||
Serial.print(" 6: ");
|
||||
Serial.print(Feedback.speedL_meas);
|
||||
Serial.print(" 7: ");
|
||||
Serial.print(Feedback.batVoltage);
|
||||
Serial.print(" 8: ");
|
||||
Serial.println(Feedback.boardTemp);
|
||||
}
|
||||
display.setCursor(0, 40);
|
||||
display.setTextSize(1); // Draw 1X-scale text
|
||||
display.setTextColor(SSD1306_WHITE);
|
||||
display.print("V : ");
|
||||
old_cursorX = display.getCursorX();
|
||||
old_cursorY = display.getCursorY();
|
||||
display.fillRect(old_cursorX, old_cursorY, (6 * 5), 8, SSD1306_BLACK); // erase previous display
|
||||
display.setCursor(old_cursorX, old_cursorY);
|
||||
display.print(Feedback.batVoltage);
|
||||
display.display();
|
||||
}
|
||||
else
|
||||
{
|
||||
if (Serial)
|
||||
Serial.println("Non-valid data skipped");
|
||||
}
|
||||
|
||||
idx = 0; // Reset the index (it prevents to enter in this if condition in the next cycle)
|
||||
}
|
||||
// Update previous states
|
||||
incomingBytePrev = incomingByte;
|
||||
}
|
||||
|
||||
// ########################## Nunchuk_control ##########################
|
||||
|
||||
void Nunchuk_control()
|
||||
{
|
||||
int old_cursorX, old_cursorY;
|
||||
|
||||
if (!nchuk.update())
|
||||
{
|
||||
nchuk.reconnect();
|
||||
cmd1 = 0;
|
||||
cmd2 = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (nchuk.buttonC()) {
|
||||
idx_ledR = 0;
|
||||
idx_ledL = 0;
|
||||
}
|
||||
cmd1 = map(nchuk.joyX(), 0, 256, 150, -150); // x - axis. Nunchuck joystick readings range 150 -150
|
||||
cmd2 = map(nchuk.joyY(), 0, 256, -150, 150); // y - axis
|
||||
}
|
||||
display.setCursor(0, 48);
|
||||
display.setTextSize(1); // Draw 1X-scale text
|
||||
display.setTextColor(SSD1306_WHITE);
|
||||
display.print("X : ");
|
||||
old_cursorX = display.getCursorX();
|
||||
old_cursorY = display.getCursorY();
|
||||
display.fillRect(old_cursorX, old_cursorY, (6 * 5), 8, SSD1306_BLACK); // erase previous display
|
||||
display.setCursor(old_cursorX, old_cursorY);
|
||||
display.print(cmd1);
|
||||
display.setCursor(0, 56);
|
||||
display.setTextSize(1); // Draw 1X-scale text
|
||||
display.setTextColor(SSD1306_WHITE);
|
||||
display.print("Y : ");
|
||||
old_cursorX = display.getCursorX();
|
||||
old_cursorY = display.getCursorY();
|
||||
display.fillRect(old_cursorX, old_cursorY, (6 * 5), 8, SSD1306_BLACK); // erase previous display
|
||||
display.setCursor(old_cursorX, old_cursorY);
|
||||
display.print(cmd2);
|
||||
display.display();
|
||||
}
|
||||
|
||||
// ########################## LOOP ##########################
|
||||
|
||||
unsigned long iTimeSendR = 0, iTimeSendL = 0;
|
||||
int speedR, speedL;
|
||||
|
||||
void loop()
|
||||
{
|
||||
unsigned long timeNow = millis();
|
||||
|
||||
// Blink the LED
|
||||
digitalWrite(LED_BUILTIN, (timeNow % 2000) < 1000);
|
||||
Receive();
|
||||
Nunchuk_control();
|
||||
Send(cmd1, cmd2);
|
||||
// ********************* simulation *********************************
|
||||
// in final project need to use Feedback.speedR_meas and Feedback.speedL_meas
|
||||
//
|
||||
speedR = cmd2 + cmd1;
|
||||
speedL = cmd2 - cmd1;
|
||||
//
|
||||
// ********************* simulation *********************************
|
||||
strip.clear();
|
||||
strip.setPixelColor(idx_ledR, strip.Color(128, 0, 0));
|
||||
strip.setPixelColor((NB_STRIP * LED_STRIP) - 1 - idx_ledL, strip.Color(128, 0, 0)); // second LEDs strip
|
||||
strip.show();
|
||||
|
||||
//*********** 1st led strip ******************
|
||||
if (timeNow > iTimeSendR)
|
||||
{
|
||||
if (speedR != 0)
|
||||
{
|
||||
iTimeSendR = timeNow + ((200 - abs(speedR)) * 2); // TIME_SEND
|
||||
if (speedR > 0)
|
||||
{
|
||||
if (idx_ledR < LED_STRIP - 1)
|
||||
{
|
||||
idx_ledR++;
|
||||
}
|
||||
else
|
||||
{
|
||||
idx_ledR = 0;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (idx_ledR > 0)
|
||||
{
|
||||
idx_ledR--;
|
||||
}
|
||||
else
|
||||
{
|
||||
idx_ledR = LED_STRIP - 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
//*********** 2nd led strip ******************
|
||||
if (timeNow > iTimeSendL)
|
||||
{
|
||||
if (speedL != 0)
|
||||
{
|
||||
iTimeSendL = timeNow + ((200 - abs(speedL)) * 2); // TIME_SEND
|
||||
if (speedL > 0)
|
||||
{
|
||||
if (idx_ledL < LED_STRIP - 1)
|
||||
{
|
||||
idx_ledL++;
|
||||
}
|
||||
else
|
||||
{
|
||||
idx_ledL = 0;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (idx_ledL > 0)
|
||||
{
|
||||
idx_ledL--;
|
||||
}
|
||||
else
|
||||
{
|
||||
idx_ledL = LED_STRIP - 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,5 @@
|
||||
|
||||
#define PIN PB5 // Pin where NeoPixels are connected
|
||||
|
||||
#define LED_STRIP 12 // number of leds per strip
|
||||
#define NB_STRIP 2 // number of strip
|
||||
@ -1,60 +0,0 @@
|
||||
;PlatformIO Project Configuration File
|
||||
;
|
||||
; Build options: build flags, source filter
|
||||
; Upload options: custom upload port, speed and extra flags
|
||||
; Library options: dependencies, extra library storages
|
||||
; Advanced options: extra scripting
|
||||
;
|
||||
; Please visit documentation for the other options and examples
|
||||
; https://docs.platformio.org/page/projectconf.html
|
||||
|
||||
[platformio]
|
||||
; default_envs = leonardo
|
||||
default_envs = stm32-411
|
||||
|
||||
[common_env_data]
|
||||
|
||||
[env:leonardo]
|
||||
board = leonardo
|
||||
framework = arduino
|
||||
platform = atmelavr
|
||||
lib_deps =
|
||||
Nintendo Extension Ctrl
|
||||
monitor_port = /dev/ttyACM*
|
||||
upload_port = /dev/ttyACM*
|
||||
monitor_speed = 115200
|
||||
|
||||
[env:usbasp]
|
||||
platform = atmelavr
|
||||
framework = arduino
|
||||
board = leonardo
|
||||
lib_deps =
|
||||
Nintendo Extension Ctrl
|
||||
upload_protocol = usbasp
|
||||
; each flag in a new line
|
||||
upload_flags =
|
||||
-Pusb
|
||||
|
||||
[env:stm32-411]
|
||||
platform = ststm32
|
||||
framework = arduino
|
||||
board = nucleo_f411re
|
||||
build_flags =
|
||||
-Wno-deprecated
|
||||
lib_deps =
|
||||
Nintendo Extension Ctrl
|
||||
|
||||
debug_tool = stlink
|
||||
|
||||
monitor_port = /dev/ttyACM*
|
||||
monitor_speed = 115200
|
||||
|
||||
[env:stm32]
|
||||
platform = ststm32
|
||||
framework = arduino
|
||||
board = nucleo_l031k6
|
||||
build_flags =
|
||||
-Wno-deprecated
|
||||
lib_deps =
|
||||
Nintendo Extension Ctrl
|
||||
|
||||
@ -0,0 +1,58 @@
|
||||
|
||||
#define my_splash_width 128
|
||||
#define my_splash_height 52
|
||||
|
||||
const uint8_t PROGMEM my_splash_data[] = {
|
||||
B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,
|
||||
B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,
|
||||
B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,
|
||||
B00000000,B00000000,B00000000,B00000000,B00000000,B00000111,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,
|
||||
B00000000,B00000000,B00000000,B00000000,B00000000,B00000111,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,
|
||||
B00000000,B00000000,B00000000,B00000000,B00000000,B00000111,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,
|
||||
B00000000,B00000000,B00000000,B00000000,B00000000,B00000111,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,
|
||||
B00000000,B00000000,B00000000,B00000000,B00000000,B00000111,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,
|
||||
B00000000,B00000000,B00000000,B00000000,B00000000,B00000111,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,
|
||||
B00000000,B00000000,B00000000,B00000000,B00000000,B00000111,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,
|
||||
B00000000,B00000000,B00000000,B00000000,B00000000,B00000111,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,
|
||||
B00000000,B00000000,B00000000,B00000000,B00000000,B00000111,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,
|
||||
B00000011,B11111000,B00000001,B11111100,B01111111,B11111111,B11111111,B11110011,B11111000,B00000001,B11111000,B00001111,B11111000,B00000000,B01111111,B11000000,
|
||||
B00000011,B11111000,B00000001,B11111100,B01111111,B11111111,B11111111,B11110011,B11111000,B00000001,B11111000,B00001111,B11111000,B00000000,B11111111,B11000000,
|
||||
B00000011,B11111000,B00000001,B11111100,B01111111,B11111111,B11111111,B11110011,B11111000,B00000001,B11111000,B00001111,B11111100,B00000000,B11111111,B11000000,
|
||||
B00000011,B11111000,B00000001,B11111100,B01111111,B11111111,B11111111,B11110011,B11111000,B00000001,B11111000,B00001111,B11111100,B00000000,B11111111,B11000000,
|
||||
B00000011,B11111000,B00000001,B11111100,B00000000,B00001111,B10000000,B00000011,B11111000,B00000001,B11111000,B00001111,B11111100,B00000001,B11111111,B11000000,
|
||||
B00000011,B11111000,B00000001,B11111100,B00000000,B00011111,B11000000,B00000011,B11111000,B00000001,B11111000,B00001111,B11111110,B00000001,B11111111,B11000000,
|
||||
B00000011,B11111000,B00000001,B11111100,B00000000,B00011111,B11000000,B00000011,B11111000,B00000001,B11111000,B00001111,B11111110,B00000011,B11111111,B11000000,
|
||||
B00000011,B11111000,B00000001,B11111100,B00000000,B00111111,B11100000,B00000011,B11111000,B00000001,B11111000,B00001111,B11011111,B10000011,B11111111,B11000000,
|
||||
B00000011,B11111000,B00000001,B11111100,B00000000,B00111111,B11100000,B00000011,B11111000,B00000001,B11111000,B00001111,B11011111,B10000111,B11011111,B11000000,
|
||||
B00000011,B11111000,B00000001,B11111100,B00000000,B01111101,B11110000,B00000011,B11111000,B00000001,B11111000,B00001111,B11001111,B11000111,B11011111,B11000000,
|
||||
B00000011,B11111111,B11111111,B11111100,B00000000,B01111000,B11110000,B00000011,B11111000,B00000001,B11111000,B00001111,B11001111,B11000111,B11011111,B11000000,
|
||||
B00000011,B11111111,B11111111,B11111100,B00000000,B11111000,B11111000,B00000011,B11111000,B00000001,B11111000,B00001111,B11001111,B11001111,B10011111,B11000000,
|
||||
B00000011,B11111111,B11111111,B11111100,B00000000,B11111000,B01111000,B00000011,B11111000,B00000001,B11111000,B00001111,B11000111,B11101111,B10011111,B11000000,
|
||||
B00000011,B11111111,B11111111,B11111100,B00000001,B11111000,B01111000,B00000011,B11111000,B00000001,B11111000,B00001111,B11000111,B11101111,B10011111,B11000000,
|
||||
B00000011,B11111111,B11111111,B11111100,B00000001,B11110000,B01111100,B00000011,B11111000,B00000001,B11111000,B00001111,B11000111,B11111111,B00011111,B11000000,
|
||||
B00000011,B11111111,B11111111,B11111100,B00000011,B11110000,B00111100,B00000011,B11111000,B00000001,B11111000,B00001111,B11000011,B11111111,B00011111,B11000000,
|
||||
B00000011,B11111000,B00000001,B11111100,B00000011,B11100000,B00111110,B00000011,B11111000,B00000001,B11111000,B00001111,B11000011,B11111110,B00011111,B11000000,
|
||||
B00000011,B11111000,B00000001,B11111100,B00000111,B11100000,B00011110,B00000001,B11111000,B00000001,B11111000,B00001111,B11000001,B11111110,B00011111,B11000000,
|
||||
B00000011,B11111000,B00000001,B11111100,B00000111,B11000000,B00011110,B00000001,B11111000,B00000011,B11111000,B00001111,B11000001,B11111110,B00011111,B11000000,
|
||||
B00000011,B11111000,B00000001,B11111100,B00001111,B11000000,B00011111,B10000001,B11111000,B00000011,B11111000,B00001111,B11000001,B11111100,B00011111,B11000000,
|
||||
B00000011,B11111000,B00000001,B11111100,B00001111,B10000000,B00001111,B10000001,B11111100,B00000011,B11111000,B00001111,B11000000,B01111100,B00011111,B11000000,
|
||||
B00000011,B11111000,B00000001,B11111100,B00011111,B10000000,B00001111,B11000001,B11111110,B00000111,B11110000,B00001111,B11000000,B00000000,B00011111,B11000000,
|
||||
B00000011,B11111000,B00000001,B11111100,B00011111,B11111111,B11111111,B11000000,B11111111,B11111111,B11110000,B00001111,B11000000,B00000000,B00011111,B11000000,
|
||||
B00000011,B11111000,B00000001,B11111100,B00111111,B11111111,B11111111,B11100000,B01111111,B11111111,B11100000,B00001111,B11000000,B00000000,B00011111,B11000000,
|
||||
B00000011,B11111000,B00000001,B11111100,B00111111,B11111111,B11111111,B11100000,B00111111,B11111111,B11000000,B00001111,B11000000,B00000000,B00011111,B11000000,
|
||||
B00000011,B11111000,B00000001,B11111100,B00111111,B11111111,B11111111,B11100000,B00011111,B11111111,B00000000,B00001111,B11000000,B00000000,B00011111,B11000000,
|
||||
B00000000,B00000000,B00000000,B00000000,B00000000,B00000111,B00000000,B00000000,B00000001,B11110000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,
|
||||
B00000000,B00000000,B00000000,B00000000,B00000000,B00000111,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,
|
||||
B00000000,B00000000,B00000000,B00000000,B00000000,B00000111,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,
|
||||
B00000000,B00000000,B00000000,B00000000,B00000000,B00000111,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,
|
||||
B00000000,B00000000,B00000000,B00000000,B00000000,B00000111,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,
|
||||
B00000000,B00000000,B00000000,B00000000,B00000000,B00000111,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,
|
||||
B00000000,B00000000,B00000000,B00000000,B00000000,B00000111,B00000000,B00000000,B00000000,B00000000,B10001000,B00000000,B00000000,B00000000,B00000000,B00000000,
|
||||
B00000000,B00000000,B00000000,B00000000,B00000000,B00000111,B00000000,B00000000,B00000000,B00000000,B10001000,B00000000,B00000000,B00000000,B00000000,B00000000,
|
||||
B00000000,B00000000,B00000000,B00000000,B00000000,B00000111,B00000000,B00000000,B00000000,B00000000,B10001001,B10001000,B10110100,B00000001,B11001011,B00011100,
|
||||
B00000000,B00000000,B00000000,B00000000,B00000000,B00000111,B00000000,B00000000,B00000000,B00000000,B11111000,B01001000,B10101010,B00000010,B00101100,B10100110,
|
||||
B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B10001001,B11001000,B10101010,B00000010,B00101000,B00011010,
|
||||
B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B10001010,B01001001,B10101010,B00110010,B00101000,B00000010,
|
||||
B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B10001001,B11100110,B10101010,B00110001,B11001000,B00011100,
|
||||
B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,B00000000,
|
||||
};
|
||||
@ -1,2 +0,0 @@
|
||||
-DENABLE_HWSERIAL1
|
||||
-DPIN_SERIAL1_RX=PA_10 -DPIN_SERIAL1_TX=PA_9
|
||||
@ -1,386 +0,0 @@
|
||||
// *******************************************************************
|
||||
// Arduino example code
|
||||
// for https://mygit.jblb.net/jerome/hoverboard-firmware-hack-FOC
|
||||
//
|
||||
// Copyright (C) 2019-2020 Emanuel FERU <aerdronix@gmail.com>
|
||||
// modified by jblb <jerome@jblb.net>
|
||||
//
|
||||
// *******************************************************************
|
||||
// INFO:
|
||||
// • This sketch uses the the Serial Software interface to communicate and send commands to the hoverboard
|
||||
// • The built-in (HW) Serial interface is used for debugging and visualization. In case the debugging is not needed,
|
||||
// it is recommended to use the built-in Serial interface for full speed perfomace.
|
||||
// • The data packaging includes a Start Frame, checksum, and re-syncronization capability for reliable communication
|
||||
//
|
||||
// by JbLb
|
||||
// • Converted to platformio
|
||||
// • Adapted to Arduino leonardo to use CDC serial interface
|
||||
// • Serial1 connected to hoverboard board
|
||||
// • CDC used to display data on serial monitor
|
||||
|
||||
//
|
||||
// CONFIGURATION on the hoverboard side in config.h:
|
||||
// • Option 1: Serial on Left Sensor cable (long wired cable)
|
||||
// #define CONTROL_SERIAL_USART2
|
||||
// #define FEEDBACK_SERIAL_USART2
|
||||
// // #define DEBUG_SERIAL_USART2
|
||||
// • Option 2: Serial on Right Sensor cable (short wired cable) - recommended, so the ADCs on the other cable are still available
|
||||
// #define CONTROL_SERIAL_USART3
|
||||
// #define FEEDBACK_SERIAL_USART3
|
||||
// // #define DEBUG_SERIAL_USART3
|
||||
// *******************************************************************
|
||||
|
||||
// ########################## DEFINES ##########################
|
||||
#define HOVER_SERIAL_BAUD 38400 // [-] Baud rate for HoverSerial (used to communicate with the hoverboard)
|
||||
#define SERIAL_BAUD 115200 // [-] Baud rate for built-in Serial (used for the Serial Monitor)
|
||||
#define START_FRAME 0xAAAA // [-] Start frme definition for reliable serial communication
|
||||
#define TIME_SEND 25 // [ms] Sending time interval
|
||||
#define SPEED_MAX_TEST 300 // [-] Maximum speed for testing
|
||||
//#define DEBUG_RX // [-] Debug received data. Prints all bytes to serial (comment-out to disable)
|
||||
|
||||
// ########################## macros ############################
|
||||
|
||||
#include <Wire.h>
|
||||
#include <Adafruit_GFX.h>
|
||||
#include <Adafruit_SSD1306.h>
|
||||
#include <NintendoExtensionCtrl.h> // https://github.com/dmadison/NintendoExtensionCtrl
|
||||
|
||||
|
||||
// #include <SoftwareSerial.h>
|
||||
// SoftwareSerial HoverSerial(2,3); // RX, TX
|
||||
|
||||
// Global variables
|
||||
uint8_t idx = 0; // Index for new data pointer
|
||||
uint16_t bufStartFrame; // Buffer Start Frame
|
||||
byte *p; // Pointer declaration for the new received data
|
||||
byte incomingByte;
|
||||
byte incomingBytePrev;
|
||||
|
||||
typedef struct {
|
||||
uint16_t start;
|
||||
int16_t steer;
|
||||
int16_t speed;
|
||||
uint16_t checksum;
|
||||
} SerialCommand;
|
||||
SerialCommand Command;
|
||||
|
||||
typedef struct {
|
||||
uint16_t start;
|
||||
int16_t cmd1;
|
||||
int16_t cmd2;
|
||||
int16_t speedR;
|
||||
int16_t speedL;
|
||||
int16_t speedR_meas;
|
||||
int16_t speedL_meas;
|
||||
int16_t batVoltage;
|
||||
int16_t boardTemp;
|
||||
int16_t checksum;
|
||||
} SerialFeedback;
|
||||
SerialFeedback Feedback;
|
||||
SerialFeedback NewFeedback;
|
||||
|
||||
int cmd1; // normalized input values. -1000 to 1000
|
||||
int cmd2;
|
||||
|
||||
int motor_test_direction = 1;
|
||||
|
||||
|
||||
#define SCREEN_WIDTH 128 // OLED display width, in pixels
|
||||
#define SCREEN_HEIGHT 64 // OLED display height, in pixels
|
||||
|
||||
// Declaration for an SSD1306 display connected to I2C (SDA, SCL pins)
|
||||
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire);
|
||||
// Connect to a Nunchuk
|
||||
Nunchuk nchuk;
|
||||
HardwareSerial Serial1(PA_10, PA_9);
|
||||
|
||||
// ########################## SETUP ##########################
|
||||
void setup()
|
||||
{
|
||||
|
||||
nchuk.begin();
|
||||
|
||||
display.begin(SSD1306_SWITCHCAPVCC, 0x3C); // initialize with the I2C addr 0x3C (for the 128x64)
|
||||
// Clear the buffer
|
||||
display.clearDisplay();
|
||||
display.display();
|
||||
|
||||
display.setTextSize(1); // Draw 1X-scale text
|
||||
display.setTextColor(SSD1306_WHITE);
|
||||
display.setCursor(int
|
||||
((display.width() -
|
||||
(strlen("Hoverboard Serial") * 6)) / 2), 0);
|
||||
display.println("Hoverboard Serial");
|
||||
display.setCursor(int
|
||||
((display.width() -(strlen("v STM32 0.1") *6))/2), 8);
|
||||
display.println("v STM32 0.1");
|
||||
/*
|
||||
display.print("Nunchuk ");
|
||||
nchuk.connect()? display.print("") : display.print("not ");
|
||||
display.println("connected");
|
||||
|
||||
display.drawCircle(80, 41, 22, SSD1306_WHITE);
|
||||
*/
|
||||
display.display();
|
||||
|
||||
Serial1.begin(HOVER_SERIAL_BAUD); // RX, TX from arduino to TX RX on hoverboard board. ! be carreful 3v3
|
||||
pinMode(LED_BUILTIN, OUTPUT);
|
||||
|
||||
Serial.begin(SERIAL_BAUD);
|
||||
if (Serial) {
|
||||
Serial.println("Hoverboard Serial v STM32 0.1");
|
||||
}
|
||||
}
|
||||
|
||||
// ########################## SEND ##########################
|
||||
void Send(int16_t uSteer, int16_t uSpeed)
|
||||
{
|
||||
// Create command
|
||||
Command.start = (uint16_t) START_FRAME;
|
||||
Command.steer = (int16_t) uSteer;
|
||||
Command.speed = (int16_t) uSpeed;
|
||||
Command.checksum =
|
||||
(uint16_t) (Command.start ^ Command.steer ^ Command.speed);
|
||||
|
||||
// Write to Serial
|
||||
Serial1.write((uint8_t *) & Command, sizeof(Command));
|
||||
}
|
||||
|
||||
// ########################## RECEIVE ##########################
|
||||
void Receive()
|
||||
|
||||
{
|
||||
int old_cursorX, old_cursorY;
|
||||
// Check for new data availability in the Serial buffer
|
||||
if (Serial1.available()) {
|
||||
incomingByte = Serial1.read(); // Read the incoming byte
|
||||
bufStartFrame = ((uint16_t) (incomingBytePrev) << 8) + incomingByte; // Construct the start frame
|
||||
} else {
|
||||
return;
|
||||
}
|
||||
|
||||
// If DEBUG_RX is defined print all incoming bytes
|
||||
#ifdef DEBUG_RX
|
||||
Serial.print(incomingByte);
|
||||
return;
|
||||
#endif
|
||||
|
||||
// Copy received data
|
||||
if (bufStartFrame == START_FRAME) { // Initialize if new data is detected
|
||||
p = (byte *) & NewFeedback;
|
||||
*p++ = incomingBytePrev;
|
||||
*p++ = incomingByte;
|
||||
idx = 2;
|
||||
} else if (idx >= 2 && idx < sizeof(SerialFeedback)) { // Save the new received data
|
||||
*p++ = incomingByte;
|
||||
idx++;
|
||||
}
|
||||
// Check if we reached the end of the package
|
||||
if (idx == sizeof(SerialFeedback)) {
|
||||
uint16_t checksum;
|
||||
checksum =
|
||||
(uint16_t) (NewFeedback.start ^ NewFeedback.cmd1 ^ NewFeedback.
|
||||
cmd2 ^ NewFeedback.speedR ^ NewFeedback.
|
||||
speedL ^ NewFeedback.speedR_meas ^ NewFeedback.
|
||||
speedL_meas ^ NewFeedback.batVoltage ^ NewFeedback.
|
||||
boardTemp);
|
||||
|
||||
// Check validity of the new data
|
||||
if (NewFeedback.start == START_FRAME
|
||||
&& checksum == NewFeedback.checksum) {
|
||||
// Copy the new data
|
||||
memcpy(&Feedback, &NewFeedback, sizeof(SerialFeedback));
|
||||
|
||||
// Print data to CDC Serial if available
|
||||
if (Serial) {
|
||||
Serial.print("1: ");
|
||||
Serial.print(Feedback.cmd1);
|
||||
Serial.print(" 2: ");
|
||||
Serial.print(Feedback.cmd2);
|
||||
Serial.print(" 3: ");
|
||||
Serial.print(Feedback.speedR);
|
||||
Serial.print(" 4: ");
|
||||
Serial.print(Feedback.speedL);
|
||||
Serial.print(" 5: ");
|
||||
Serial.print(Feedback.speedR_meas);
|
||||
Serial.print(" 6: ");
|
||||
Serial.print(Feedback.speedL_meas);
|
||||
Serial.print(" 7: ");
|
||||
Serial.print(Feedback.batVoltage);
|
||||
Serial.print(" 8: ");
|
||||
Serial.println(Feedback.boardTemp);
|
||||
}
|
||||
display.setCursor(0, 30);
|
||||
display.setTextSize(1); // Draw 1X-scale text
|
||||
display.setTextColor(SSD1306_WHITE);
|
||||
display.print("V : ");
|
||||
old_cursorX = display.getCursorX();
|
||||
old_cursorY = display.getCursorY();
|
||||
display.fillRect(old_cursorX, old_cursorY, (6 * 5), 8, SSD1306_BLACK); // erase previous display
|
||||
display.setCursor(old_cursorX, old_cursorY);
|
||||
display.print(Feedback.batVoltage);
|
||||
display.display();
|
||||
} else {
|
||||
if (Serial)
|
||||
Serial.println("Non-valid data skipped");
|
||||
}
|
||||
|
||||
idx = 0; // Reset the index (it prevents to enter in this if condition in the next cycle)
|
||||
}
|
||||
// Update previous states
|
||||
incomingBytePrev = incomingByte;
|
||||
}
|
||||
|
||||
void Nunchuk_control()
|
||||
{
|
||||
int old_cursorX, old_cursorY;
|
||||
|
||||
if (!nchuk.update()) {
|
||||
nchuk.reconnect();
|
||||
} else {
|
||||
|
||||
|
||||
cmd1 = map(nchuk.joyX(),0,256,150,-150); // x - axis. Nunchuck joystick readings range 30 - 230
|
||||
cmd2 = map(nchuk.joyY(),0,256,-150,150); // y - axis
|
||||
|
||||
Serial.print("X : "); Serial.println(nchuk.joyX());
|
||||
Serial.print("Y : "); Serial.println(nchuk.joyY());
|
||||
|
||||
}
|
||||
/*
|
||||
display.setCursor(0, 40);
|
||||
display.setTextSize(1); // Draw 1X-scale text
|
||||
display.setTextColor(SSD1306_WHITE);
|
||||
display.print("X : ");
|
||||
old_cursorX = display.getCursorX();
|
||||
old_cursorY = display.getCursorY();
|
||||
display.fillRect(old_cursorX, old_cursorY, (6 * 5), 8, SSD1306_BLACK); // erase previous display
|
||||
display.setCursor(old_cursorX, old_cursorY);
|
||||
display.print(cmd1);
|
||||
display.setCursor(0, 50);
|
||||
display.setTextSize(1); // Draw 1X-scale text
|
||||
display.setTextColor(SSD1306_WHITE);
|
||||
display.print("y : ");
|
||||
old_cursorX = display.getCursorX();
|
||||
old_cursorY = display.getCursorY();
|
||||
display.fillRect(old_cursorX, old_cursorY, (6 * 5), 8, SSD1306_BLACK); // erase previous display
|
||||
display.setCursor(old_cursorX, old_cursorY);
|
||||
display.print(cmd2);
|
||||
|
||||
display.display();
|
||||
*/
|
||||
Send(cmd1, cmd2);
|
||||
|
||||
}
|
||||
// ########################## nunchuk ##########################
|
||||
|
||||
void Nunchuk_display()
|
||||
{
|
||||
if (!nchuk.update()) {
|
||||
if (Serial) {
|
||||
Serial.println("Controller disconnected!");
|
||||
}
|
||||
nchuk.reconnect();
|
||||
} else {
|
||||
if (Serial) {
|
||||
// Read a button (on/off, C and Z)
|
||||
Serial.print("Z: ");
|
||||
nchuk.buttonZ()? Serial.print("On ") : Serial.print("Off ");
|
||||
Serial.print("C: ");
|
||||
nchuk.buttonC()? Serial.print("On ") : Serial.print("Off ");
|
||||
|
||||
// Read joystick axis (0-255, X and Y)
|
||||
Serial.print("The joystick's Y axis is at ");
|
||||
Serial.print(nchuk.joyY());
|
||||
Serial.print(" and X axis is at ");
|
||||
Serial.print(nchuk.joyX());
|
||||
|
||||
// Read an accelerometer and print values (0-1023, X, Y, and Z)
|
||||
Serial.print(" - The accelerometer's X-axis is at ");
|
||||
Serial.println(nchuk.accelX());
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// ########################## nunchuk ##########################
|
||||
|
||||
void show_dot()
|
||||
{
|
||||
int old_cursorX, old_cursorY;
|
||||
|
||||
char text[22];
|
||||
|
||||
if (nchuk.update()) {
|
||||
display.setCursor(0, 20);
|
||||
display.setTextSize(1); // Draw 1X-scale text
|
||||
display.setTextColor(SSD1306_WHITE);
|
||||
display.print("X : ");
|
||||
old_cursorX = display.getCursorX();
|
||||
old_cursorY = display.getCursorY();
|
||||
display.fillRect(old_cursorX, old_cursorY, (6 * 4), 8, SSD1306_BLACK); // erase previous display
|
||||
display.setCursor(old_cursorX, old_cursorY);
|
||||
display.print(nchuk.joyX());
|
||||
display.setCursor(0, 50);
|
||||
display.setTextSize(1); // Draw 1X-scale text
|
||||
display.setTextColor(SSD1306_WHITE);
|
||||
display.print("Y : ");
|
||||
old_cursorX = display.getCursorX();
|
||||
old_cursorY = display.getCursorY();
|
||||
display.fillRect(old_cursorX, old_cursorY, (6 * 4), 8, SSD1306_BLACK); // erase previous display
|
||||
display.setCursor(old_cursorX, old_cursorY);
|
||||
display.print(nchuk.joyY());
|
||||
|
||||
int y_d = int(41 + (44*(nchuk.joyY()-128))/256);
|
||||
int x_d = int(80 + (44*(nchuk.joyX()-128))/256);
|
||||
|
||||
if (Serial) {
|
||||
sprintf (text,"y_d : %d | x_d : %d", y_d , x_d );
|
||||
Serial.println(text);
|
||||
}
|
||||
|
||||
display.drawPixel(x_d,y_d,SSD1306_WHITE);
|
||||
}
|
||||
else {
|
||||
nchuk.reconnect();
|
||||
}
|
||||
}
|
||||
|
||||
// ########################## LOOP ##########################
|
||||
|
||||
unsigned long iTimeSend = 0;
|
||||
int iTestMax = SPEED_MAX_TEST;
|
||||
int iTest = 0;
|
||||
int16_t old_cursorX;
|
||||
int16_t old_cursorY;
|
||||
|
||||
void loop(void)
|
||||
{
|
||||
unsigned long timeNow = millis();
|
||||
|
||||
// Check for new received data
|
||||
// Receive();
|
||||
|
||||
if (iTimeSend > timeNow)
|
||||
return;
|
||||
iTimeSend = timeNow + TIME_SEND;
|
||||
// Nunchuk_display();
|
||||
Nunchuk_control();
|
||||
/*
|
||||
// Send commands
|
||||
if (iTimeSend > timeNow)
|
||||
return;
|
||||
iTimeSend = timeNow + TIME_SEND;
|
||||
Send(0, cmd2);
|
||||
|
||||
// Calculate test command signal
|
||||
|
||||
if (motor_test_direction == 1) cmd2 += 1;
|
||||
else cmd2 -= 1;
|
||||
if (abs(cmd2) > SPEED_MAX_TEST) motor_test_direction = -motor_test_direction;
|
||||
*/
|
||||
// Blink the LED
|
||||
digitalWrite(LED_BUILTIN, (timeNow % 2000) < 1000);
|
||||
}
|
||||
|
||||
// ########################## END ##########################
|
||||
Loading…
Reference in new issue