diff --git a/.vscode/arduino.json b/.vscode/arduino.json new file mode 100644 index 0000000..5160feb --- /dev/null +++ b/.vscode/arduino.json @@ -0,0 +1,5 @@ +{ + "board": "arduino:sam:arduino_due_x_dbg", + "sketch": "testsoftware\\testsoftware.ino", + "port": "COM4" +} \ No newline at end of file diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json new file mode 100644 index 0000000..a6b6baa --- /dev/null +++ b/.vscode/c_cpp_properties.json @@ -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" + ] + } + ] +} \ No newline at end of file diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..dcefee7 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,9 @@ +{ + "files.associations": { + "functional": "cpp", + "array": "cpp", + "tuple": "cpp", + "type_traits": "cpp", + "utility": "cpp" + } +} \ No newline at end of file diff --git a/main/main.ino b/main/main.ino index 7980c07..31b2a21 100644 --- a/main/main.ino +++ b/main/main.ino @@ -17,7 +17,8 @@ #define BLOCK_DETECTION_VALUE setpoint #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() struct Timer { @@ -65,6 +66,7 @@ float filteredTorque = 0; // gefilterter Wert float filteredSetpoint = 0; float setpoint; int oldSetpoint; +unsigned int output; static int lastOutput; float manVolt; unsigned char CW; @@ -75,8 +77,8 @@ unsigned char BLOCK_SOLVED; bool In_Src_Sw = false; -float Kp = 0.0; -float Ki = 0.1; +float Kp = 0.11; +float Ki = 0.9; float Kd = 0.0; /*float Kp = 1.0; @@ -172,12 +174,6 @@ void loop() { 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 @@ -196,13 +192,20 @@ void loop() { }*/ 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(); + if (Richtungswechsel) { + //myPID.reset(); + Richtungswechsel = 0; } + + 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; if (LOGGING) { @@ -285,6 +288,7 @@ void setAnalogResolution(unsigned char res) { void UPDATE_VALUES() { angle = _getAngle; + actTorque = _getTorque; prg.updateActOut(actTorque); @@ -322,7 +326,7 @@ void DEBUG_PRINTER() { Serial.print("Winkel: "); Serial.println((float)angle / 1000, 3); Serial.print("Drehmoment: "); - Serial.println((float)actTorque / 1000, 3); + Serial.println(abs(actTorque) / 1000, 2); Serial.print("Geschwindigkeit: "); Serial.println(RotSpeed); Serial.print("Drehrichtung: "); @@ -386,7 +390,7 @@ void parseCommand(const String &command) { } else if (command[0] == 'a') { Serial.print(angle); Serial.print(";"); - Serial.print(actTorque); + Serial.print(abs(actTorque)); Serial.print(";"); Serial.print(prg.getSetPoint(1)); Serial.print(";"); diff --git a/testsoftware/testsoftware.ino b/testsoftware/testsoftware.ino index 7980c07..a3e217c 100644 --- a/testsoftware/testsoftware.ino +++ b/testsoftware/testsoftware.ino @@ -9,8 +9,9 @@ #define SPEEDCHECK_TIME 200 #define PID_DELAY 0 #define LOG_TIMER_DELAY 100 -#define MEAS_DELAY 0 -#define K_FAKTOR 1.0995380532183 +#define MEAS_DELAY 1000 +//#define K_FAKTOR 1.0995380532183 +#define K_FAKTOR 1.00 #define OFFSET 0 #define ALPHA 0.0008 #define N_SAMPLES 100 @@ -121,147 +122,19 @@ void setup() { WinkelArray[i] = -1; } - Serial.begin(19200); // Beginne serielle Kommunikation + Serial.begin(115200); // Beginne serielle Kommunikation } void loop() { - /*if (waitIsOver(meas_timer)) { - UPDATE_VALUES(); - for (int i = 0; i < N_SAMPLES - 1; i++) { - actTorqueArray[i] = actTorqueArray[i + 1]; - } - 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 + prg.setOutput(2048); + delay(10); + prg.setOutput(2049); + delay(10); + 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) { 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); } -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) { analogWriteResolution(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() { // Debug-Informationen ausgeben Serial.println("####################"); @@ -451,7 +298,6 @@ void parseCommand(const String &command) { } lastIndex = (index == -1) ? -1 : index + 1; } - interpolateWinkelArray(); } else if (command[0] == 'v') { if (manOut) { setParameter(command, 'v', manVolt); // Hier wurde 'manVolt' als float deklariert