copy send and receive functions to new project for controller with teensy32
This commit is contained in:
parent
227005f1d6
commit
e62286cb8b
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
||||
|
|
@ -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();
|
||||
|
||||
}
|
||||
}
|
||||
Loading…
Reference in New Issue