musabe24/t2000_arduino_due
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Drehmoment wird nun normal gemittelt, output Variable ist jetzt global, PID Parameter aktualisiert, der Spannungsausgang bleibt erhalten - auch wenn die Regelung pausiert wurde.

Browse Source
This commit is contained in:
Musab Erdem
2024-07-25 16:28:38 +02:00
parent a49ca289e1
commit 34218c251f
5 changed files with 443 additions and 180 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
.vscode/arduino.json vendored Normal file
View File

@@ -0,0 +1,5 @@
{
"board": "arduino:sam:arduino_due_x_dbg",
"sketch": "testsoftware\\testsoftware.ino",
"port": "COM4"
}

399
.vscode/c_cpp_properties.json vendored Normal file
View File

@@ -0,0 +1,399 @@
{
"version": 4,
"configurations": [
{
"name": "Arduino",
"compilerPath": "C:\\Users\\erdem musab\\AppData\\Local\\Arduino15\\packages\\arduino\\tools\\arm-none-eabi-gcc\\4.8.3-2014q1\\bin\\arm-none-eabi-g++",
"compilerArgs": [
"-w",
"-std=gnu++11",
"-ffunction-sections",
"-fdata-sections",
"-nostdlib",
"-fno-threadsafe-statics",
"--param",
"-fno-rtti",
"-fno-exceptions"
],
"intelliSenseMode": "gcc-x64",
"includePath": [
"C:\\Users\\erdem musab\\AppData\\Local\\Arduino15\\packages\\arduino\\hardware\\sam\\1.6.12\\system\\libsam",
"C:\\Users\\erdem musab\\AppData\\Local\\Arduino15\\packages\\arduino\\hardware\\sam\\1.6.12\\system\\CMSIS\\CMSIS\\Include\\",
"C:\\Users\\erdem musab\\AppData\\Local\\Arduino15\\packages\\arduino\\hardware\\sam\\1.6.12\\system\\CMSIS\\Device\\ATMEL\\",
"C:\\Users\\erdem musab\\AppData\\Local\\Arduino15\\packages\\arduino\\hardware\\sam\\1.6.12\\cores\\arduino",
"C:\\Users\\erdem musab\\AppData\\Local\\Arduino15\\packages\\arduino\\hardware\\sam\\1.6.12\\variants\\arduino_due_x",
"C:\\Users\\erdem musab\\Documents\\Arduino\\libraries\\PRG_342",
"C:\\Users\\erdem musab\\Documents\\Arduino\\libraries\\datalogger",
"C:\\Users\\erdem musab\\Documents\\Arduino\\libraries\\pid",
"c:\\users\\erdem musab\\appdata\\local\\arduino15\\packages\\arduino\\tools\\arm-none-eabi-gcc\\4.8.3-2014q1\\arm-none-eabi\\include\\c++\\4.8.3",
"c:\\users\\erdem musab\\appdata\\local\\arduino15\\packages\\arduino\\tools\\arm-none-eabi-gcc\\4.8.3-2014q1\\arm-none-eabi\\include\\c++\\4.8.3\\arm-none-eabi",
"c:\\users\\erdem musab\\appdata\\local\\arduino15\\packages\\arduino\\tools\\arm-none-eabi-gcc\\4.8.3-2014q1\\arm-none-eabi\\include\\c++\\4.8.3\\backward",
"c:\\users\\erdem musab\\appdata\\local\\arduino15\\packages\\arduino\\tools\\arm-none-eabi-gcc\\4.8.3-2014q1\\lib\\gcc\\arm-none-eabi\\4.8.3\\include",
"c:\\users\\erdem musab\\appdata\\local\\arduino15\\packages\\arduino\\tools\\arm-none-eabi-gcc\\4.8.3-2014q1\\lib\\gcc\\arm-none-eabi\\4.8.3\\include-fixed",
"c:\\users\\erdem musab\\appdata\\local\\arduino15\\packages\\arduino\\tools\\arm-none-eabi-gcc\\4.8.3-2014q1\\arm-none-eabi\\include"
],
"forcedInclude": [
"C:\\Users\\erdem musab\\AppData\\Local\\Arduino15\\packages\\arduino\\hardware\\sam\\1.6.12\\cores\\arduino\\Arduino.h"
],
"cStandard": "c11",
"cppStandard": "c++11",
"defines": [
"F_CPU=84000000L",
"ARDUINO=10607",
"ARDUINO_SAM_DUE",
"ARDUINO_ARCH_SAM",
"__SAM3X8E__",
"USB_VID=0x2341",
"USB_PID=0x003e",
"USBCON",
"USB_MANUFACTURER=\"Arduino LLC\"",
"USB_PRODUCT=\"Arduino Due\"",
"__DBL_MIN_EXP__=(-1021)",
"__HQ_FBIT__=15",
"__UINT_LEAST16_MAX__=65535",
"__ARM_SIZEOF_WCHAR_T=32",
"__ATOMIC_ACQUIRE=2",
"__SFRACT_IBIT__=0",
"__FLT_MIN__=1.1754943508222875e-38F",
"__UFRACT_MAX__=0XFFFFP-16UR",
"__UINT_LEAST8_TYPE__=unsigned char",
"__DQ_FBIT__=63",
"__INTMAX_C(c)=c ## LL",
"__ULFRACT_FBIT__=32",
"__SACCUM_EPSILON__=0x1P-7HK",
"__CHAR_BIT__=8",
"__USQ_IBIT__=0",
"__UINT8_MAX__=255",
"__ACCUM_FBIT__=15",
"__WINT_MAX__=4294967295U",
"__USFRACT_FBIT__=8",
"__ORDER_LITTLE_ENDIAN__=1234",
"__SIZE_MAX__=4294967295U",
"__ARM_ARCH_ISA_ARM=1",
"__WCHAR_MAX__=4294967295U",
"__LACCUM_IBIT__=32",
"__DBL_DENORM_MIN__=double(4.9406564584124654e-324L)",
"__GCC_ATOMIC_CHAR_LOCK_FREE=1",
"__FLT_EVAL_METHOD__=0",
"__LLACCUM_MAX__=0X7FFFFFFFFFFFFFFFP-31LLK",
"__GCC_ATOMIC_CHAR32_T_LOCK_FREE=1",
"__FRACT_FBIT__=15",
"__UINT_FAST64_MAX__=18446744073709551615ULL",
"__SIG_ATOMIC_TYPE__=int",
"__UACCUM_FBIT__=16",
"__DBL_MIN_10_EXP__=(-307)",
"__FINITE_MATH_ONLY__=0",
"__ARMEL__=1",
"__LFRACT_IBIT__=0",
"__GNUC_PATCHLEVEL__=3",
"__LFRACT_MAX__=0X7FFFFFFFP-31LR",
"__UINT_FAST8_MAX__=4294967295U",
"__DEC64_MAX_EXP__=385",
"__INT8_C(c)=c",
"__UINT_LEAST64_MAX__=18446744073709551615ULL",
"__SA_FBIT__=15",
"__SHRT_MAX__=32767",
"__LDBL_MAX__=1.7976931348623157e+308L",
"__FRACT_MAX__=0X7FFFP-15R",
"__UFRACT_FBIT__=16",
"__ARM_FP=12",
"__UFRACT_MIN__=0.0UR",
"__UINT_LEAST8_MAX__=255",
"__GCC_ATOMIC_BOOL_LOCK_FREE=1",
"__UINTMAX_TYPE__=long long unsigned int",
"__LLFRACT_EPSILON__=0x1P-63LLR",
"__DEC32_EPSILON__=1E-6DF",
"__CHAR_UNSIGNED__=1",
"__UINT32_MAX__=4294967295UL",
"__ULFRACT_MAX__=0XFFFFFFFFP-32ULR",
"__TA_IBIT__=64",
"__LDBL_MAX_EXP__=1024",
"__WINT_MIN__=0U",
"__ULLFRACT_MIN__=0.0ULLR",
"__SCHAR_MAX__=127",
"__WCHAR_MIN__=0U",
"__INT64_C(c)=c ## LL",
"__DBL_DIG__=15",
"__GCC_ATOMIC_POINTER_LOCK_FREE=1",
"__LLACCUM_MIN__=(-0X1P31LLK-0X1P31LLK)",
"__SIZEOF_INT__=4",
"__SIZEOF_POINTER__=4",
"__GCC_ATOMIC_CHAR16_T_LOCK_FREE=1",
"__USACCUM_IBIT__=8",
"__USER_LABEL_PREFIX__",
"__STDC_HOSTED__=1",
"__LDBL_HAS_INFINITY__=1",
"__LFRACT_MIN__=(-0.5LR-0.5LR)",
"__HA_IBIT__=8",
"__ARM_NEON_FP=4",
"__TQ_IBIT__=0",
"__FLT_EPSILON__=1.1920928955078125e-7F",
"__APCS_32__=1",
"__GXX_WEAK__=1",
"__USFRACT_IBIT__=0",
"__LDBL_MIN__=2.2250738585072014e-308L",
"__FRACT_MIN__=(-0.5R-0.5R)",
"__DEC32_MAX__=9.999999E96DF",
"__DA_IBIT__=32",
"__ARM_SIZEOF_MINIMAL_ENUM=1",
"__INT32_MAX__=2147483647L",
"__UQQ_FBIT__=8",
"__SIZEOF_LONG__=4",
"__UACCUM_MAX__=0XFFFFFFFFP-16UK",
"__UINT16_C(c)=c",
"__DECIMAL_DIG__=17",
"__LFRACT_EPSILON__=0x1P-31LR",
"__ULFRACT_MIN__=0.0ULR",
"__LDBL_HAS_QUIET_NAN__=1",
"__ULACCUM_IBIT__=32",
"__UACCUM_EPSILON__=0x1P-16UK",
"__GNUC__=4",
"__ULLACCUM_MAX__=0XFFFFFFFFFFFFFFFFP-32ULLK",
"__GXX_RTTI=1",
"__HQ_IBIT__=0",
"__FLT_HAS_DENORM__=1",
"__SIZEOF_LONG_DOUBLE__=8",
"__BIGGEST_ALIGNMENT__=8",
"__DQ_IBIT__=0",
"__DBL_MAX__=double(1.7976931348623157e+308L)",
"__ULFRACT_IBIT__=0",
"__INT_FAST32_MAX__=2147483647",
"__DBL_HAS_INFINITY__=1",
"__INT64_MAX__=9223372036854775807LL",
"__ACCUM_IBIT__=16",
"__DEC32_MIN_EXP__=(-94)",
"__THUMB_INTERWORK__=1",
"__LACCUM_MAX__=0X7FFFFFFFFFFFFFFFP-31LK",
"__INT_FAST16_TYPE__=int",
"__LDBL_HAS_DENORM__=1",
"__cplusplus=199711L",
"__DEC128_MAX__=9.999999999999999999999999999999999E6144DL",
"__INT_LEAST32_MAX__=2147483647L",
"__ARM_PCS=1",
"__DEC32_MIN__=1E-95DF",
"__ACCUM_MAX__=0X7FFFFFFFP-15K",
"__DEPRECATED=1",
"__DBL_MAX_EXP__=1024",
"__USACCUM_EPSILON__=0x1P-8UHK",
"__DEC128_EPSILON__=1E-33DL",
"__SFRACT_MAX__=0X7FP-7HR",
"__FRACT_IBIT__=0",
"__PTRDIFF_MAX__=2147483647",
"__UACCUM_MIN__=0.0UK",
"__UACCUM_IBIT__=16",
"__GNUG__=4",
"__LONG_LONG_MAX__=9223372036854775807LL",
"__SIZEOF_SIZE_T__=4",
"__ULACCUM_MAX__=0XFFFFFFFFFFFFFFFFP-32ULK",
"__SIZEOF_WINT_T__=4",
"__SA_IBIT__=16",
"__ULLACCUM_MIN__=0.0ULLK",
"__GXX_ABI_VERSION=1002",
"__UTA_FBIT__=64",
"__SOFTFP__=1",
"__FLT_MIN_EXP__=(-125)",
"__USFRACT_MAX__=0XFFP-8UHR",
"__UFRACT_IBIT__=0",
"__INT_FAST64_TYPE__=long long int",
"__DBL_MIN__=double(2.2250738585072014e-308L)",
"__FLT_MIN_10_EXP__=(-37)",
"__LACCUM_MIN__=(-0X1P31LK-0X1P31LK)",
"__ULLACCUM_FBIT__=32",
"__GXX_TYPEINFO_EQUALITY_INLINE=0",
"__ULLFRACT_EPSILON__=0x1P-64ULLR",
"__USES_INITFINI__=1",
"__DEC128_MIN__=1E-6143DL",
"__REGISTER_PREFIX__",
"__UINT16_MAX__=65535",
"__DBL_HAS_DENORM__=1",
"__ACCUM_MIN__=(-0X1P15K-0X1P15K)",
"__SQ_IBIT__=0",
"__UINT8_TYPE__=unsigned char",
"__UHA_FBIT__=8",
"__NO_INLINE__=1",
"__SFRACT_MIN__=(-0.5HR-0.5HR)",
"__UTQ_FBIT__=128",
"__FLT_MANT_DIG__=24",
"__VERSION__=\"4.8.3 20140228 (release) [ARM/embedded-4_8-branch revision 208322]\"",
"__UINT64_C(c)=c ## ULL",
"__ULLFRACT_FBIT__=64",
"__FRACT_EPSILON__=0x1P-15R",
"__ULACCUM_MIN__=0.0ULK",
"__UDA_FBIT__=32",
"__LLACCUM_EPSILON__=0x1P-31LLK",
"__GCC_ATOMIC_INT_LOCK_FREE=1",
"__FLOAT_WORD_ORDER__=__ORDER_LITTLE_ENDIAN__",
"__USFRACT_MIN__=0.0UHR",
"__ULLACCUM_IBIT__=32",
"__UQQ_IBIT__=0",
"__INT32_C(c)=c ## L",
"__DEC64_EPSILON__=1E-15DD",
"__ORDER_PDP_ENDIAN__=3412",
"__DEC128_MIN_EXP__=(-6142)",
"__UHQ_FBIT__=16",
"__LLACCUM_FBIT__=31",
"__INT_FAST32_TYPE__=int",
"__UINT_LEAST16_TYPE__=short unsigned int",
"__INT16_MAX__=32767",
"__SIZE_TYPE__=unsigned int",
"__UINT64_MAX__=18446744073709551615ULL",
"__UDQ_FBIT__=64",
"__INT8_TYPE__=signed char",
"__ELF__=1",
"__ULFRACT_EPSILON__=0x1P-32ULR",
"__LLFRACT_FBIT__=63",
"__FLT_RADIX__=2",
"__INT_LEAST16_TYPE__=short int",
"__LDBL_EPSILON__=2.2204460492503131e-16L",
"__UINTMAX_C(c)=c ## ULL",
"__SACCUM_MAX__=0X7FFFP-7HK",
"__SIG_ATOMIC_MAX__=2147483647",
"__GCC_ATOMIC_WCHAR_T_LOCK_FREE=1",
"__VFP_FP__=1",
"__SIZEOF_PTRDIFF_T__=4",
"__LACCUM_EPSILON__=0x1P-31LK",
"__DEC32_SUBNORMAL_MIN__=0.000001E-95DF",
"__INT_FAST16_MAX__=2147483647",
"__UINT_FAST32_MAX__=4294967295U",
"__UINT_LEAST64_TYPE__=long long unsigned int",
"__USACCUM_MAX__=0XFFFFP-8UHK",
"__SFRACT_EPSILON__=0x1P-7HR",
"__FLT_HAS_QUIET_NAN__=1",
"__FLT_MAX_10_EXP__=38",
"__LONG_MAX__=2147483647L",
"__DEC128_SUBNORMAL_MIN__=0.000000000000000000000000000000001E-6143DL",
"__FLT_HAS_INFINITY__=1",
"__USA_FBIT__=16",
"__UINT_FAST16_TYPE__=unsigned int",
"__DEC64_MAX__=9.999999999999999E384DD",
"__CHAR16_TYPE__=short unsigned int",
"__PRAGMA_REDEFINE_EXTNAME=1",
"__INT_LEAST16_MAX__=32767",
"__DEC64_MANT_DIG__=16",
"__UINT_LEAST32_MAX__=4294967295UL",
"__SACCUM_FBIT__=7",
"__GCC_ATOMIC_LONG_LOCK_FREE=1",
"__INT_LEAST64_TYPE__=long long int",
"__INT16_TYPE__=short int",
"__INT_LEAST8_TYPE__=signed char",
"__SQ_FBIT__=31",
"__DEC32_MAX_EXP__=97",
"__ARM_ARCH_ISA_THUMB=1",
"__INT_FAST8_MAX__=2147483647",
"__ARM_ARCH=4",
"__INTPTR_MAX__=2147483647",
"__QQ_FBIT__=7",
"__UTA_IBIT__=64",
"__EXCEPTIONS=1",
"__LDBL_MANT_DIG__=53",
"__SFRACT_FBIT__=7",
"__SACCUM_MIN__=(-0X1P7HK-0X1P7HK)",
"__DBL_HAS_QUIET_NAN__=1",
"__SIG_ATOMIC_MIN__=(-__SIG_ATOMIC_MAX__ - 1)",
"__INTPTR_TYPE__=int",
"__UINT16_TYPE__=short unsigned int",
"__WCHAR_TYPE__=unsigned int",
"__SIZEOF_FLOAT__=4",
"__USQ_FBIT__=32",
"__UINTPTR_MAX__=4294967295U",
"__DEC64_MIN_EXP__=(-382)",
"__INT_FAST64_MAX__=9223372036854775807LL",
"__GCC_ATOMIC_TEST_AND_SET_TRUEVAL=1",
"__FLT_DIG__=6",
"__UINT_FAST64_TYPE__=long long unsigned int",
"__INT_MAX__=2147483647",
"__LACCUM_FBIT__=31",
"__USACCUM_MIN__=0.0UHK",
"__UHA_IBIT__=8",
"__INT64_TYPE__=long long int",
"__FLT_MAX_EXP__=128",
"__UTQ_IBIT__=0",
"__DBL_MANT_DIG__=53",
"__INT_LEAST64_MAX__=9223372036854775807LL",
"__DEC64_MIN__=1E-383DD",
"__WINT_TYPE__=unsigned int",
"__UINT_LEAST32_TYPE__=long unsigned int",
"__SIZEOF_SHORT__=2",
"__ULLFRACT_IBIT__=0",
"__LDBL_MIN_EXP__=(-1021)",
"__arm__=1",
"__UDA_IBIT__=32",
"__INT_LEAST8_MAX__=127",
"__LFRACT_FBIT__=31",
"__WCHAR_UNSIGNED__=1",
"__LDBL_MAX_10_EXP__=308",
"__ATOMIC_RELAXED=0",
"__DBL_EPSILON__=double(2.2204460492503131e-16L)",
"__UINT8_C(c)=c",
"__INT_LEAST32_TYPE__=long int",
"__SIZEOF_WCHAR_T__=4",
"__UINT64_TYPE__=long long unsigned int",
"__LLFRACT_MAX__=0X7FFFFFFFFFFFFFFFP-63LLR",
"__TQ_FBIT__=127",
"__INT_FAST8_TYPE__=int",
"__ULLACCUM_EPSILON__=0x1P-32ULLK",
"__UHQ_IBIT__=0",
"__LLACCUM_IBIT__=32",
"__DBL_DECIMAL_DIG__=17",
"__DEC_EVAL_METHOD__=2",
"__TA_FBIT__=63",
"__UDQ_IBIT__=0",
"__ORDER_BIG_ENDIAN__=4321",
"__ACCUM_EPSILON__=0x1P-15K",
"__UINT32_C(c)=c ## UL",
"__INTMAX_MAX__=9223372036854775807LL",
"__BYTE_ORDER__=__ORDER_LITTLE_ENDIAN__",
"__FLT_DENORM_MIN__=1.4012984643248171e-45F",
"__LLFRACT_IBIT__=0",
"__INT8_MAX__=127",
"__UINT_FAST32_TYPE__=unsigned int",
"__CHAR32_TYPE__=long unsigned int",
"__FLT_MAX__=3.4028234663852886e+38F",
"__USACCUM_FBIT__=8",
"__INT32_TYPE__=long int",
"__SIZEOF_DOUBLE__=8",
"__UFRACT_EPSILON__=0x1P-16UR",
"__INTMAX_TYPE__=long long int",
"__DEC128_MAX_EXP__=6145",
"__ATOMIC_CONSUME=1",
"__GNUC_MINOR__=8",
"__UINTMAX_MAX__=18446744073709551615ULL",
"__DEC32_MANT_DIG__=7",
"__HA_FBIT__=7",
"__DBL_MAX_10_EXP__=308",
"__LDBL_DENORM_MIN__=4.9406564584124654e-324L",
"__INT16_C(c)=c",
"__STDC__=1",
"__ARM_ARCH_4T__=1",
"__PTRDIFF_TYPE__=int",
"__LLFRACT_MIN__=(-0.5LLR-0.5LLR)",
"__ATOMIC_SEQ_CST=5",
"__DA_FBIT__=31",
"__UINT32_TYPE__=long unsigned int",
"__UINTPTR_TYPE__=unsigned int",
"__USA_IBIT__=16",
"__DEC64_SUBNORMAL_MIN__=0.000000000000001E-383DD",
"__ARM_EABI__=1",
"__DEC128_MANT_DIG__=34",
"__LDBL_MIN_10_EXP__=(-307)",
"__SIZEOF_LONG_LONG__=8",
"__ULACCUM_EPSILON__=0x1P-32ULK",
"__SACCUM_IBIT__=8",
"__GCC_ATOMIC_LLONG_LOCK_FREE=1",
"__LDBL_DIG__=15",
"__FLT_DECIMAL_DIG__=9",
"__UINT_FAST16_MAX__=4294967295U",
"__GNUC_GNU_INLINE__=1",
"__GCC_ATOMIC_SHORT_LOCK_FREE=1",
"__ULLFRACT_MAX__=0XFFFFFFFFFFFFFFFFP-64ULLR",
"__UINT_FAST8_TYPE__=unsigned int",
"__USFRACT_EPSILON__=0x1P-8UHR",
"__ULACCUM_FBIT__=32",
"__QQ_IBIT__=0",
"__ATOMIC_ACQ_REL=4",
"__ATOMIC_RELEASE=3",
"USBCON"
]
}
]
}

9
.vscode/settings.json vendored Normal file
View File

@@ -0,0 +1,9 @@
{
"files.associations": {
"functional": "cpp",
"array": "cpp",
"tuple": "cpp",
"type_traits": "cpp",
"utility": "cpp"
}
}

38
main/main.ino
View File

@@ -17,7 +17,8 @@
#define BLOCK_DETECTION_VALUE setpoint #define BLOCK_DETECTION_VALUE setpoint
#define ROTATION_DETECTION_VALUE 100 #define ROTATION_DETECTION_VALUE 100
#define _getTorque abs(prg.getTorque(false, ALPHA)) //#define _getTorque abs(prg.getTorque(false, ALPHA))
#define _getTorque abs(prg.getTorque(false))
#define _getAngle prg.getAngle() #define _getAngle prg.getAngle()
struct Timer { struct Timer {
@@ -65,6 +66,7 @@ float filteredTorque = 0; // gefilterter Wert
float filteredSetpoint = 0; float filteredSetpoint = 0;
float setpoint; float setpoint;
int oldSetpoint; int oldSetpoint;
unsigned int output;
static int lastOutput; static int lastOutput;
float manVolt; float manVolt;
unsigned char CW; unsigned char CW;
@@ -75,8 +77,8 @@ unsigned char BLOCK_SOLVED;
bool In_Src_Sw = false; bool In_Src_Sw = false;
float Kp = 0.0; float Kp = 0.11;
float Ki = 0.1; float Ki = 0.9;
float Kd = 0.0; float Kd = 0.0;
/*float Kp = 1.0; /*float Kp = 1.0;
@@ -172,12 +174,6 @@ void loop() {
waitStart(speedcheck_timer, SPEEDCHECK_TIME); waitStart(speedcheck_timer, SPEEDCHECK_TIME);
} }
if (Richtungswechsel) {
myPID.reset();
Richtungswechsel = 0;
}
if (!BLOCK) { if (!BLOCK) {
// Aktualisieren des Setpoints basierend auf dem Winkel // Aktualisieren des Setpoints basierend auf dem Winkel
int index = (float)angle * 0.002; // 0.5 Grad pro Index int index = (float)angle * 0.002; // 0.5 Grad pro Index
@@ -196,13 +192,20 @@ void loop() {
}*/ }*/
myPID.setTunings(Kp, Ki, Kd); myPID.setTunings(Kp, Ki, Kd);
if (lastAnglePID != angle && Pon && !manOut && !checkLimits(setpoint, abs(actTorque), upperLimit, lowerLimit) &&((abs(actTorque) >= 1000 || abs(RotSpeed) >= 400) || abs(actTorque) > setpoint)) { if (Richtungswechsel) {
unsigned int output = myPID.compute(setpoint, actTorque); //myPID.reset();
prg.setOutput(output); Richtungswechsel = 0;
prg.ActOut = output;
} else if (abs(actTorque) < 1000) {
myPID.reset();
} }
if (lastAnglePID != angle && Pon && !manOut && !checkLimits(setpoint, abs(actTorque), upperLimit, lowerLimit) &&((abs(actTorque) >= 1000 || abs(RotSpeed) >= 400) || abs(actTorque) > setpoint)) {
output = myPID.compute(setpoint, abs(actTorque));
prg.setOutput(output);
//prg.ActOut = output;
} else if (abs(actTorque) < 1000) {
//myPID.reset();
}
prg.ActOut = output;
lastAnglePID = angle; lastAnglePID = angle;
if (LOGGING) { if (LOGGING) {
@@ -285,6 +288,7 @@ void setAnalogResolution(unsigned char res) {
void UPDATE_VALUES() { void UPDATE_VALUES() {
angle = _getAngle; angle = _getAngle;
actTorque = _getTorque; actTorque = _getTorque;
prg.updateActOut(actTorque); prg.updateActOut(actTorque);
@@ -322,7 +326,7 @@ void DEBUG_PRINTER() {
Serial.print("Winkel: "); Serial.print("Winkel: ");
Serial.println((float)angle / 1000, 3); Serial.println((float)angle / 1000, 3);
Serial.print("Drehmoment: "); Serial.print("Drehmoment: ");
Serial.println((float)actTorque / 1000, 3); Serial.println(abs(actTorque) / 1000, 2);
Serial.print("Geschwindigkeit: "); Serial.print("Geschwindigkeit: ");
Serial.println(RotSpeed); Serial.println(RotSpeed);
Serial.print("Drehrichtung: "); Serial.print("Drehrichtung: ");
@@ -386,7 +390,7 @@ void parseCommand(const String &command) {
} else if (command[0] == 'a') { } else if (command[0] == 'a') {
Serial.print(angle); Serial.print(angle);
Serial.print(";"); Serial.print(";");
Serial.print(actTorque); Serial.print(abs(actTorque));
Serial.print(";"); Serial.print(";");
Serial.print(prg.getSetPoint(1)); Serial.print(prg.getSetPoint(1));
Serial.print(";"); Serial.print(";");

172
testsoftware/testsoftware.ino
View File

@@ -9,8 +9,9 @@
#define SPEEDCHECK_TIME 200 #define SPEEDCHECK_TIME 200
#define PID_DELAY 0 #define PID_DELAY 0
#define LOG_TIMER_DELAY 100 #define LOG_TIMER_DELAY 100
#define MEAS_DELAY 0 #define MEAS_DELAY 1000
#define K_FAKTOR 1.0995380532183 //#define K_FAKTOR 1.0995380532183
#define K_FAKTOR 1.00
#define OFFSET 0 #define OFFSET 0
#define ALPHA 0.0008 #define ALPHA 0.0008
#define N_SAMPLES 100 #define N_SAMPLES 100
@@ -121,146 +122,18 @@ void setup() {
WinkelArray[i] = -1; WinkelArray[i] = -1;
} }
Serial.begin(19200); // Beginne serielle Kommunikation Serial.begin(115200); // Beginne serielle Kommunikation
} }
void loop() { void loop() {
/*if (waitIsOver(meas_timer)) {
UPDATE_VALUES(); prg.setOutput(2048);
for (int i = 0; i < N_SAMPLES - 1; i++) { delay(10);
actTorqueArray[i] = actTorqueArray[i + 1]; prg.setOutput(2049);
} delay(10);
actTorqueArray[N_SAMPLES - 1] = actTorque;
actTorque = 0;
for (int j = 0; j < N_SAMPLES; j++) {
actTorque += actTorqueArray[j];
}
actTorque = (float)actTorque / N_SAMPLES;
waitStart(meas_timer, MEAS_DELAY);
}*/
UPDATE_VALUES();
filteredTorque = exponentialFilter(actTorque, filteredTorque, ALPHA); // Verwenden Sie einen geeigneten Alpha-Wert
COMMUNICATION_HANDLER(); COMMUNICATION_HANDLER();
if (DEBUG_MODE) {
if (waitIsOver(debug_timer)) {
DEBUG_PRINTER();
waitStart(debug_timer, DEBUG_PRINT_TIME);
} }
}
if (waitIsOver(speedcheck_timer)) {
SPEEDCHECK();
BLOCK = BLOCK_DETECTION(abs(actTorque), abs(RotSpeed));
/*if (BLOCK == 0) {
if (BLOCK_SOLVED == 1) {
setpoint = oldSetpoint;
BLOCK_SOLVED = 0;
} else {
oldSetpoint = setpoint;
}
} else if (BLOCK == 1) {
if (BLOCK_SOLVED == 0) {
setpoint = 0;
BLOCK_SOLVED = 1;
}
}*/
waitStart(speedcheck_timer, SPEEDCHECK_TIME);
}
if (Richtungswechsel) {
myPID.reset();
Richtungswechsel = 0;
}
if (!BLOCK) {
// Aktualisieren des Setpoints basierend auf dem Winkel
int index = (float)angle * 0.002; // 0.5 Grad pro Index
if (index >= 0 && index < 181) {
setpoint = WinkelArray[index];
}
if (In_Src_Sw) {
setpoint = float(prg.getSetPoint(1)) * 5.0;
}
}
/*if (abs(setpoint) - abs(actTorque) <= 1000) {
myPID.setTunings(Kp, Ki, Kd);
} else {
myPID.setTunings(Kp, Ki, Kd);
}*/
myPID.setTunings(Kp, Ki, Kd);
if (lastAnglePID != angle && Pon && !manOut && !checkLimits(setpoint, abs(actTorque), upperLimit, lowerLimit) &&((abs(actTorque) >= 1000 || abs(RotSpeed) >= 400) || abs(actTorque) > setpoint)) {
unsigned int output = myPID.compute(setpoint, actTorque);
prg.setOutput(output);
prg.ActOut = output;
} else if (abs(actTorque) < 1000) {
myPID.reset();
}
lastAnglePID = angle;
if (LOGGING) {
if (waitIsOver(log_timer)) {
sendDataToDatalogger();
waitStart(log_timer, LOG_TIMER_DELAY);
}
}
prg.isRunning == false;
}
bool checkLimits(float setpoint, int actVal, int upperLimit, int lowerLimit) {
int error = setpoint - actVal;
if (error <= upperLimit && error >= lowerLimit) {
return true;
} else {
return false;
}
}
void interpolateWinkelArray() {
int lastSetIndex = -1;
for (int i = 0; i < 180; i++) {
int start = i;
int k = i + 1;
while (WinkelArray[k] == -1) {
k++;
}
int end = k;
float startTorque = WinkelArray[start];
float endTorque = WinkelArray[end];
for (int j = start + 1; j < end; j++) {
float interpolatedTorque = startTorque + ((endTorque - startTorque) / (end - start)) * (j - start);
WinkelArray[j] = interpolatedTorque;
}
}
/*for (int i = 0; i < 181; i++) {
if (WinkelArray[i] != 0 || i == 0) { // Sicherstellen, dass der Startwert berücksichtigt wird
if (lastSetIndex != -1) {
int start = lastSetIndex;
int end = i;
float startTorque = WinkelArray[start];
float endTorque = WinkelArray[end];
for (int j = start + 1; j < end; j++) {
float interpolatedTorque = startTorque + ((endTorque - startTorque) / (end - start)) * (j - start);
WinkelArray[j] = interpolatedTorque;
}
}
lastSetIndex = i;
}
}*/
}
float exponentialFilter(float currentValue, float previousFilteredValue, float alpha) { float exponentialFilter(float currentValue, float previousFilteredValue, float alpha) {
return alpha * currentValue + (1.0 - alpha) * previousFilteredValue; return alpha * currentValue + (1.0 - alpha) * previousFilteredValue;
@@ -270,14 +143,6 @@ void sendDataToDatalogger() {
datalogger.addData((float)((float)millis() - (float)logtime) / 1000.0, (float)angle / 1000.0, setpoint / 1000.0, actTorque / 1000.0, (float)prg.getOutVolt() / 1000.0, RotSpeed); datalogger.addData((float)((float)millis() - (float)logtime) / 1000.0, (float)angle / 1000.0, setpoint / 1000.0, actTorque / 1000.0, (float)prg.getOutVolt() / 1000.0, RotSpeed);
} }
unsigned char BLOCK_DETECTION(unsigned int torque, float speed) {
if (torque >= BLOCK_DETECTION_VALUE && speed < ROTATION_DETECTION_VALUE) {
return 1;
} else {
return 0;
}
}
void setAnalogResolution(unsigned char res) { void setAnalogResolution(unsigned char res) {
analogWriteResolution(res); analogWriteResolution(res);
analogReadResolution(res); analogReadResolution(res);
@@ -298,24 +163,6 @@ void COMMUNICATION_HANDLER() {
} }
} }
void SPEEDCHECK() {
int deltaAngle = angle - lastAngle;
RotSpeed = deltaAngle * 5; // Umrechnen in Winkel pro Sekunde
lastAngle = angle;
if (RotSpeed > 0) {
if (CW == 0) {
Richtungswechsel = 1;
}
CW = 1;
} else if (RotSpeed < 0) {
if (CW == 1) {
Richtungswechsel = 1;
}
CW = 0;
}
}
void DEBUG_PRINTER() { void DEBUG_PRINTER() {
// Debug-Informationen ausgeben // Debug-Informationen ausgeben
Serial.println("####################"); Serial.println("####################");
@@ -451,7 +298,6 @@ void parseCommand(const String &command) {
} }
lastIndex = (index == -1) ? -1 : index + 1; lastIndex = (index == -1) ? -1 : index + 1;
} }
interpolateWinkelArray();
} else if (command[0] == 'v') { } else if (command[0] == 'v') {
if (manOut) { if (manOut) {
setParameter(command, 'v', manVolt); // Hier wurde 'manVolt' als float deklariert setParameter(command, 'v', manVolt); // Hier wurde 'manVolt' als float deklariert