mirror of
https://github.com/okalachev/flix.git
synced 2025-07-27 17:49:33 +00:00
More cleanups
This commit is contained in:
parent
88cc792287
commit
f84d1e95dd
15
flix/cli.ino
15
flix/cli.ino
@ -5,10 +5,6 @@
|
|||||||
|
|
||||||
#include "pid.h"
|
#include "pid.h"
|
||||||
|
|
||||||
static String command;
|
|
||||||
static String value;
|
|
||||||
static bool parsingCommand = true;
|
|
||||||
|
|
||||||
extern PID rollRatePID, pitchRatePID, yawRatePID, rollPID, pitchPID;
|
extern PID rollRatePID, pitchRatePID, yawRatePID, rollPID, pitchPID;
|
||||||
|
|
||||||
const char* motd =
|
const char* motd =
|
||||||
@ -34,8 +30,6 @@ const char* motd =
|
|||||||
"fullmot <n> - test motor on all signals\n"
|
"fullmot <n> - test motor on all signals\n"
|
||||||
"wifi - start wi-fi access point\n";
|
"wifi - start wi-fi access point\n";
|
||||||
|
|
||||||
bool showMotd = true;
|
|
||||||
|
|
||||||
static const struct Param {
|
static const struct Param {
|
||||||
const char* name;
|
const char* name;
|
||||||
float* value;
|
float* value;
|
||||||
@ -57,7 +51,7 @@ static const struct Param {
|
|||||||
// {"m", &mode, nullptr},
|
// {"m", &mode, nullptr},
|
||||||
};
|
};
|
||||||
|
|
||||||
static void doCommand()
|
static void doCommand(String& command, String& value)
|
||||||
{
|
{
|
||||||
if (command == "help" || command == "motd") {
|
if (command == "help" || command == "motd") {
|
||||||
Serial.println(motd);
|
Serial.println(motd);
|
||||||
@ -138,6 +132,11 @@ static void cliTestMotor(uint8_t n)
|
|||||||
|
|
||||||
void parseInput()
|
void parseInput()
|
||||||
{
|
{
|
||||||
|
static bool showMotd = true;
|
||||||
|
static String command;
|
||||||
|
static String value;
|
||||||
|
static bool parsingCommand = true;
|
||||||
|
|
||||||
if (showMotd) {
|
if (showMotd) {
|
||||||
Serial.println(motd);
|
Serial.println(motd);
|
||||||
showMotd = false;
|
showMotd = false;
|
||||||
@ -148,7 +147,7 @@ void parseInput()
|
|||||||
if (c == '\n') {
|
if (c == '\n') {
|
||||||
parsingCommand = true;
|
parsingCommand = true;
|
||||||
if (!command.isEmpty()) {
|
if (!command.isEmpty()) {
|
||||||
doCommand();
|
doCommand(command, value);
|
||||||
}
|
}
|
||||||
command.clear();
|
command.clear();
|
||||||
value.clear();
|
value.clear();
|
||||||
|
@ -26,7 +26,6 @@ float dt; // time delta from previous step, s
|
|||||||
float loopFreq; // loop frequency, Hz
|
float loopFreq; // loop frequency, Hz
|
||||||
uint16_t channels[16]; // raw rc channels
|
uint16_t channels[16]; // raw rc channels
|
||||||
float controls[RC_CHANNELS]; // normalized controls in range [-1..1] ([0..1] for throttle)
|
float controls[RC_CHANNELS]; // normalized controls in range [-1..1] ([0..1] for throttle)
|
||||||
uint32_t rcFailSafe, rcLostFrame;
|
|
||||||
float motors[4]; // normalized motors thrust in range [-1..1]
|
float motors[4]; // normalized motors thrust in range [-1..1]
|
||||||
Vector rates; // angular rates, rad/s
|
Vector rates; // angular rates, rad/s
|
||||||
Vector acc; // accelerometer data, m/s/s
|
Vector acc; // accelerometer data, m/s/s
|
||||||
|
@ -1,46 +1,25 @@
|
|||||||
// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
|
// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
|
||||||
// Repository: https://github.com/okalachev/flix
|
// Repository: https://github.com/okalachev/flix
|
||||||
|
|
||||||
// https://habr.com/ru/company/first/blog/664922/
|
// Motors output
|
||||||
// esc: https://aliexpress.com/item/4000280617058.html (KINGDUO Micro Mini 4A 1S Brushed Esc 3.6-6V)
|
// Motor: 8520 3.7V
|
||||||
// motor: https://aliexpress.com/item/32731613504.html (8520 3.7V)
|
// ESC: KINGDUO Micro Mini 4A 1S Brushed Esc 3.6-6V
|
||||||
|
|
||||||
#define MOTOR_0_PIN 12
|
#define MOTOR_0_PIN 12
|
||||||
#define MOTOR_1_PIN 13
|
#define MOTOR_1_PIN 13
|
||||||
#define MOTOR_2_PIN 14
|
#define MOTOR_2_PIN 14
|
||||||
#define MOTOR_3_PIN 15
|
#define MOTOR_3_PIN 15
|
||||||
|
|
||||||
// #define PWM_FREQUENCY 200
|
#define PWM_FREQUENCY 200
|
||||||
// #define PWM_FREQUENCY 50 // TODO: way low frequency considering the IMU is 1kHz
|
|
||||||
#define PWM_FREQUENCY 200 // WARNING: original 50
|
|
||||||
|
|
||||||
#define PWM_RESOLUTION 8
|
#define PWM_RESOLUTION 8
|
||||||
|
|
||||||
// #define PWM_MIN 1575
|
|
||||||
// #define PWM_MAX 2300
|
|
||||||
#define PWM_NEUTRAL 1500
|
#define PWM_NEUTRAL 1500
|
||||||
// #define PWM_REVERSE_MAX 700
|
|
||||||
// #define PWM_REVERSE_MIN 1425
|
|
||||||
|
|
||||||
// static const uint16_t pwmMin[] = {1600-50, 1600-50, 1600-50, 1600-50};
|
static const uint16_t pwmMin[] = {1600, 1600, 1600, 1600};
|
||||||
// static const uint16_t pwmMax[] = {2100, 2300, 2000, 2000}; // NOTE: ORIGINAL
|
|
||||||
|
|
||||||
static const uint16_t pwmMin[] = {1600, 1600, 1600, 1600}; // NOTE: success
|
|
||||||
// static const uint16_t pwmMax[] = {2000, 2000, 2000, 2000}; // NOTE: success
|
|
||||||
static const uint16_t pwmMax[] = {2300, 2300, 2300, 2300};
|
static const uint16_t pwmMax[] = {2300, 2300, 2300, 2300};
|
||||||
// from esc description
|
|
||||||
// static const uint16_t pwmMin[] = {1600, 1600, 1600, 1600};
|
|
||||||
// static const uint16_t pwmMax[] = {2300, 2300, 2300, 2300};
|
|
||||||
|
|
||||||
// static const uint16_t pwmReverseMin[] = {1420+50, 1440+50, 1440+50, 1440+50};
|
|
||||||
|
|
||||||
// static const uint16_t pwmReverseMin[] = {1420, 1440, 1440, 1440};
|
|
||||||
// static const uint16_t pwmReverseMax[] = {700, 1100, 1100, 1100}; // NOTE: ???
|
|
||||||
static const uint16_t pwmReverseMin[] = {1390, 1440, 1440, 1440};
|
static const uint16_t pwmReverseMin[] = {1390, 1440, 1440, 1440};
|
||||||
static const uint16_t pwmReverseMax[] = {1100, 1100, 1100, 1100};
|
static const uint16_t pwmReverseMax[] = {1100, 1100, 1100, 1100};
|
||||||
|
|
||||||
bool useBreak; // TODO: redesign
|
|
||||||
|
|
||||||
void setupMotors() {
|
void setupMotors() {
|
||||||
Serial.println("Setup Motors");
|
Serial.println("Setup Motors");
|
||||||
|
|
||||||
@ -56,12 +35,6 @@ void setupMotors() {
|
|||||||
ledcAttachPin(MOTOR_2_PIN, 2);
|
ledcAttachPin(MOTOR_2_PIN, 2);
|
||||||
ledcAttachPin(MOTOR_3_PIN, 3);
|
ledcAttachPin(MOTOR_3_PIN, 3);
|
||||||
|
|
||||||
// send initial break to initialize ESCs
|
|
||||||
// Serial.println("Calibrating ESCs");
|
|
||||||
// useBreak = true;
|
|
||||||
// sendMotors();
|
|
||||||
// delay(2000);
|
|
||||||
// useBreak = false;
|
|
||||||
sendMotors();
|
sendMotors();
|
||||||
Serial.println("Motors initialized");
|
Serial.println("Motors initialized");
|
||||||
}
|
}
|
||||||
@ -69,7 +42,7 @@ void setupMotors() {
|
|||||||
static uint16_t getPWM(float val, int n)
|
static uint16_t getPWM(float val, int n)
|
||||||
{
|
{
|
||||||
if (val == 0) {
|
if (val == 0) {
|
||||||
return PWM_NEUTRAL; // useBreak ? PWM_NEUTRAL : 0;
|
return PWM_NEUTRAL;
|
||||||
} else if (val > 0) {
|
} else if (val > 0) {
|
||||||
return mapff(val, 0, 1, pwmMin[n], pwmMax[n]);
|
return mapff(val, 0, 1, pwmMin[n], pwmMax[n]);
|
||||||
} else {
|
} else {
|
||||||
|
@ -18,8 +18,8 @@ void readRC()
|
|||||||
{
|
{
|
||||||
bool failSafe, lostFrame;
|
bool failSafe, lostFrame;
|
||||||
if (RC.read(channels, &failSafe, &lostFrame)) {
|
if (RC.read(channels, &failSafe, &lostFrame)) {
|
||||||
if (failSafe) { rcFailSafe++; return; } // TODO: NOT TESTED YET
|
if (failSafe) { return; } // TODO:
|
||||||
if (lostFrame) { rcLostFrame++; return; }
|
if (lostFrame) { return; }
|
||||||
normalizeRC();
|
normalizeRC();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -36,9 +36,9 @@ class __FlashStringHelper;
|
|||||||
// https://www.arduino.cc/reference/en/language/variables/data-types/stringobject/
|
// https://www.arduino.cc/reference/en/language/variables/data-types/stringobject/
|
||||||
class String: public std::string {
|
class String: public std::string {
|
||||||
public:
|
public:
|
||||||
long toInt() { return atol(this->c_str()); }
|
long toInt() const { return atol(this->c_str()); }
|
||||||
float toFloat() { return atof(this->c_str()); }
|
float toFloat() const { return atof(this->c_str()); }
|
||||||
bool isEmpty() { return this->empty(); }
|
bool isEmpty() const { return this->empty(); }
|
||||||
};
|
};
|
||||||
|
|
||||||
class Print;
|
class Print;
|
||||||
|
Loading…
x
Reference in New Issue
Block a user