diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..89cc49c --- /dev/null +++ b/.gitignore @@ -0,0 +1,5 @@ +.pio +.vscode/.browse.c_cpp.db* +.vscode/c_cpp_properties.json +.vscode/launch.json +.vscode/ipch diff --git a/.vscode/extensions.json b/.vscode/extensions.json new file mode 100644 index 0000000..080e70d --- /dev/null +++ b/.vscode/extensions.json @@ -0,0 +1,10 @@ +{ + // See http://go.microsoft.com/fwlink/?LinkId=827846 + // for the documentation about the extensions.json format + "recommendations": [ + "platformio.platformio-ide" + ], + "unwantedRecommendations": [ + "ms-vscode.cpptools-extension-pack" + ] +} diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..0c77f34 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,73 @@ +{ + "C_Cpp_Runner.cCompilerPath": "/home/drake/.platformio/packages/toolchain-atmelavr/bin/avr-gcc", + "C_Cpp_Runner.cppCompilerPath": "/home/drake/.platformio/packages/toolchain-atmelavr/bin/avr-g++", + "C_Cpp_Runner.debuggerPath": "/home/drake/.platformio/packages/toolchain-atmelavr/bin/avr-gdb", + "C_Cpp_Runner.cStandard": "gnu11", + "C_Cpp_Runner.cppStandard": "gnu++11", + "C_Cpp_Runner.msvcBatchPath": "", + "C_Cpp_Runner.useMsvc": false, + "C_Cpp_Runner.warnings": [ + "-Wall", + "-Wextra", + "-Wpedantic", + "-Wshadow", + "-Wformat=2", + "-Wcast-align", + "-Wconversion", + "-Wsign-conversion", + "-Wnull-dereference" + ], + "C_Cpp_Runner.msvcWarnings": [ + "/W4", + "/permissive-", + "/w14242", + "/w14287", + "/w14296", + "/w14311", + "/w14826", + "/w44062", + "/w44242", + "/w14905", + "/w14906", + "/w14263", + "/w44265", + "/w14928" + ], + "C_Cpp_Runner.enableWarnings": true, + "C_Cpp_Runner.warningsAsError": false, + "C_Cpp_Runner.compilerArgs": [], + "C_Cpp_Runner.linkerArgs": [], + "C_Cpp_Runner.includePaths": [ + "/home/drake/Documents/PlatformIO/Projects/DMX-motor-controller/include", + "/home/drake/Documents/PlatformIO/Projects/DMX-motor-controller/src", + "/home/drake/Documents/PlatformIO/Projects/DMX-motor-controller/lib/Encoder", + "/home/drake/Documents/PlatformIO/Projects/DMX-motor-controller/lib/Encoder/utility", + "/home/drake/Documents/PlatformIO/Projects/DMX-motor-controller/lib/DMXSerial/src", + "/home/drake/.platformio/packages/framework-arduino-avr/cores/arduino", + "/home/drake/.platformio/packages/framework-arduino-avr/variants/standard", + "/home/drake/.platformio/packages/framework-arduino-avr/libraries/EEPROM/src", + "/home/drake/.platformio/packages/framework-arduino-avr/libraries/HID/src", + "/home/drake/.platformio/packages/framework-arduino-avr/libraries/SPI/src", + "/home/drake/.platformio/packages/framework-arduino-avr/libraries/SoftwareSerial/src", + "/home/drake/.platformio/packages/framework-arduino-avr/libraries/Wire/src", + "" + ], + "C_Cpp_Runner.includeSearch": [ + "*", + "**/*" + ], + "C_Cpp_Runner.excludeSearch": [ + "**/build", + "**/build/**", + "**/.*", + "**/.*/**", + "**/.vscode", + "**/.vscode/**" + ], + "C_Cpp_Runner.useAddressSanitizer": false, + "C_Cpp_Runner.useUndefinedSanitizer": false, + "C_Cpp_Runner.useLeakSanitizer": false, + "C_Cpp_Runner.showCompilationTime": false, + "C_Cpp_Runner.useLinkTimeOptimization": false, + "C_Cpp_Runner.msvcSecureNoWarnings": false +} \ No newline at end of file diff --git a/include/README b/include/README new file mode 100644 index 0000000..194dcd4 --- /dev/null +++ b/include/README @@ -0,0 +1,39 @@ + +This directory is intended for project header files. + +A header file is a file containing C declarations and macro definitions +to be shared between several project source files. You request the use of a +header file in your project source file (C, C++, etc) located in `src` folder +by including it, with the C preprocessing directive `#include'. + +```src/main.c + +#include "header.h" + +int main (void) +{ + ... +} +``` + +Including a header file produces the same results as copying the header file +into each source file that needs it. Such copying would be time-consuming +and error-prone. With a header file, the related declarations appear +in only one place. If they need to be changed, they can be changed in one +place, and programs that include the header file will automatically use the +new version when next recompiled. The header file eliminates the labor of +finding and changing all the copies as well as the risk that a failure to +find one copy will result in inconsistencies within a program. + +In C, the usual convention is to give header files names that end with `.h'. +It is most portable to use only letters, digits, dashes, and underscores in +header file names, and at most one dot. + +Read more about using header files in official GCC documentation: + +* Include Syntax +* Include Operation +* Once-Only Headers +* Computed Includes + +https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html diff --git a/lib/PWM b/lib/PWM new file mode 160000 index 0000000..669bae3 --- /dev/null +++ b/lib/PWM @@ -0,0 +1 @@ +Subproject commit 669bae33aa840f47844cefc1c9b77f5598fde15a diff --git a/platformio.ini b/platformio.ini index 73cac0b..f015c4a 100644 --- a/platformio.ini +++ b/platformio.ini @@ -12,4 +12,4 @@ platform = atmelavr board = uno framework = arduino -lib_deps = DMXSerial +monitor_speed = 115200 diff --git a/src/main.cpp b/src/main.cpp index 5d2563f..50a2d98 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,26 +1,29 @@ #include #include #include +#include // #define SENSOR_PIN A0 -#define RPWM_PIN 10 -#define LPWM_PIN 11 -#define L_EN_PIN 12 -#define R_EN_PIN 13 +#define RPWM_PIN 9 +#define LPWM_PIN 10 +#define L_EN_PIN 11 +#define R_EN_PIN 12 #define ENCODER_A 2 #define ENCODER_B 3 const int DMX_CHANNEL = 1; -double pidGain; +double pidGain = 0.01; double currentPosition = 0; -double targetPosition = 50; -int positionDif = 0; -double pidPwm = 0; +double targetPosition = 0; +// int positionDif = 0; +double pidPWM = 0; +double motorPWM = 0; +double lastSwitch = 0; -PID pid(¤tPosition, &pidPwm, &targetPosition, 2, 5, 1, DIRECT); +PID pid(¤tPosition, &pidPWM, &targetPosition, 0.2, 0.0, 0.1, DIRECT); void updateEncoder() { if (digitalRead(ENCODER_B) == LOW) { @@ -31,71 +34,87 @@ void updateEncoder() { } void setup() { - DMXSerial.init(DMXReceiver); - DMXSerial.write(DMX_CHANNEL, 0); + Serial.begin(115200); + + // DMXSerial.init(DMXReceiver); + // DMXSerial.write(DMX_CHANNEL, 0); pinMode(RPWM_PIN, OUTPUT); pinMode(LPWM_PIN, OUTPUT); pinMode(L_EN_PIN, OUTPUT); - pinMode(L_EN_PIN, OUTPUT); + pinMode(R_EN_PIN, OUTPUT); pinMode(ENCODER_A, INPUT_PULLUP); pinMode(ENCODER_B, INPUT_PULLUP); attachInterrupt(digitalPinToInterrupt(ENCODER_A), updateEncoder, RISING); - pid.SetMode(AUTOMATIC); - // pid.SetSampleTime(50); - pid.SetOutputLimits(-255, 255); + InitTimersSafe(); + SetPinFrequencySafe(9, 10000); + pid.SetMode(AUTOMATIC); + pid.SetSampleTime(50); + pid.SetOutputLimits(-255, 255); pinMode(5, OUTPUT); pinMode(6, OUTPUT); + digitalWrite(LPWM_PIN, HIGH); + digitalWrite(RPWM_PIN, HIGH); + lastSwitch = millis(); } void loop() { - // Calculate how long no data bucket was received - if (DMXSerial.noDataSince() < 5000) { - if (DMXSerial.read(DMX_CHANNEL + 7) > 0) { - pidPwm = DMXSerial.read(DMX_CHANNEL + 7); - } else { - - // read recent DMX values and set pwm levels - // pidGain = 0.0001 + (0.002 * (DMXSerial.read(DMX_CHANNEL + 1) / 255)); - // float kP = 10 * (DMXSerial.read(DMX_CHANNEL + 2) / 255); - // float kI = 10 * (DMXSerial.read(DMX_CHANNEL + 3) / 255); - // float kD = 10 * (DMXSerial.read(DMX_CHANNEL + 4) / 255); - - // pid.SetTunings(kP, kI, kD); - - targetPosition = (DMXSerial.read(DMX_CHANNEL) - 100) * 6; // 0-200. 100 is center, <100 is CW, >100 is CCW. 0 & 200 are 360 degrees - - // PID - pid.Compute(); - - // positionDif = targetPosition - currentPosition; - // pidPwm += (positionDif - pidPwm) * pidGain; - // if (pidPwm > 255) pidPwm = 255; - // if (pidPwm < -255) pidPwm = -255; - - } - - if (pidPwm > 0) { - analogWrite(RPWM_PIN, 0); - analogWrite(LPWM_PIN, pidPwm); - - analogWrite(5, 0); - analogWrite(6, pidPwm); + if ((millis() - lastSwitch) > 5000) { + if (targetPosition == 0) { + targetPosition = 3000; } else { - analogWrite(LPWM_PIN, 0); - analogWrite(RPWM_PIN, -pidPwm); - - analogWrite(6, 0); - analogWrite(5, -pidPwm); + targetPosition = 0; } + lastSwitch = millis(); + } + // Calculate how long no data bucket was received + // if (DMXSerial.noDataSince() < 5000) { + digitalWrite(R_EN_PIN, 1); + digitalWrite(L_EN_PIN, 1); - } else { - digitalWrite(R_EN_PIN, 0); - digitalWrite(L_EN_PIN, 0); - } + // read recent DMX values and set pwm levels + // pidGain = 0.0001 + (0.002 * (DMXSerial.read(DMX_CHANNEL + 1) / 255)); + // float kP = 10 * (DMXSerial.read(DMX_CHANNEL + 2) / 255); + // float kI = 10 * (DMXSerial.read(DMX_CHANNEL + 3) / 255); + // float kD = 10 * (DMXSerial.read(DMX_CHANNEL + 4) / 255); + // pid.SetTunings(kP, kI, kD); + + // targetPosition = (DMXSerial.read(DMX_CHANNEL) - 100) * 6; // 0-200. 100 is center, <100 is CW, >100 is CCW. 0 & 200 are 360 degrees + + pid.Compute(); + // motorPWM += (pidPWM - motorPWM) * pidGain; + motorPWM = pidPWM; + + Serial.print("current: "); + Serial.print(currentPosition); + Serial.print(", target: "); + Serial.print(targetPosition); + Serial.print(", PID: "); + Serial.print(pidPWM); + Serial.print(", Motor: "); + Serial.println(motorPWM); + + if (motorPWM > 0) { + pwmWrite(RPWM_PIN, 0); + pwmWrite(LPWM_PIN, motorPWM); + + analogWrite(5, 0); + analogWrite(6, motorPWM); + } else { + pwmWrite(LPWM_PIN, 0); + pwmWrite(RPWM_PIN, -motorPWM); + + analogWrite(6, 0); + analogWrite(5, -motorPWM); + } + + // } else { + // digitalWrite(R_EN_PIN, 0); + // digitalWrite(L_EN_PIN, 0); + // } } \ No newline at end of file diff --git a/test/README b/test/README new file mode 100644 index 0000000..9b1e87b --- /dev/null +++ b/test/README @@ -0,0 +1,11 @@ + +This directory is intended for PlatformIO Test Runner and project tests. + +Unit Testing is a software testing method by which individual units of +source code, sets of one or more MCU program modules together with associated +control data, usage procedures, and operating procedures, are tested to +determine whether they are fit for use. Unit testing finds problems early +in the development cycle. + +More information about PlatformIO Unit Testing: +- https://docs.platformio.org/en/latest/advanced/unit-testing/index.html