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