Input Mehtods can now be active simultaneously

inculde guards and renamed macros
adapted baud rates

visualize wheel speed
testrun
phail 4 years ago
parent a715396aac
commit a242e00b66
  1. 9
      include/BLE.h
  2. 6
      include/ESPnowMaster.h
  3. 29
      include/ESPnowSlave.h
  4. 16
      include/Paddelec.h
  5. 7
      include/Platooning.h
  6. 55
      include/config.h
  7. 6
      include/oled.h
  8. 6
      include/serialbridge.h
  9. 9
      lib/ArduinoNunchuk/ArduinoNunchuk.cpp
  10. 7
      lib/ArduinoNunchuk/ArduinoNunchuk.h
  11. 7
      lib/CRC32/src/crc.h
  12. 9
      lib/Gametrak/src/Gametrak.h
  13. 2
      lib/Protocol/src/protocol.cpp
  14. 2
      lib/Protocol/src/protocol.h
  15. 78
      platformio.ini
  16. 5
      src/Paddelec.cpp
  17. 3
      src/Platooning.cpp
  18. 214
      src/main.cpp

@ -1,10 +1,5 @@
#ifndef BLE_H
#define BLE_H
#pragma once
void setupBLE();
void loopBLE();
extern double ble_pitch, ble_roll;
#endif
extern double ble_pitch, ble_roll;

@ -1,5 +1,4 @@
#ifndef ESPNOWMASTER_H
#define ESPNOWMASTER_H
#pragma once
#include <Arduino.h>
/**
@ -265,5 +264,4 @@ void loopESPnowMaster() {
// wait for 3seconds to run the logic again
delay(3000);
}
#endif
}

@ -1,5 +1,4 @@
#ifndef ESPNOWSLAVE_H
#define ESPNOWSLAVE_H
#pragma once
/**
ESPNOW - Basic communication - Slave
Date: 26th September 2017
@ -37,6 +36,8 @@
#define CHANNEL 1
extern bool ESPnowDataReceived;
void OnDataRecv(const uint8_t *mac_addr, const uint8_t *data, int data_len);
// Init ESP Now with fallback
@ -85,19 +86,17 @@ uint8_t ESPnowdata = 0;
// callback when data is recv from Master
void OnDataRecv(const uint8_t *mac_addr, const uint8_t *data, int data_len) {
char macStr[18];
/* char macStr[18];
snprintf(macStr, sizeof(macStr), "%02x:%02x:%02x:%02x:%02x:%02x",
mac_addr[0], mac_addr[1], mac_addr[2], mac_addr[3], mac_addr[4], mac_addr[5]);
//Serial.print("Last Packet Recv from: "); Serial.println(macStr);
//Serial.print("Last Packet Recv Data: "); Serial.println(data_len);
//Serial.println("");
if(sizeof motor == data_len)
{
memcpy((void*)&motor, data, sizeof(motorControl)); //TODO: dangerous..
}
Serial.print("Last Packet Recv from: "); Serial.println(macStr);
Serial.print("Last Packet Recv Data: "); Serial.println(data_len);
Serial.println("");
*/
}
#endif
if(sizeof motor == data_len)
{
ESPnowDataReceived = true;
memcpy((void*)&motor, data, sizeof(motorControl)); //TODO: dangerous..
}
}

@ -1,7 +1,5 @@
#pragma once
#if defined(INPUT_PADDELECIMU) || defined(INPUT_PADDELEC)
#include <Arduino.h>
#include "config.h"
#ifdef INPUT_PADDELEC
@ -10,6 +8,7 @@
#include <IMU.h>
#endif
#if defined(INPUT_PADDELECIMU) || defined(INPUT_PADDELEC)
class Paddelec
{
@ -46,15 +45,17 @@ class Paddelec
imu.init();
cfgPaddle.paddleAngleThreshold = 10.0; // activation angle threshold of paddle. Below threshold, paddle is not enganged and paddelec is freewheeling.
cfgPaddle.deltaRtoSpeed = 0.01; // conversion factor between Gametrak movement to speed. This defines also the maximum speed.
cfgPaddle.pwmMultiplier = 0.035; // effect of paddle stroke to speed
cfgPaddle.crosstalkLR = 0.002; // multiplier for steering
cfgPaddle.realign = 0.0005; // paddelc tries to go straight forward
cfgPaddle.drag = 0.0002; // drag/water resistance
cfgPaddle.pwmMultiplier = 0.015; // effect of paddle stroke to speed
cfgPaddle.crosstalkLR = 0.001; // multiplier for steering
cfgPaddle.realign = 0.0001; // paddelc tries to go straight forward
cfgPaddle.drag = 0.0001; // drag/water resistance
#endif
return(true);
}
void update(double &pwm, double &steer, double &actualSpeed_kmh, double &actualSteer, uint32_t deltaMillis);
void debug(Stream &port);
void RLpwmToSteer(double &steer, double &pwm, double &pwmR, double &pwmL);
void steerToRL(double &steer, double &pwm, double &pwmR, double &pwmL);
private:
#ifdef INPUT_PADDELEC
@ -94,9 +95,6 @@ class Paddelec
#endif
}
#endif
void RLpwmToSteer(double &steer, double &pwm, double &pwmR, double &pwmL);
void steerToRL(double &steer, double &pwm, double &pwmR, double &pwmL);
};
#endif

@ -1,12 +1,10 @@
#ifndef Platooning_H
#define Platooning_H
#ifdef PLATOONING
#pragma once
#include <Arduino.h>
#include "config.h"
#include "Gametrak.h"
#ifdef INPUT_PLATOONING
class Platooning
{
@ -70,5 +68,4 @@ class Platooning
}
};
#endif
#endif

@ -1,5 +1,4 @@
#pragma once
/*******************************************************************/
/* Whenever possible, configs should be managed in platformio.ini! */
/*******************************************************************/
@ -19,10 +18,18 @@ extern motorControl motor;
extern bool debug;
#define MULTITASKING
//#define BLE
//#define ESPnowMASTER
//#define ESPnowSLAVE
//#define INPUT_BLE
//#define OUTPUT_BINARY
//#define OUTPUT_BINARY_CRC
#ifdef OUTPUT_BINARY_CRC
#define OUTPUT_BINARY
#endif
//#define OUTPUT_PROTOCOL
#ifdef OUTPUT_PROTOCOL
#define INCLUDE_PROTOCOL
#endif
//#define OUTPUT_ESPnowMASTER
//#define INPUT_ESPnowSLAVE
//#define OLED
@ -32,8 +39,6 @@ extern bool debug;
//#define WIFI
#ifdef WIFI
#include <WiFi.h>
#include <esp_wifi.h>
/*********************************************************************/
@ -76,7 +81,7 @@ extern bool debug;
//#define SERIAL2_VCCPIN 5 // Pin used as VCC
//#define SERIAL2_GNDPIN 0 // Pin used as GND
#define bufferSize 1024
#define bufferSize 1024 // Buffer size used to exchange data between COM and telnet
#define MAX_NMEA_CLIENTS 4
/*********************************************************************/
@ -84,8 +89,8 @@ extern bool debug;
/*********************************************************************/
//#define INPUT_PADDELEC // look at Paddelec.h for paddelec specific config options!
//#define INPUT_PADDELECIMU
//#define NUNCHUCK // look at ArduinoNunchuck.h for Nunchuck specific config options!
//#define PLATOONING
//#define INPUT_NUNCHUCK // look at ArduinoNunchuck.h for Nunchuck specific config options!
//#define INPUT_PLATOONING
#define MOTORINPUT_PERIOD 20 // Update Motor Input each xx milliseconds
// #define INPUT_IMU
@ -105,4 +110,32 @@ extern bool debug;
//#define GAMETRAK2_PHIPIN 35 // horizontal angle Pin Gametrak 2
//#define GAMETRAK2_THETAPIN 33 // vertical angle Pin Gametrak 2
//#define GAMETRAK2_PHI_REV 1 // Phi is inverted
//#define GAMETRAK2_THETA_REV 1 // Theta is inverted
//#define GAMETRAK2_THETA_REV 1 // Theta is inverted
// ############################### VALIDATE SETTINGS ###############################
#if defined(OUTPUT_PROTOCOL) && defined(OUTPUT_BINARY)
#error OUTPUT_PROTOCOL and OUTPUT_BINARY not allowed, both on same serial.
#endif
#if !defined(OUTPUT_PROTOCOL) && !defined(OUTPUT_BINARY) && !defined(OUTPUT_ESPnowMASTER)
#error no Output Method defined. Nothing will be done..
#endif
#if defined(INPUT_PADDELEC) && defined(INPUT_PADDELECIMU)
#error INPUT_PADDELEC and INPUT_PADDELECIMU cannot be used simultaneously.
#endif
#if !defined(INPUT_BLE) && \
!defined(INPUT_ESPnowSLAVE) && \
!defined(INPUT_IMU) && \
!defined(INPUT_NUNCHUCK) && \
!defined(INPUT_PADDELEC) && \
!defined(INPUT_PADDELECIMU) && \
!defined(INPUT_PLATOONING)
#error no Input Method defined. What should I do?
#endif
#if defined(INPUT_BLE)
#error INPUT_BLE not yet implmented. Also set define guard here.
#endif

@ -1,5 +1,4 @@
#ifndef OLED_H
#define OLED_H
#pragma once
// TTGO Test. This is a test script for a "TTGO" ESP32 Dev Board
// including an LCD module and a 18650 battery pack.
//
@ -471,5 +470,4 @@ void loopOLED(void) {
// delay between each page
delay(150);
}
#endif
}

@ -1,6 +1,4 @@
#ifndef SERIALBRIDGE_H
#define SERIALBRIDGE_H
#pragma once
#include "config.h"
#include <Arduino.h>
@ -31,5 +29,3 @@
extern HardwareSerial* COM[NUM_COM];
void setupSerial();
#endif //SERIALBRIDGE_H

@ -180,8 +180,8 @@ int ArduinoNunchuk::update(double &speed, double &steer)
if(analogX > analogX_max) analogX_max = analogX;
if(analogY > analogY_max) analogY_max = analogY;
}
steer = 0;
speed = 0;
steer = 0.0;
speed = 0.0;
} else if(!cButton && zButton && !zButton_last && (rollangle() > 90.0 || rollangle() < -90.0) ) {
Serial2.print("1234567"); // Insert padding byte into serial stream to realign serial buffer
} else
@ -200,8 +200,8 @@ int ArduinoNunchuk::update(double &speed, double &steer)
else speed = (analogY - analogY_zero) * 1000.0/(analogY_zero - analogY_min) * NUNCHUCK_JOYSTICK_SPEED_MULT;
} else {
steer = 0;
speed = 0;
steer = 0.0;
speed = 0.0;
}
}
return error;
@ -215,7 +215,6 @@ uint8_t ArduinoNunchuk::_sendByte(byte data, byte location)
if(Wire.write(data) != 1) return 9;
return Wire.endTransmission();
// delay(10); //TODO was: 10ms, is it necessary?
}
void ArduinoNunchuk::debug(Stream &port)

@ -1,3 +1,4 @@
#pragma once
/*
* ArduinoNunchuk.h - Improved Wii Nunchuk library for Arduino
*
@ -12,8 +13,6 @@
*
*/
#ifndef ArduinoNunchuk_H
#define ArduinoNunchuk_H
#include <Arduino.h>
/************************* Nunchuk *******************************/
@ -66,6 +65,4 @@ class ArduinoNunchuk
private:
uint8_t _sendByte(byte data, byte location);
};
#endif
};

@ -1,5 +1,4 @@
#ifndef CRC_H
#define CRC_H
#pragma once
/* Simple public domain implementation of the standard CRC32 checksum.
* Outputs the checksum for each file given as a command line argument.
@ -48,6 +47,4 @@ int main(int ac, char** av) {
}
return 0;
}
*/
#endif
*/

@ -1,6 +1,4 @@
#ifndef Gametrak_H
#define Gametrak_H
#pragma once
#include <Arduino.h>
@ -42,7 +40,4 @@ class Gametrak {
uint8_t pin_theta;
bool inv_phi;
bool inv_theta;
};
#endif
};

@ -19,7 +19,7 @@
//#include "stm32f1xx_hal.h"
//#include "defines.h"
//#include "config.h"
#include "config.h"
//#include "sensorcoms.h"
#include "protocol.h"
//#include "hallinterrupts.h"

@ -19,7 +19,7 @@
#ifndef PROCOTOL_H
#define PROCOTOL_H
//#include "config.h"
#include "config.h"
#ifdef INCLUDE_PROTOCOL

@ -11,12 +11,12 @@
[platformio]
;env_default = featheresp32_ESPSLAVE
env_default = TTGO_ESPMASTER
;env_default = TTGO_ESPMASTER
;env_default = TTGO_ESPMASTER_PADDELEC
;env_default = nodemcu-32s_WIFI
;env_default = nodemcu-32s_ESPSLAVE
;env_default = featheresp32_psycho
;env_default = nodemcu-woodenboard
[env:featheresp32_ESPSLAVE]
@ -37,13 +37,13 @@ build_flags =
-D SERIAL1_RXPIN=15 ; receive Pin UART1
-D SERIAL1_TXPIN=02 ; transmit Pin UART1
-D MOTOR_COM=2 ; motor control output
-D UART_BAUD2=19200 ; Baudrate UART2
-D UART_BAUD2=9600 ; Baudrate UART2
-D SERIAL2_RXPIN=16 ; receive Pin UART2
-D SERIAL2_TXPIN=17 ; transmit Pin UART2
-D SERIAL2_GNDPIN=19 ; GND reference for UART
-D ESPnowSLAVE
-D INPUT_ESPnowSLAVE
-D OLED
-D INCLUDE_PROTOCOL
-D OUTPUT_PROTOCOL
; -D INPUT_IMU
; -D IMU_GNDPIN=14
; -D IMU_VCCPIN=32
@ -72,7 +72,7 @@ build_flags =
-D SERIAL2_RXPIN=16 ; receive Pin UART2
-D SERIAL2_TXPIN=17 ; transmit Pin UART2
-D TTGO
-D ESPnowMASTER
-D OUTPUT_ESPnowMASTER
-D OLED
-D INPUT_IMU
-D IMU_CPIN=23
@ -102,7 +102,7 @@ build_flags =
-D SERIAL2_RXPIN=16 ; receive Pin UART2
-D SERIAL2_TXPIN=17 ; transmit Pin UART2
-D TTGO
; -D ESPnowMASTER
-D OUTPUT_ESPnowMASTER
-D OLED
-D INPUT_PADDELECIMU
-D IMU_CPIN=23
@ -112,6 +112,7 @@ build_flags =
[env:nodemcu-32s_WIFI]
; 2018-11-12/phail: Tested, this is running on the board
; 2018-11-16/phail: Obsolete now. Only kept to have something which has the WIFI option..
board = nodemcu-32s
platform = espressif32
framework = arduino
@ -134,16 +135,17 @@ build_flags =
-D SERIAL2_RXPIN=16 ; receive Pin UART2
-D SERIAL2_TXPIN=17 ; transmit Pin UART2
-D WIFI
; -D BLE
-D NUNCHUCK
; -D INPUT_BLE
-D INPUT_NUNCHUCK
-D NUNCHUCK_VCCPIN=18
-D NUNCHUCK_GNDPIN=19
; -D PLATOONING
; -D INPUT_PLATOONING
-D GAMETRAK1_RPIN=33 ; wire length Pin Gametrak 1
-D GAMETRAK1_PHIPIN=35 ; horizontal angle Pin Gametrak 1
-D GAMETRAK1_THETAPIN=32 ; vertical angle Pin Gametrak 1
-D GAMETRAK1_PHI_REV=1 ; Phi is inverted
-D GAMETRAK1_THETA_REV=0 ; Theta is inverted
-D OUTPUT_BINARY_CRC
[env:nodemcu-32s_ESPSLAVE]
@ -166,23 +168,24 @@ build_flags =
-D SERIAL1_RXPIN=15 ; receive Pin UART1
-D SERIAL1_TXPIN=02 ; transmit Pin UART1
-D MOTOR_COM=2 ; motor control output
-D UART_BAUD2=19200 ; Baudrate UART2
-D UART_BAUD2=9600 ; Baudrate UART2
-D SERIAL2_RXPIN=16 ; receive Pin UART2
-D SERIAL2_TXPIN=17 ; transmit Pin UART2
; -D WIFI
; -D BLE
; -D NUNCHUCK
; -D INPUT_BLE
; -D INPUT_NUNCHUCK
; -D NUNCHUCK_VCCPIN=18
; -D NUNCHUCK_GNDPIN=19
; -D PLATOONING
; -D INPUT_PLATOONING
; -D GAMETRAK1_RPIN=33 ; wire length Pin Gametrak 1
; -D GAMETRAK1_PHIPIN=35 ; horizontal angle Pin Gametrak 1
; -D GAMETRAK1_THETAPIN=32 ; vertical angle Pin Gametrak 1
; -D GAMETRAK1_PHI_REV=1 ; Phi is inverted
; -D GAMETRAK1_THETA_REV=0 ; Theta is inverted
-D ESPnowSLAVE
-D INPUT_ESPnowSLAVE
; -D OLED
; -D INCLUDE_PROTOCOL
; -D OUTPUT_PROTOCOL
-D OUTPUT_BINARY_CRC
@ -211,9 +214,10 @@ build_flags =
-D SERIAL2_TXPIN=17 ; transmit Pin UART2
-D WIFI
-D OUTPUT_BINARY_CRC
; Nunchuck
-D NUNCHUCK
-D INPUT_NUNCHUCK
-D NUNCHUCK_GNDPIN=26
; Gametrak 1
@ -228,4 +232,42 @@ build_flags =
-D GAMETRAK2_RPIN=39 ; wire length Pin Gametrak 1
-D GAMETRAK2_THETAPIN=36 ; vertical angle Pin Gametrak 1
-D GAMETRAK2_PHI_REV=1 ; Phi is inverted
-D GAMETRAK2_THETA_REV=0 ; Theta is inverted
-D GAMETRAK2_THETA_REV=0 ; Theta is inverted
[env:nodemcu-woodenboard]
board = nodemcu-32s
platform = espressif32
framework = arduino
lib_deps = U8g2, I2Cdevlib-MPU6050
board_build.partitions = min_spiffs.csv
monitor_speed = 115000
;upload_port = 192.168.4.1
build_flags =
-Iinclude
-D NUM_COM=3 ; total number of COM Ports
-D DEBUG_COM=0 ; debug output
-D UART_BAUD0=115200 ; Baudrate UART0
-D SERIAL0_RXPIN=03 ; receive Pin UART0
-D SERIAL0_TXPIN=01 ; transmit Pin UART0
-D UART_BAUD1=9600 ; Baudrate UART1
-D SERIAL1_RXPIN=15 ; receive Pin UART1
-D SERIAL1_TXPIN=02 ; transmit Pin UART1
-D MOTOR_COM=2 ; motor control output
-D UART_BAUD2=19200 ; Baudrate UART2
-D SERIAL2_RXPIN=16 ; receive Pin UART2
-D SERIAL2_TXPIN=17 ; transmit Pin UART2
; -D WIFI
; -D INPUT_BLE
-D INPUT_NUNCHUCK
-D NUNCHUCK_VCCPIN=18
-D NUNCHUCK_GNDPIN=19
-D INPUT_PLATOONING
-D GAMETRAK1_RPIN=33 ; wire length Pin Gametrak 1
-D GAMETRAK1_PHIPIN=35 ; horizontal angle Pin Gametrak 1
-D GAMETRAK1_THETAPIN=32 ; vertical angle Pin Gametrak 1
-D GAMETRAK1_PHI_REV=1 ; Phi is inverted
-D GAMETRAK1_THETA_REV=0 ; Theta is inverted
-D INPUT_ESPnowSLAVE
-D OUTPUT_BINARY_CRC

@ -1,8 +1,9 @@
#if defined(INPUT_PADDELECIMU) || defined(INPUT_PADDELEC)
#include <Arduino.h>
#include "Paddelec.h"
#if defined(INPUT_PADDELECIMU) || defined(INPUT_PADDELEC)
void Paddelec::update(double &pwm, double &steer, double &actualSpeed_kmh, double &actualSteer_kmh, uint32_t deltaMillis) {

@ -1,8 +1,7 @@
#ifdef PLATOONING
#include <Arduino.h>
#include "Platooning.h"
#ifdef INPUT_PLATOONING
void Platooning::update(double &pwm, double &steer) {

@ -9,12 +9,13 @@ void mainloop(void *pvParameters);
void motorCommunication(void *pvParameters);
#endif //MULTITASKING
#ifdef INCLUDE_PROTOCOL
#ifdef OUTPUT_PROTOCOL
#include <protocol.h>
size_t send_serial_data( const uint8_t *data, size_t len ) {
COM[DEBUG_COM]->write(data,len);
COM[DEBUG_COM]->println();
return COM[MOTOR_COM]->write(data,len);
// COM[DEBUG_COM]->write(data,len);
}
#endif
@ -23,20 +24,21 @@ void motorCommunication(void *pvParameters);
Paddelec paddelec = Paddelec();
#endif // INPUT_PADDELEC
#ifdef NUNCHUCK
#ifdef INPUT_NUNCHUCK
#include <ArduinoNunchuk.h>
ArduinoNunchuk nunchuk = ArduinoNunchuk();
#endif // NUNCHUCK
#endif // INPUT_NUNCHUCK
#ifdef ESPnowMASTER
#ifdef OUTPUT_ESPnowMASTER
#include "ESPnowMaster.h"
#endif
#ifdef ESPnowSLAVE
#ifdef INPUT_ESPnowSLAVE
#include "ESPnowSlave.h"
bool ESPnowDataReceived = false;
#endif
#ifdef PLATOONING
#ifdef INPUT_PLATOONING
#include "Platooning.h"
Platooning platooning = Platooning();
#endif
@ -45,7 +47,7 @@ void motorCommunication(void *pvParameters);
#include "oled.h"
#endif
#ifdef BLE
#ifdef INPUT_BLE
#include "BLE.h"
#endif
@ -74,11 +76,11 @@ void setup() {
setupSerialbridge();
#endif
#ifdef ESPnowMASTER
#ifdef OUTPUT_ESPnowMASTER
setupESPnowMaster();
#endif
#ifdef ESPnowSLAVE
#ifdef INPUT_ESPnowSLAVE
setupESPnowSlave();
#endif
@ -90,11 +92,11 @@ void setup() {
setupOTA();
#endif
#ifdef NUNCHUCK
#ifdef INPUT_NUNCHUCK
nunchuk.init();
#endif
#ifdef BLE
#ifdef INPUT_BLE
setupBLE();
#endif
@ -138,6 +140,7 @@ double limit(double min, double value, double max) {
return value;
}
// Incrementally decrease variable
void slowReset(double &variable, double goal, double step) {
if ((variable - goal) > step) variable -= step;
else if ((goal - variable) > step) variable += step;
@ -156,7 +159,7 @@ void updateSpeed() {
motor.actualSteer_kmh = motor.actualSteer_kmh * (1.0 - (SPEED_FILTER * deltaMillis)) + motor.steer * (SPEED_FILTER * deltaMillis) * SPEED_PWM_CONVERSION_FACTOR;
}
#ifdef ESPnowMASTER
#ifdef OUTPUT_ESPnowMASTER
bool isPaired = false;
#endif
@ -167,8 +170,8 @@ void loop() {
}
void mainloop( void *pvparameters ) {
int taskno = (int)pvparameters;
while(1) {
int taskno = (int)pvparameters;
while(1) {
#endif //MULTITASKING
#ifdef OTA_HANDLER
@ -182,30 +185,81 @@ void mainloop( void *pvparameters ) {
deltaMillis = millis() - millisMotorcomm;
millisMotorcomm = millis();
// Process all Inputs
do {
slowReset(motor.pwm, 0.0, 10.0);
slowReset(motor.steer, 0.0, 10.0);
#if defined(INPUT_PADDELEC) || defined(INPUT_PADDELECIMU)
paddelec.update(motor.pwm, motor.steer, motor.actualSpeed_kmh, motor.actualSteer_kmh, (uint32_t)deltaMillis);
if(debug) {
paddelec.debug(*COM[DEBUG_COM]);
}
#endif // INPUT_PADDELEC
#ifdef BLE
// loopBLE();
motor.pwm = ble_pitch;
motor.steer = ble_roll;
slowReset(ble_pitch, 0.0, 0.1);
slowReset(ble_roll, 0.0, 0.1);
#ifdef INPUT_ESPnowSLAVE
// Disable all other Input Methods as soon as data from ESPnow was received
if(ESPnowDataReceived) break;
#endif
#ifdef INPUT_BLE
// loopBLE();
motor.pwm = ble_pitch;
motor.steer = ble_roll;
slowReset(ble_pitch, 0.0, 0.1);
slowReset(ble_roll, 0.0, 0.1);
break;
#endif
#ifdef INPUT_IMU
// imu.loopIMU();
// imu.loopIMU();
imu.update(motor.pwm, motor.steer);
if(debug) imu.debug(*COM[DEBUG_COM]);
// Allow other Inputs when no Button is pressed
if(imu.cButton == 1) break;
#endif
#ifdef INPUT_PLATOONING
platooning.update(motor.pwm, motor.steer);
if(debug) platooning.debug(*COM[DEBUG_COM]);
// Allow other Inputs when Gametrak is not pulled out
if(platooning.gametrak1.getR_mm() > platooning.cfgPlatooning.rActivationThreshold_mm) break;
#endif
#if defined(INPUT_NUNCHUCK)
int nunchuckError = nunchuk.update(motor.pwm, motor.steer);
++errorCount;
nunchuk.debug(*COM[DEBUG_COM]);
if(nunchuckError >= 1000) {
if(debug) COM[DEBUG_COM]->printf("Reinit Nunchuck %4i ", nunchuckError);
} else if(nunchuckError >= 100) {
if(debug) COM[DEBUG_COM]->printf("I2C Problems %4i ", nunchuckError);
} else if(nunchuckError > 0) {
if(debug) COM[DEBUG_COM]->printf("Nunchuck Comm Problems %4i ", nunchuckError);
} else {
errorCount = 0;
}
if(errorCount>=5) {
nunchuk.reInit();
errorCount = 0;
}
// Allow other input when Nunchuck does not send speed
if(motor.pwm != 0.0 && motor.steer != 0.0 ) break;
#endif
#ifdef INPUT_PADDELEC
paddelec.update(motor.pwm, motor.steer, motor.actualSpeed_kmh, motor.actualSteer_kmh, (uint32_t)deltaMillis);
if(debug) paddelec.debug(*COM[DEBUG_COM]);
// Allow other Inputs when Gametraks are not pulled out
if(paddelec.gametrak1.r > 500 || paddelec.gametrak2.r > 500) break;
#endif
#ifdef INPUT_PADDELECIMU
paddelec.update(motor.pwm, motor.steer, motor.actualSpeed_kmh, motor.actualSteer_kmh, (uint32_t)deltaMillis);
if(debug) paddelec.debug(*COM[DEBUG_COM]);
#endif
} while(false);
#ifdef OLED
uint mX = u8g2.getDisplayWidth()/2;
uint mY = u8g2.getDisplayHeight()/2;
@ -222,6 +276,16 @@ void mainloop( void *pvparameters ) {
double gZ = imu.gz / 32768.0 * u8g2.getDisplayHeight()/2.0;
#endif
#ifdef INPUT_PADDELECIMU
double pwmR=0.0, pwmL=0.0;
paddelec.steerToRL(motor.steer, motor.pwm, pwmR, pwmL);
pwmR = -pwmR / 1000.0 * u8g2.getDisplayHeight()/2.0;
pwmL = -pwmL / 1000.0 * u8g2.getDisplayHeight()/2.0;
double pitchangle = paddelec.imu.pitchangle();
#endif
u8g2.firstPage();
do {
@ -245,6 +309,18 @@ void mainloop( void *pvparameters ) {
if(gZ>0) u8g2.drawFrame(u8g2.getDisplayWidth()-3 ,mY ,1, gZ);
else u8g2.drawFrame(u8g2.getDisplayWidth()-3 ,mY+gZ,1,-gZ);
#endif
#ifdef INPUT_PADDELECIMU
if(pwmL>0) u8g2.drawFrame(0 ,mY ,1, pwmL);
else u8g2.drawFrame(0 ,mY+pwmL,1,-pwmL);
if(pwmR>0) u8g2.drawFrame(u8g2.getDisplayWidth()-1 ,mY ,1, pwmR);
else u8g2.drawFrame(u8g2.getDisplayWidth()-1 ,mY+pwmR,1,-pwmR);
u8g2.setCursor(0,mY+30);
u8g2.printf("Pitch%4.0f", pitchangle);
#endif
u8g2.setCursor(5,5);
@ -252,50 +328,13 @@ void mainloop( void *pvparameters ) {
u8g2.drawLine(mX,mY,motorX,motorY);
#ifdef ESPnowSLAVE
#ifdef INPUT_ESPnowSLAVE
u8g2.setCursor(5,15);
u8g2.printf("%4i", ESPnowdata);
#endif
} while( u8g2.nextPage() );
#endif
#if defined(NUNCHUCK) && (defined(INPUT_PADDELEC) || defined(INPUT_PADDELECIMU))
if(paddelec.gametrak1.r < 500 || paddelec.gametrak2.r < 500) { // switch to nunchuck control when paddle is not used
#endif
#if defined(NUNCHUCK)
int nunchuckError = nunchuk.update(motor.pwm, motor.steer);
++errorCount;
nunchuk.debug(*COM[DEBUG_COM]);
if(nunchuckError >= 1000) {
if(debug) COM[DEBUG_COM]->printf("Reinit Nunchuck %4i ", nunchuckError);
} else if(nunchuckError >= 100) {
if(debug) COM[DEBUG_COM]->printf("I2C Problems %4i ", nunchuckError);
} else if(nunchuckError > 0) {
if(debug) COM[DEBUG_COM]->printf("Nunchuck Comm Problems %4i ", nunchuckError);
} else {
errorCount = 0;
}
if(errorCount>=5) {
nunchuk.reInit();
errorCount = 0;
}
#endif
#if defined(NUNCHUCK) && (defined(INPUT_PADDELEC) || defined(INPUT_PADDELECIMU))
}
#endif
#ifdef PLATOONING
platooning.update(motor.pwm, motor.steer);
if(debug) {
platooning.debug(*COM[DEBUG_COM]);
#ifdef WIFI
for(byte cln = 0; cln < MAX_NMEA_CLIENTS; cln++)
if(TCPClient[1][cln]) platooning.debug(TCPClient[1][cln]);
#endif
}
#endif // PLATOONING
#ifdef MULTITASKING
vTaskDelay(20);
}
@ -307,10 +346,10 @@ void motorCommunication( void * pvparameters) {
while(1) {
#endif //MULTITASKING
#ifdef ESPnowMASTER
#ifdef OUTPUT_ESPnowMASTER
if(!isPaired) {
ScanForSlave();
// If Slave is found, it would be populate in `slave` variable
// If Slave is found, it would be populated in `slave` variable
// We will check if `slave` is defined and then we proceed further
if (slave.channel == CHANNEL) { // check if slave channel is defined
// `slave` is defined
@ -320,10 +359,8 @@ void motorCommunication( void * pvparameters) {
} else {
sendData((const void *) &motor, sizeof(motor));
}
// wait for 3seconds to run the logic again
delay(30);
#elif INCLUDE_PROTOCOL
#endif
#ifdef OUTPUT_PROTOCOL
PROTOCOL_MSG newMsg;
memset((void*)&newMsg,0x00,sizeof(PROTOCOL_MSG));
PROTOCOL_MSG *msg = &newMsg;
@ -350,20 +387,25 @@ void motorCommunication( void * pvparameters) {
{
protocol_byte( COM[MOTOR_COM]->read() );
}
#else
#endif
#ifdef OUTPUT_BINARY
/* cast & limit values to a valid range */
int16_t steer = (int16_t) limit(-1000.0, motor.steer, 1000.0);
int16_t pwm = (int16_t) limit(-1000.0, motor.pwm, 1000.0);
/* calc checksum */
/* Send motor pwm values to motor control unit */
COM[MOTOR_COM]->write((uint8_t *) &steer, sizeof(steer));
COM[MOTOR_COM]->write((uint8_t *) &pwm, sizeof(pwm));
#ifdef OUTPUT_BINARY_CRC
/* calc and send checksum */
uint32_t crc = 0;
crc32((const void *)&steer, sizeof(steer), &crc);
crc32((const void *)&pwm, sizeof(pwm), &crc);
/* Send motor pwm values to motor control unit */
COM[MOTOR_COM]->write((uint8_t *) &steer, sizeof(steer));
COM[MOTOR_COM]->write((uint8_t *) &pwm, sizeof(pwm));
COM[MOTOR_COM]->write((uint8_t *) &crc, sizeof(crc));
#endif
/* refresh actual motor speed */
updateSpeed();
@ -383,17 +425,7 @@ void motorCommunication( void * pvparameters) {
if(debug) COM[DEBUG_COM]->printf("%7.2f %7.2f ", motor.actualSpeed_kmh, motor.actualSteer_kmh);
#endif
#ifdef MULTITASKING
delay(MOTORINPUT_PERIOD);
delay(MOTORINPUT_PERIOD);
}
#endif //MULTITASKING
}
/* TODO
* calculate paddle angle. if only gametrak angles are subtracted, the activation threshold depends on the distance (r)
* independet variables for left and right wheel, recover MIXER
* CRC checksum, maybe message counter?
* perfect freewheeling, paddle strenght effect based on paddle angle
* define minimum r to function
* check phi vor valid values
* use timer to get constant deltas
* put everything inti functions
*/
}
Loading…
Cancel
Save