Last active
January 28, 2024 18:29
-
-
Save sbasami/06c419e61e01a2bc9e5d60622069f288 to your computer and use it in GitHub Desktop.
Teensy4.1でKISS ESCとDSHOT通信するプログラム
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#include <Arduino.h> | |
#include "DSHOT.h" | |
#include "ESCCMD.h" | |
#define ESCPID_USB_UART_SPEED 115200 //シリアル通信の速度 | |
#define ESCPID_NB_ESC 1 //ESCの数 | |
const float LOGGING_CYCLE = 0.01; //ログ周期[s] | |
IntervalTimer Timer; //10msの割り込み | |
int16_t val = 0; //指令値を格納する変数 | |
typedef struct { | |
uint32_t magic; // Magic number | |
int8_t err[ESCPID_NB_ESC]; // Last error number | |
uint8_t deg[ESCPID_NB_ESC]; // ESC temperature (°C) | |
uint16_t cmd[ESCPID_NB_ESC]; // Current ESC command value | |
uint16_t volt[ESCPID_NB_ESC]; // Voltage of the ESC power supply (0.01V) | |
uint16_t amp[ESCPID_NB_ESC]; // ESC current (0.01A) | |
int16_t rpm[ESCPID_NB_ESC]; // Motor rpm (10 rpm) | |
} ESCPIDcomm_struct_t; | |
ESCPIDcomm_struct_t ESCPID_comm = { | |
{}, | |
{}, | |
{}, | |
{}, | |
{}, | |
{}, | |
{} | |
}; | |
void setup() { | |
Serial.begin( ESCPID_USB_UART_SPEED ); | |
// Initialize the CMD subsystem | |
ESCCMD_init( ESCPID_NB_ESC ); | |
// Arming ESCs | |
ESCCMD_arm_all( ); | |
// Switch 3D mode on | |
ESCCMD_3D_off( ); | |
// Arming ESCs | |
ESCCMD_arm_all( ); | |
// Start periodic loop | |
ESCCMD_start_timer( ); | |
// Stop all motors | |
for (int i = 0; i < ESCPID_NB_ESC; i++ ) { | |
ESCCMD_stop( i ); | |
} | |
Timer.priority(200); | |
Timer.begin(interrupt_10ms, LOGGING_CYCLE * 1000000); //10msでタイマー割り込み開始 | |
} | |
void interrupt_10ms(void) | |
{ | |
//シリアルモニタに表示 | |
for (int i = 0; i < ESCPID_NB_ESC; i++ ) { | |
Serial.print("Channel:\t"); | |
Serial.println(i); | |
Serial.print("Error:\t\t"); | |
Serial.println(ESCPID_comm.err[i]); | |
Serial.print("Command:\t"); | |
Serial.println(ESCPID_comm.cmd[i]); | |
Serial.print("Temperature:\t"); | |
Serial.println(ESCPID_comm.deg[i]); | |
Serial.print("Voltage:\t"); | |
Serial.println(ESCPID_comm.volt[i]*0.01); | |
Serial.print("Current:\t"); | |
Serial.println(ESCPID_comm.amp[i]*0.01); | |
Serial.print("rpm:\t\t"); | |
Serial.println(ESCPID_comm.rpm[i]); | |
Serial.println(); | |
} | |
if (KICK_SERIAL()) | |
{ | |
val = SERIAL_VAL();//シリアル通信で送られてきた指令値 | |
} | |
} | |
void loop() { | |
static int ret; | |
ret = ESCCMD_tic();//テレメトリ情報の更新 | |
//テレメトリ情報の取得 | |
for (int i = 0; i < ESCPID_NB_ESC; i++ ) { | |
ESCCMD_read_err(i, &ESCPID_comm.err[i]); | |
ESCCMD_read_cmd(i, &ESCPID_comm.cmd[i]); | |
ESCCMD_read_deg(i, &ESCPID_comm.deg[i]); | |
ESCCMD_read_volt(i, &ESCPID_comm.volt[i]); | |
ESCCMD_read_amp(i, &ESCPID_comm.amp[i]); | |
ESCCMD_read_rpm(i, &ESCPID_comm.rpm[i]); | |
} | |
//ESCへ指令値送信 | |
if (ret == ESCCMD_TIC_OCCURED) | |
{ | |
for (int i = 0; i < ESCPID_NB_ESC; i++) | |
{ | |
ret = ESCCMD_throttle(i, val); | |
} | |
} | |
} | |
//シリアル通信の受信処理 | |
bool KICK_SERIAL() { | |
bool flag = false; | |
if (Serial.available() > 0) | |
{ | |
flag = true; | |
//delay(20); | |
} | |
return flag; | |
} | |
//シリアル通信の受信処理 | |
int SERIAL_VAL() { | |
byte data_size = Serial.available(); | |
byte buf[data_size], degree = 1; | |
long recv_data = 0, dub = 1; | |
bool minus = 0; | |
for (byte i = 0 ; i < data_size ; i++) | |
{ | |
buf[i] = Serial.read(); | |
if (buf[i] >= '0' && buf[i] <= '9') buf[i] -= '0'; | |
else { | |
if (buf[0] == '-') minus = 1; | |
else degree = 0; | |
} | |
} | |
if (degree == 1) degree = data_size - minus; | |
for (byte i = 0 ; i < degree ; i++) | |
{ | |
recv_data += buf[(data_size - 1) - i] * dub; | |
dub *= 10; | |
} | |
if (minus) recv_data *= -1; | |
return recv_data; | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment