|
|
|
|
@ -30,24 +30,26 @@
|
|
|
|
|
// // #define DEBUG_SERIAL_USART3
|
|
|
|
|
// *******************************************************************
|
|
|
|
|
|
|
|
|
|
#define VERSION_TAG "V test 0.2"
|
|
|
|
|
#define BTN_PLUS 4
|
|
|
|
|
#define BTN_MOINS 5
|
|
|
|
|
#define BTN_MARCHE 6
|
|
|
|
|
#define MAX_SPEED 420
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// ########################## DEFINES ##########################
|
|
|
|
|
#define HOVER_SERIAL_BAUD 38400 // [-] Baud rate for HoverSerial (used to communicate with the hoverboard)
|
|
|
|
|
#define HOVER_SERIAL_BAUD 115200 // [-] 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)
|
|
|
|
|
#define START_FRAME 0xABCD // [-] Start frme definition for reliable serial communication
|
|
|
|
|
#define TIME_SEND 25 // [ms] Sending time interval
|
|
|
|
|
#define SPEED_MAX_TEST 490 // [-] 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 <EEPROM.h>
|
|
|
|
|
|
|
|
|
|
// #include <SoftwareSerial.h>
|
|
|
|
|
// SoftwareSerial HoverSerial(2,3); // RX, TX
|
|
|
|
|
#include <FastLED.h>
|
|
|
|
|
|
|
|
|
|
// Global variables
|
|
|
|
|
uint8_t idx = 0; // Index for new data pointer
|
|
|
|
|
@ -68,67 +70,67 @@ 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;
|
|
|
|
|
uint16_t cmdLed;
|
|
|
|
|
uint16_t checksum;
|
|
|
|
|
} SerialFeedback;
|
|
|
|
|
SerialFeedback Feedback;
|
|
|
|
|
SerialFeedback NewFeedback;
|
|
|
|
|
|
|
|
|
|
int cmd1; // normalized input values. -1000 to 1000
|
|
|
|
|
int cmd2;
|
|
|
|
|
int accel_speed =10;
|
|
|
|
|
|
|
|
|
|
int eeprom_address = 0;
|
|
|
|
|
|
|
|
|
|
int motor_test_direction = 1;
|
|
|
|
|
int consigne_speed = 10;
|
|
|
|
|
int actual_speed = 0;
|
|
|
|
|
int btn_control = 20;
|
|
|
|
|
|
|
|
|
|
bool last_run, last_plus, last_moins, last_marche = false;
|
|
|
|
|
bool rel_marche = true;
|
|
|
|
|
bool run_mode = false; // false by default
|
|
|
|
|
|
|
|
|
|
#define SCREEN_WIDTH 128 // OLED display width, in pixels
|
|
|
|
|
#define SCREEN_HEIGHT 64 // OLED display height, in pixels
|
|
|
|
|
CRGB powerled;
|
|
|
|
|
// ################ Led definition ########################
|
|
|
|
|
|
|
|
|
|
#define LED_PIN 7
|
|
|
|
|
#define NUM_LEDS 9
|
|
|
|
|
#define LED_TYPE WS2812B
|
|
|
|
|
#define COLOR_ORDER GRB
|
|
|
|
|
|
|
|
|
|
CRGB leds[NUM_LEDS];
|
|
|
|
|
|
|
|
|
|
// 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();
|
|
|
|
|
pinMode(BTN_MARCHE, INPUT_PULLUP);
|
|
|
|
|
pinMode(BTN_PLUS, INPUT_PULLUP);
|
|
|
|
|
pinMode(BTN_MOINS, INPUT_PULLUP);
|
|
|
|
|
|
|
|
|
|
Serial1.begin(HOVER_SERIAL_BAUD); // RX, TX from arduino to TX RX on hoverboard board. ! be carreful 3v3
|
|
|
|
|
pinMode(LED_BUILTIN, OUTPUT);
|
|
|
|
|
|
|
|
|
|
consigne_speed = EEPROMReadInt(eeprom_address);
|
|
|
|
|
if (consigne_speed < 0) {
|
|
|
|
|
consigne_speed =0;
|
|
|
|
|
}
|
|
|
|
|
if (consigne_speed > MAX_SPEED) {
|
|
|
|
|
consigne_speed = MAX_SPEED ;
|
|
|
|
|
}
|
|
|
|
|
LEDS.addLeds<LED_TYPE,LED_PIN,COLOR_ORDER>(leds,NUM_LEDS);
|
|
|
|
|
LEDS.setBrightness(32);
|
|
|
|
|
memset(leds, 0, NUM_LEDS * 3); // all leds off
|
|
|
|
|
// FastLED.clear ();
|
|
|
|
|
powerled = CRGB(128,32,0);
|
|
|
|
|
leds[0] = powerled;
|
|
|
|
|
LEDS.show();
|
|
|
|
|
|
|
|
|
|
Serial.begin(SERIAL_BAUD);
|
|
|
|
|
if (Serial) {
|
|
|
|
|
Serial.println("Hoverboard Serial v STM32 0.1");
|
|
|
|
|
Serial.println(VERSION_TAG);
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
@ -144,243 +146,271 @@ void Send(int16_t uSteer, int16_t uSpeed)
|
|
|
|
|
|
|
|
|
|
// Write to Serial
|
|
|
|
|
Serial1.write((uint8_t *) & Command, sizeof(Command));
|
|
|
|
|
// Serial.println(uSpeed);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// ########################## 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 {
|
|
|
|
|
incomingByte = Serial1.read(); // Read the incoming byte
|
|
|
|
|
bufStartFrame = ((uint16_t)(incomingByte) << 8) | incomingBytePrev; // Construct the start frame
|
|
|
|
|
}
|
|
|
|
|
else {
|
|
|
|
|
return;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// If DEBUG_RX is defined print all incoming bytes
|
|
|
|
|
#ifdef DEBUG_RX
|
|
|
|
|
Serial.print(incomingByte);
|
|
|
|
|
return;
|
|
|
|
|
#endif
|
|
|
|
|
// 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;
|
|
|
|
|
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);
|
|
|
|
|
checksum = (uint16_t)(NewFeedback.start ^ NewFeedback.cmd1 ^ NewFeedback.cmd2 ^ NewFeedback.speedR_meas ^ NewFeedback.speedL_meas
|
|
|
|
|
^ NewFeedback.batVoltage ^ NewFeedback.boardTemp ^ NewFeedback.cmdLed);
|
|
|
|
|
|
|
|
|
|
// Check validity of the new data
|
|
|
|
|
if (NewFeedback.start == START_FRAME
|
|
|
|
|
&& checksum == NewFeedback.checksum) {
|
|
|
|
|
if (NewFeedback.start == START_FRAME && checksum == NewFeedback.checksum) {
|
|
|
|
|
// Copy the new data
|
|
|
|
|
memcpy(&Feedback, &NewFeedback, sizeof(SerialFeedback));
|
|
|
|
|
|
|
|
|
|
// Print data to CDC Serial if available
|
|
|
|
|
// Print data to built-in Serial
|
|
|
|
|
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();
|
|
|
|
|
Serial.print("1: "); Serial.print(Feedback.cmd1);
|
|
|
|
|
Serial.print(" 2: "); Serial.print(Feedback.cmd2);
|
|
|
|
|
Serial.print(" 3: "); Serial.print(Feedback.speedR_meas);
|
|
|
|
|
Serial.print(" 4: "); Serial.print(Feedback.speedL_meas);
|
|
|
|
|
Serial.print(" 5: "); Serial.print(Feedback.batVoltage);
|
|
|
|
|
Serial.print(" 6: "); Serial.print(Feedback.boardTemp);
|
|
|
|
|
Serial.print(" 7: "); Serial.println(Feedback.cmdLed);
|
|
|
|
|
}
|
|
|
|
|
} else {
|
|
|
|
|
if (Serial)
|
|
|
|
|
Serial.println("Non-valid data skipped");
|
|
|
|
|
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)
|
|
|
|
|
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();
|
|
|
|
|
// #############################
|
|
|
|
|
//This function will write a 2 byte integer to the eeprom at the specified address and address + 1
|
|
|
|
|
void EEPROMWriteInt(int p_address, int p_value)
|
|
|
|
|
{
|
|
|
|
|
byte lowByte = ((p_value >> 0) & 0xFF);
|
|
|
|
|
byte highByte = ((p_value >> 8) & 0xFF);
|
|
|
|
|
|
|
|
|
|
EEPROM.write(p_address, lowByte);
|
|
|
|
|
EEPROM.write(p_address + 1, highByte);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
//This function will read a 2 byte integer from the eeprom at the specified address and address + 1
|
|
|
|
|
int EEPROMReadInt(int p_address)
|
|
|
|
|
{
|
|
|
|
|
byte lowByte = EEPROM.read(p_address);
|
|
|
|
|
byte highByte = EEPROM.read(p_address + 1);
|
|
|
|
|
|
|
|
|
|
return ((lowByte << 0) & 0xFF) + ((highByte << 8) & 0xFF00);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
void run_control(void) {
|
|
|
|
|
if (!digitalRead(BTN_MARCHE)) {
|
|
|
|
|
if (last_marche){
|
|
|
|
|
if (!run_mode ){
|
|
|
|
|
if (rel_marche){
|
|
|
|
|
// run
|
|
|
|
|
run_mode = true;
|
|
|
|
|
rel_marche = false;
|
|
|
|
|
return;
|
|
|
|
|
}
|
|
|
|
|
} else {
|
|
|
|
|
if (rel_marche){
|
|
|
|
|
rel_marche = false;
|
|
|
|
|
run_mode = false;
|
|
|
|
|
actual_speed = 0;
|
|
|
|
|
if ( EEPROMReadInt(eeprom_address) != consigne_speed ){
|
|
|
|
|
EEPROMWriteInt(eeprom_address, consigne_speed);
|
|
|
|
|
}
|
|
|
|
|
return;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
} else {
|
|
|
|
|
last_marche = true;
|
|
|
|
|
return;
|
|
|
|
|
}
|
|
|
|
|
} 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());
|
|
|
|
|
}
|
|
|
|
|
rel_marche = true;
|
|
|
|
|
last_marche = false;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// ########################## 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);
|
|
|
|
|
void speed_control(void) {
|
|
|
|
|
if (run_mode){
|
|
|
|
|
if (!digitalRead(BTN_PLUS)){
|
|
|
|
|
if (last_plus){
|
|
|
|
|
consigne_speed = consigne_speed + 3;
|
|
|
|
|
if (consigne_speed > MAX_SPEED) {
|
|
|
|
|
consigne_speed = MAX_SPEED ;
|
|
|
|
|
}
|
|
|
|
|
} else {
|
|
|
|
|
last_plus = true;
|
|
|
|
|
}
|
|
|
|
|
} else {
|
|
|
|
|
last_plus = false;
|
|
|
|
|
}
|
|
|
|
|
if (!digitalRead(BTN_MOINS)){
|
|
|
|
|
if (last_moins){
|
|
|
|
|
consigne_speed = consigne_speed - 3;
|
|
|
|
|
if (consigne_speed < 0 ) {
|
|
|
|
|
consigne_speed = 0 ;
|
|
|
|
|
}
|
|
|
|
|
} else {
|
|
|
|
|
last_moins = true;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
display.drawPixel(x_d,y_d,SSD1306_WHITE);
|
|
|
|
|
}
|
|
|
|
|
else {
|
|
|
|
|
nchuk.reconnect();
|
|
|
|
|
} else {
|
|
|
|
|
last_moins = false;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// ########################## 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();
|
|
|
|
|
Receive();
|
|
|
|
|
|
|
|
|
|
// Send(0,actual_speed);
|
|
|
|
|
|
|
|
|
|
// if (Serial) Serial.println(actual_speed);
|
|
|
|
|
|
|
|
|
|
if (iTimeSend > timeNow) return;
|
|
|
|
|
|
|
|
|
|
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);
|
|
|
|
|
Send(0,actual_speed);
|
|
|
|
|
if (Serial) {
|
|
|
|
|
if (run_mode != last_run) {
|
|
|
|
|
last_run = run_mode;
|
|
|
|
|
if (run_mode){
|
|
|
|
|
Serial.println("Run");
|
|
|
|
|
} else {
|
|
|
|
|
Serial.println("Stop");
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
if (run_mode){
|
|
|
|
|
leds[1] = CRGB(0,255,0); // Green
|
|
|
|
|
if ( actual_speed != consigne_speed){
|
|
|
|
|
if ( actual_speed < consigne_speed){
|
|
|
|
|
actual_speed ++ ;
|
|
|
|
|
} else {
|
|
|
|
|
actual_speed --;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
// indication vitesse sur barre leds
|
|
|
|
|
int max_vit = int(actual_speed / 70);
|
|
|
|
|
memset(leds, 0, NUM_LEDS * 3); // all leds off
|
|
|
|
|
leds[0] = powerled;
|
|
|
|
|
|
|
|
|
|
for (int i_led = 0; i_led-1 < max_vit ; i_led = i_led +1 )
|
|
|
|
|
{
|
|
|
|
|
leds[i_led + 1] = CRGB(0,255,0);
|
|
|
|
|
}
|
|
|
|
|
} else { // rouge si stop
|
|
|
|
|
FastLED.clear ();
|
|
|
|
|
leds[0] = powerled;
|
|
|
|
|
|
|
|
|
|
leds[1] = CRGB(255,0,0); // Red
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
LEDS.show(); // update LEDS
|
|
|
|
|
|
|
|
|
|
run_control();
|
|
|
|
|
|
|
|
|
|
btn_control --;
|
|
|
|
|
if (btn_control == 0){
|
|
|
|
|
btn_control = 6;
|
|
|
|
|
speed_control();
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// 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);
|
|
|
|
|
//digitalWrite(LED_BUILTIN, (timeNow % 2000) < 1000);
|
|
|
|
|
|
|
|
|
|
((timeNow % 2000) < 1000)?powerled = CRGB(128,32,0):powerled = CRGB(0,0,0);
|
|
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// ########################## END ##########################
|
|
|
|
|
/*
|
|
|
|
|
*
|
|
|
|
|
if (Feedback.cmdLed & LED1_SET) { gpio_bit_set(LED1_GPIO_Port, LED1_Pin); } else { gpio_bit_reset(LED1_GPIO_Port, LED1_Pin); } // red
|
|
|
|
|
if (Feedback.cmdLed & LED2_SET) { gpio_bit_set(LED2_GPIO_Port, LED2_Pin); } else { gpio_bit_reset(LED2_GPIO_Port, LED2_Pin); } // green
|
|
|
|
|
if (Feedback.cmdLed & LED3_SET) { gpio_bit_set(LED3_GPIO_Port, LED3_Pin); } else { gpio_bit_reset(LED3_GPIO_Port, LED3_Pin); } // orange
|
|
|
|
|
if (Feedback.cmdLed & LED4_SET) { gpio_bit_set(LED4_GPIO_Port, LED4_Pin); } else { gpio_bit_reset(LED4_GPIO_Port, LED4_Pin); } // blue
|
|
|
|
|
if (Feedback.cmdLed & LED5_SET) { gpio_bit_set(LED5_GPIO_Port, LED5_Pin); } else { gpio_bit_reset(LED5_GPIO_Port, LED5_Pin); } // blue
|
|
|
|
|
if (Feedback.cmdLed & LED4_SET) { gpio_bit_set(AUX3_GPIO_Port, AUX3_Pin); } else { gpio_bit_reset(AUX3_GPIO_Port, AUX3_Pin); }
|
|
|
|
|
}
|
|
|
|
|
*
|
|
|
|
|
*
|
|
|
|
|
#define LED1_SET (0x01)
|
|
|
|
|
#define LED2_SET (0x02)
|
|
|
|
|
#define LED3_SET (0x04)
|
|
|
|
|
#define LED4_SET (0x08)
|
|
|
|
|
#define LED5_SET (0x10)
|
|
|
|
|
|
|
|
|
|
***********************************************************************************************
|
|
|
|
|
cablage
|
|
|
|
|
Db9 arduino
|
|
|
|
|
|
|
|
|
|
1 GND
|
|
|
|
|
2 power_on
|
|
|
|
|
3 D7 led
|
|
|
|
|
4 D5 moins
|
|
|
|
|
5 5V
|
|
|
|
|
6 nc
|
|
|
|
|
7 power_on
|
|
|
|
|
8 D6 marche
|
|
|
|
|
9 D4 plus
|
|
|
|
|
|
|
|
|
|
cablage
|
|
|
|
|
HE10-10 arduino
|
|
|
|
|
1 gnd
|
|
|
|
|
2 nc
|
|
|
|
|
3 power_on
|
|
|
|
|
4 power_on
|
|
|
|
|
5 D7 led
|
|
|
|
|
6 D6 marche
|
|
|
|
|
7 D5 moin
|
|
|
|
|
8 D4 plus
|
|
|
|
|
9 5V
|
|
|
|
|
*/
|
|
|
|
|
|