You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
325 lines
9.7 KiB
325 lines
9.7 KiB
#include <Arduino.h>
|
|
#include "splash-h.h"
|
|
#include <Adafruit_GFX.h>
|
|
#include <Adafruit_SSD1306.h>
|
|
#include <Adafruit_NeoPixel.h>
|
|
|
|
#define PIN PB5 // Pin where NeoPixels are connected
|
|
#define NB_LED 12 // number of LEDs
|
|
|
|
// Declare our NeoPixel strip object:
|
|
Adafruit_NeoPixel strip(NB_LED, 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;
|
|
|
|
// ########################## 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.1") * 6)) / 2), 8);
|
|
display.println("v STM32 0.1");
|
|
|
|
// 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, 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;
|
|
}
|
|
|
|
// ########################## Nunchuk_control ##########################
|
|
|
|
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 150 -150
|
|
cmd2 = map(nchuk.joyY(), 0, 256, -150, 150); // y - axis
|
|
|
|
Send(cmd1, cmd2);
|
|
}
|
|
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();
|
|
}
|
|
|
|
// ########################## LOOP ##########################
|
|
|
|
unsigned long iTimeSend = 0;
|
|
int idx_led = 0;
|
|
|
|
void loop()
|
|
{
|
|
unsigned long timeNow = millis();
|
|
|
|
// Blink the LED
|
|
digitalWrite(LED_BUILTIN, (timeNow % 2000) < 1000);
|
|
Receive();
|
|
Nunchuk_control();
|
|
|
|
if (iTimeSend > timeNow)
|
|
return;
|
|
if (cmd2 == 0)
|
|
return;
|
|
iTimeSend = timeNow + ((200 - abs(cmd2)) * 2); // TIME_SEND
|
|
strip.clear();
|
|
strip.setPixelColor(idx_led, strip.Color(128, 0, 0));
|
|
if (cmd2 > 0){
|
|
if (idx_led < strip.numPixels() -1 )
|
|
{
|
|
idx_led++;
|
|
}
|
|
else
|
|
{
|
|
idx_led = 0;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (idx_led > 0 )
|
|
{
|
|
idx_led--;
|
|
}
|
|
else
|
|
{
|
|
idx_led = strip.numPixels() -1 ;
|
|
}
|
|
}
|
|
strip.show();
|
|
}
|