copy send and receive functions to new project for controller with teensy32

This commit is contained in:
interfisch 2021-02-26 19:37:12 +01:00
parent 227005f1d6
commit e62286cb8b
3 changed files with 225 additions and 0 deletions

View File

@ -0,0 +1,46 @@
This directory is intended for project specific (private) libraries.
PlatformIO will compile them to static libraries and link into executable file.
The source code of each library should be placed in a an own separate directory
("lib/your_library_name/[here are source files]").
For example, see a structure of the following two libraries `Foo` and `Bar`:
|--lib
| |
| |--Bar
| | |--docs
| | |--examples
| | |--src
| | |- Bar.c
| | |- Bar.h
| | |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html
| |
| |--Foo
| | |- Foo.c
| | |- Foo.h
| |
| |- README --> THIS FILE
|
|- platformio.ini
|--src
|- main.c
and a contents of `src/main.c`:
```
#include <Foo.h>
#include <Bar.h>
int main (void)
{
...
}
```
PlatformIO Library Dependency Finder will find automatically dependent
libraries scanning project source files.
More information about PlatformIO Library Dependency Finder
- https://docs.platformio.org/page/librarymanager/ldf.html

View File

@ -0,0 +1,15 @@
; 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
[env:teensy31]
platform = teensy
board = teensy31
framework = arduino

View File

@ -0,0 +1,164 @@
#include <Arduino.h>
// ########################## DEFINES ##########################
#define SERIAL_CONTROL_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 SENDPERIOD 50 //ms. delay for sending speed and steer data to motor controller via serial
int16_t out_speedFL=0;
int16_t out_speedFR=0;
int16_t out_speedRL=0;
int16_t out_speedRR=0;
unsigned long last_send = 0;
void SendSerial(SerialCommand &scom, int16_t uSpeedLeft, int16_t uSpeedRight);
void ReceiveSerial(SerialRead &sread, SerialFeedback &Feedback,SerialFeedback &NewFeedback);
// Global variables for serial communication
typedef struct{
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;
long lastValidDataSerial_time;
long boardlastPoweron=0; //mainly for failcheck
long boardlastPoweroff=0;
} SerialRead;
SerialRead SerialcomFront;
typedef struct{
uint16_t start;
int16_t speedLeft;
int16_t speedRight;
uint16_t checksum;
} SerialCommand;
SerialCommand CommandFront;
typedef struct{
uint16_t start;
int16_t cmd1;
int16_t cmd2;
int16_t speedL_meas;
int16_t speedR_meas;
int16_t batVoltage;
int16_t boardTemp;
int16_t curL_DC;
int16_t curR_DC;
uint16_t cmdLed;
uint16_t checksum;
} SerialFeedback;
SerialFeedback FeedbackFront;
SerialFeedback NewFeedbackFront;
void SendSerial(SerialCommand &scom, int16_t uSpeedLeft, int16_t uSpeedRight)
{
// Create command
scom.start = (uint16_t)START_FRAME;
scom.speedLeft = (int16_t)uSpeedLeft;
scom.speedRight = (int16_t)uSpeedRight;
scom.checksum = (uint16_t)(scom.start ^ scom.speedLeft ^ scom.speedRight);
Serial1.write((uint8_t *) &scom, sizeof(scom)); //TODO: use correct serial object
}
void ReceiveSerial(SerialRead &sread, SerialFeedback &Feedback,SerialFeedback &NewFeedback)
{
// Check for new data availability in the Serial buffer
if ( Serial1.available() ) {
sread.incomingByte = Serial1.read(); // Read the incoming byte
sread.bufStartFrame = ((uint16_t)(sread.incomingByte) << 8) | sread.incomingBytePrev; // Construct the start frame
}
else {
return;
}
// If DEBUG_RX is defined print all incoming bytes
#ifdef DEBUG_RX
Serial.print(sread.incomingByte);
return;
#endif
// Copy received data
if (sread.bufStartFrame == START_FRAME) { // Initialize if new data is detected
sread.p = (byte *)&NewFeedback;
*sread.p++ = sread.incomingBytePrev;
*sread.p++ = sread.incomingByte;
sread.idx = 2;
} else if (sread.idx >= 2 && sread.idx < sizeof(SerialFeedback)) { // Save the new received data
*sread.p++ = sread.incomingByte;
sread.idx++;
}
// Check if we reached the end of the package
if (sread.idx == sizeof(SerialFeedback)) {
uint16_t checksum;
checksum = (uint16_t)(NewFeedback.start ^ NewFeedback.cmd1 ^ NewFeedback.cmd2
^ NewFeedback.speedR_meas ^ NewFeedback.speedL_meas ^ NewFeedback.batVoltage ^ NewFeedback.boardTemp ^ NewFeedback.curL_DC ^ NewFeedback.curR_DC ^ NewFeedback.cmdLed);
// Check validity of the new data
if (NewFeedback.start == START_FRAME && checksum == NewFeedback.checksum) {
// Copy the new data
memcpy(&Feedback, &NewFeedback, sizeof(SerialFeedback));
sread.lastValidDataSerial_time = millis();
}
sread.idx = 0; // Reset the index (it prevents to enter in this if condition in the next cycle)
/*
// Print data to built-in 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);
} else {
Serial.println("Non-valid data skipped");
}*/
// Update previous states
sread.incomingBytePrev = sread.incomingByte;
}
// ########################## SETUP ##########################
void setup()
{
Serial.begin(SERIAL_BAUD); //Debug and Program. A9=TX1, A10=RX1 (3v3 level)
Serial1.begin(SERIAL_CONTROL_BAUD); //control. A2=TX2, A3=RX2 (Serial1 is Usart 2). Marked with "1" on connector (Rear)
Serial2.begin(SERIAL_CONTROL_BAUD); //control. B10=TX3, B11=RX3 (Serial2 is Usart 3). Marked with "II" on connector (Front)
}
// ########################## LOOP ##########################
void loop() {
loopmillis=millis(); //read millis for this cycle
ReceiveSerial();
if (loopmillis - last_send > SENDPERIOD) {
last_send=loopmillis;
SendSerial(out_speedRL,out_speedRR);
float _current = (Feedback1.curL_DC+Feedback1.curR_DC+Feedback2.curL_DC+Feedback2.curR_DC)/4.0 / 50;
Serial.print(Feedback1.curL_DC); Serial.print(", "); //1 is rear
Serial.print(Feedback1.curR_DC); Serial.print(", ");
Serial.print(Feedback2.curL_DC); Serial.print(", "); //2 is front
Serial.print(Feedback2.curR_DC);
Serial.print(", mean="); Serial.print(_current);
Serial.println();
}
}