From 4bc7e99780e4b326bbc60e6ad17c54e845ce7004 Mon Sep 17 00:00:00 2001 From: brucker Date: Wed, 12 Apr 2023 15:18:14 +0200 Subject: [PATCH] First commit --- .gitignore | 5 + .vscode/extensions.json | 10 + include/M034.h | 38 +++ include/Pin_Definitions.h | 66 ++++ include/README | 39 +++ lib/README | 46 +++ platformio.ini | 16 + src/M034.cpp | 58 ++++ src/main.cpp | 632 ++++++++++++++++++++++++++++++++++++++ test/README | 11 + 10 files changed, 921 insertions(+) create mode 100644 .gitignore create mode 100644 .vscode/extensions.json create mode 100644 include/M034.h create mode 100644 include/Pin_Definitions.h create mode 100644 include/README create mode 100644 lib/README create mode 100644 platformio.ini create mode 100644 src/M034.cpp create mode 100644 src/main.cpp create mode 100644 test/README 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/include/M034.h b/include/M034.h new file mode 100644 index 0000000..71ad533 --- /dev/null +++ b/include/M034.h @@ -0,0 +1,38 @@ +/* +* M034.h - Library for M034 +* Created by brucker 22.03.2023 +*/ + +#ifndef M034_H +#define M034_H + +#include // main arduino library +#include // Arduino SPI library +/* +#include "Pin_Definitions.h" // pin definitions +#include // stepper motor shield library +#include // Arduino SPI library +#include // Arduino EEPROM library +*/ + +/********************************************************************** + * classes * + **********************************************************************/ +// encoder position class +class NanotecAbsolutEncoder{ + public: + NanotecAbsolutEncoder(SPIClass *SPIPort); + void readPosition(void); + uint16_t getMultiturn(void); + uint32_t getSingleturn(void); + uint8_t getStatus(void); + private: + uint16_t _startbits = 0; + uint16_t _multiturn = 0; + uint32_t _singleturn = 0; + uint8_t _status = 0; + uint8_t _readBytes[7]; + SPIClass *_SPI; +}; + +#endif \ No newline at end of file diff --git a/include/Pin_Definitions.h b/include/Pin_Definitions.h new file mode 100644 index 0000000..a386199 --- /dev/null +++ b/include/Pin_Definitions.h @@ -0,0 +1,66 @@ +/* +* Pin Definitins for ST Nucleo-F429ZI 144 Pin +* Created by brucker 21.03.2023 +*/ + +#ifndef M034_PIN_DEFINITIONS_H +#define M034_PIN_DEFINITIONS_H +// Pin Numbers: Nucleo Pin Name // Nucleo Connector Pin, STM32 Pin, Function +// Pin definitions + +// I2C +int const PIN_I2C_SDA = D20; // CN7:4, PB9 +int const PIN_I2C_SCL = D21; // CN7:2, PB8 + +// SPI1 (Arduino compatible) +int const PIN_SPI_1_MOSI = D11; // CN7:14, PA7 +int const PIN_SPI_1_MISO = D12; // CN7:12, PA6 +int const PIN_SPI_1_CLK = D13; // CN7:10, PA5 +int const PIN_SPI_1_SS = D10; // CN7:16, PD14 +// SPI3 +int const PIN_SPI_3_MOSI = PB5_ALT1;// CN7:13, PB5, D22 +int const PIN_SPI_3_MISO = PB4_ALT1;// CN7:19, PB4, D25 +int const PIN_SPI_3_CLK = PB3_ALT1; // CN7:15, PB3, D23 +int const PIN_SPI_3_SS = D24; // CN7:17, PA4 +// SPI4 +int const PIN_SPI_4_MOSI = D59; // CN9:20, PE6 +int const PIN_SPI_4_MISO = D58; // CN9:18, PE5 +int const PIN_SPI_4_CLK = D39; // CN10:26, PE12 +int const PIN_SPI_4_SS = D57; // CN9:16, PE4 +// SPI5 +int const PIN_SPI_5_MOSI = D63; // CN9:28, PF9 +int const PIN_SPI_5_MISO = D61; // CN9:24, PF8 +int const PIN_SPI_5_CLK = D62; // CN9:26, PF7 + +// Pin definitions for the X-NUCLEO-IHM03A1 +int const PIN_STEPPER_FLAG = D2; // CN10:12, PF15 +int const PIN_STEPPER_BUSY = D4; // CN10:8, PF14 +int const PIN_STEPPER_RESET = D8; // CN7:20, PF12 +int const PIN_STEPPER_STEP_CLK = D9;// CN7:18, PD15, SPI1 +int const PIN_STEPPER_SPI_SS = D10; // CN7:16, PD14, SPI1 +// naming scheme of stepper motor library +int const nBUSY_PIN = D4; +int const nSTBY_nRESET_PIN = D8; +int const STCK_PIN = D9; +int const nCS_PIN = D10; + +// RGB-LED +int const PIN_RGB_LED_R = D6; // CN10:4, PE9 +int const PIN_RGB_LED_G = D5; // CN10:6, PE11 +int const PIN_RGB_LED_B = D3; // CN10:10, PE13 + +// Optical UART +int const PIN_OPTICAL_RX = D0; // CN10:16, PG9, USART6 +int const PIN_OPTICAL_TX = D1; // CN10:14, PG14, USART6 + +// TTL input on Lemo for position controll +int const PIN_POS1 = A3; // CN9:7, PF3 +int const PIN_POS2 = A4; // CN9:9, PF5 +int const PIN_POS3 = A5; // CN9:11, PF10 + +// ADC inputs +int const PIN_ADC_5V = A1; // CN9:3, PC0 +int const PIN_ADV_VIN = A2; // CN9:5, PC3 +int const PIN_ADC_I_M = A0; // CN9:1, PA3 + +#endif //M034_PIN_DEFINITIONS_H 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/README b/lib/README new file mode 100644 index 0000000..6debab1 --- /dev/null +++ b/lib/README @@ -0,0 +1,46 @@ + +This directory is intended for project specific (private) libraries. +PlatformIO will compile them to static libraries and link into executable file. + +The source code of each library should be placed in a an own separate directory +("lib/your_library_name/[here are source files]"). + +For example, see a structure of the following two libraries `Foo` and `Bar`: + +|--lib +| | +| |--Bar +| | |--docs +| | |--examples +| | |--src +| | |- Bar.c +| | |- Bar.h +| | |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html +| | +| |--Foo +| | |- Foo.c +| | |- Foo.h +| | +| |- README --> THIS FILE +| +|- platformio.ini +|--src + |- main.c + +and a contents of `src/main.c`: +``` +#include +#include + +int main (void) +{ + ... +} + +``` + +PlatformIO Library Dependency Finder will find automatically dependent +libraries scanning project source files. + +More information about PlatformIO Library Dependency Finder +- https://docs.platformio.org/page/librarymanager/ldf.html diff --git a/platformio.ini b/platformio.ini new file mode 100644 index 0000000..ee22e76 --- /dev/null +++ b/platformio.ini @@ -0,0 +1,16 @@ +; PlatformIO Project Configuration File +; +; Build options: build flags, source filter +; Upload options: custom upload port, speed and extra flags +; Library options: dependencies, extra library storages +; Advanced options: extra scripting +; +; Please visit documentation for the other options and examples +; https://docs.platformio.org/page/projectconf.html + +[env:nucleo_f429zi] +platform = ststm32 +board = nucleo_f429zi +framework = arduino +lib_deps = ponoor/Ponoor PowerSTEP01 Library@^1.0.2 +monitor_speed = 921600 \ No newline at end of file diff --git a/src/M034.cpp b/src/M034.cpp new file mode 100644 index 0000000..0a37df0 --- /dev/null +++ b/src/M034.cpp @@ -0,0 +1,58 @@ +/* +* M034.h - Library for M034 +* Created by brucker 22.03.2023 +*/ + +#include "M034.h" // classes and functions for M034 + +/* +* Nanotec absolut position encoder +* SSI format length: 51 bit +* 16 startbits, all always zero +* 16 multiturn bits, MSB first +* 17 singleturn bits, MSB first +* 1 bit "D1", always zero +* 1 bit "D2", Error if 0 +* SPI read 7 bytes = 56 bit +* convert read data: +* shift relevant bits to the right position, mask not relevant bits and link bytes +*/ + +NanotecAbsolutEncoder::NanotecAbsolutEncoder(SPIClass *SPIPort){ // constructor + _SPI = SPIPort; +} + +void NanotecAbsolutEncoder::readPosition(){ // read position from encoder + + noInterrupts(); + for(uint8_t i=0; i<7; i++){ // read 7 bytes + _readBytes[i] = _SPI->transfer (0); // store in array + } + interrupts(); + delayMicroseconds(10); // time out of encoder is typ. 7µs +} + +uint16_t NanotecAbsolutEncoder::getMultiturn(){ + _multiturn = (uint16_t) _readBytes[2] << 9; // convert and return multiturn + _multiturn |= (uint16_t) _readBytes[3] << 1; + _multiturn |= (uint16_t) _readBytes[4] >> 7; + return _multiturn; +} + +uint32_t NanotecAbsolutEncoder::getSingleturn(){ // convert and return singelturn + _singleturn = (uint32_t) (_readBytes[4] & 0x7F) << 10; + _singleturn |= (uint32_t) _readBytes[5] << 2; + _singleturn |= (uint32_t) _readBytes[6] >> 6; + return _singleturn; +} + +uint8_t NanotecAbsolutEncoder::getStatus(){ // check if encoder or transmission error exists + _startbits = (uint16_t) _readBytes[0] << 9; // convert startbits + _startbits |= (uint16_t) _readBytes[1] << 1; + _startbits |= (uint16_t) _readBytes[2] >> 7; + _status = (_readBytes[6] >> 4) & 0x03; // convert status + if(_status == 1 & _startbits == 0){ + return 0; // return without error + } + else return 1; // return with error +} diff --git a/src/main.cpp b/src/main.cpp new file mode 100644 index 0000000..168a9b5 --- /dev/null +++ b/src/main.cpp @@ -0,0 +1,632 @@ +/* + * short explanation + */ + +#include // main arduino library +#include // stepper motor shield library +#include // Arduino SPI library +#include // Arduino EEPROM library +#include "Pin_Definitions.h" // pin definitions +#include "M034.h" // include header file for project + +// string with manual for printing over UART +const String manual = "\nM034 stepper motor controll\n" + "protocol structure: command parameter e.g. movl 200\n" + "commands (in small letters):\n" + "help: print instruction manual\n" + "movl (move left): spin x steps counterclockwise\n" + "movr (move right): spin x steps clockwise\n" + "movp (move to position): move motor to position uint32_t x\n" + "gtmp (go to motor position): go to saved motor position x {0-2}\n" + "gtep (go to encoder position): go to saved encoder position x {0-2}\n" + "smpo (set motor position): save current motor position (moved steps) as position x {0-2}\n" + "sepo (set encoder position): save current encoder position as position x {0-2}\n" + "getp (get position): get current position\n" + "setn (set null): set current position as null (home)\n" + "setm (set mode): set mode to: 0 - voltage mode; 1 - current mode\n" + "sets (set microstep size):\n" + "\tOptions: 1, 1/2, 1/4, 1/8, 1/16, 1/32, 1/64, 1/128\n" + "\tSetS parameter = 1 for 1; 2 for 1/2; ...\n" + "\tIn current mode the maximum microstepping resolution is 1/16\n" + "\tIf changed, Null and saved positions get lost\n" + "gete (get encoder position): get current encoder singleturn position (average over x)\n" + "seep (set EEPROM encoder position): save current encoder singleturn home position to EEPROM\n" + "reep (reed EEPROM encoder position): reed current encoder singleturn home position from EEPROM\n" + "init (initialize): move to encoder EEPROM home position and set it to position null\n" + "\n" +; + + +/********************************************************************** + * constant definitions * + **********************************************************************/ +const uint32_t SERIAL_BAUD_RATE = 921600; // Baud Rate of serial output + // if changed, must also be changed in platformio.ini: monitor_speed +const SPISettings SPI_ENC_Settings(2600000, MSBFIRST, SPI_MODE3); // SPI settings for stepper motor encoder +const uint8_t POSITION_COUNT = 3; // legth of position array +const uint32_t ENCODER_AV_COUNT = 1024; // Number of encoder positions over which to average +const uint32_t MOTORSTEPS = 200; // number of full steps wich the motor has + +// EEPROM (test) +// const + +// motor power settings. Complete explanation in setup. Tested with 16 microsteps +const byte CM_RUN_TVAL = 20; // motor power settings for current mode +const byte CM_ACC_TVAL = 32; +const byte CM_DEC_TVAL = 32; +const byte CM_HOLD_TVAL = 16; +const float CM_MAX_SPEED = 1000.0; +const byte VM_RUN_TVAL = 16; // motor power settings for voltage mode +const byte VM_ACC_TVAL = 24; +const byte VM_DEC_TVAL = 24; +const byte VM_HOLD_TVAL = 28; +const float VM_MAX_SPEED = 1000.0; + + +/********************************************************************** + * global variables * + **********************************************************************/ + +long positionMotor[POSITION_COUNT] = {(long)0, (long)1062, (long)-1067}; // array to save motor positions +int32_t positionEncoder[POSITION_COUNT] = {}; // array to save motor positions +uint16_t eeAddress = 0; // Location of next free EEPROM address +uint32_t motorMicrosteps = 16; // number of microsteps wich currently used + + +/********************************************************************** + * function declaration * + **********************************************************************/ +void controlUart(uint8_t port); // check if uart communication is incomming on one of the UART's. If this is the case, read and execute incomming command +int32_t averageEncPosition(uint32_t count); // read and build avverage of current encoder position + + +/********************************************************************** + * instance generation * + **********************************************************************/ +HardwareSerial Serial6 (D0, D1); // create Serial instance for optical links on USART6 +SPIClass SPI_Encoder(PIN_SPI_5_MOSI, PIN_SPI_5_MISO, PIN_SPI_5_CLK); // create SPI instance for encoder on SPI5 +//SPIClass * SPI_Encoder_ptr = &SPI_Encoder; // pointer to SPI_Encoder +//NanotecAbsolutEncoder encoder(SPI_Encoder_ptr); // create encoder instance +NanotecAbsolutEncoder encoder(&SPI_Encoder); // create encoder instance +powerSTEP driver(0, nCS_PIN, nSTBY_nRESET_PIN); // powerSTEP library instance, parameters are distance from the end of a daisy-chain of drivers, !CS pin, !STBY/!Reset pin + + + + +/********************************************************************** + * setup * + **********************************************************************/ +void setup() { + // Prepare pins + pinMode(nSTBY_nRESET_PIN, OUTPUT);// for stepper motor shield + pinMode(nCS_PIN, OUTPUT); // for stepper motor shield + pinMode(PIN_SPI_1_MOSI, OUTPUT); // SPI1 MOSI for stepper motor shield + pinMode(PIN_SPI_1_MISO, INPUT); // SPI1 MISO for stepper motor shield + pinMode(PIN_SPI_1_CLK, OUTPUT); // SPI1 CLK for stepper motor shield + + pinMode(PIN_SPI_5_MOSI, OUTPUT); // SPI5 MOSI: not nessesary for our encoder + pinMode(PIN_SPI_5_MISO, INPUT); // SPI5 MISO from encoder + pinMode(PIN_SPI_5_CLK, OUTPUT); // SPI5 CLK for encoder + + pinMode(LED_GREEN, OUTPUT); // Green LED on Nucleo-144 + pinMode(LED_BLUE, OUTPUT); // Blue LED on Nucleo-144 + pinMode(LED_RED, OUTPUT); // Red LED on Nucleo-144 + + pinMode(PIN_RGB_LED_R, OUTPUT); // Red part of the RGB LED + pinMode(PIN_RGB_LED_G, OUTPUT); // Green part of the RGB LED + pinMode(PIN_RGB_LED_B, OUTPUT); // Blue part of the RGB LED + + pinMode(PIN_POS1, INPUT_PULLDOWN);// TTL Input for position controll + pinMode(PIN_POS2, INPUT_PULLDOWN);// TTL Input for position controll + pinMode(PIN_POS3, INPUT_PULLDOWN);// TTL Input for position controll + + Serial.begin(SERIAL_BAUD_RATE); // Start serial UART on USB + Serial6.begin(SERIAL_BAUD_RATE); // Start serial UART on optical links + + SPI_Encoder.begin(); // Start SPI for stepper motor encoder + SPI_Encoder.beginTransaction(SPI_ENC_Settings); // set correct settings + + + + // initialise stepper motor shield + // Reset powerSTEP and set CS + digitalWrite(nSTBY_nRESET_PIN, HIGH); + digitalWrite(nSTBY_nRESET_PIN, LOW); + digitalWrite(nSTBY_nRESET_PIN, HIGH); + digitalWrite(nCS_PIN, HIGH); + + SPI.begin(); // Start SPI1 for stepper shield + SPI.setDataMode(SPI_MODE3); // set correct SPI mode + + // Configure powerSTEP + driver.SPIPortConnect(&SPI); // give library the SPI port (only the one on an Uno) + + driver.configSyncPin(BUSY_PIN, 0); // use SYNC/nBUSY pin as nBUSY, + // thus syncSteps (2nd paramater) does nothing + + driver.configStepMode(STEP_FS_16); // 1/16 microstepping, full steps = STEP_FS, + //driver.configStepMode(STEP_FS); // 1/16 microstepping, full steps = STEP_FS, + // options: 1, 1/2, 1/4, 1/8, 1/16, 1/32, 1/64, 1/128 + // In current mode the maximum microstepping resolution is 1/16 + + driver.setMaxSpeed(1000.0); // max speed in units of full steps/s (15.25 to 15610) + driver.setFullSpeed(2000.0); // full steps/s threshold for disabling microstepping + //driver.setAcc(1800); // full steps/s^2 acceleration + //driver.setDec(1800); // full steps/s^2 deceleration + driver.setAcc(900); // full steps/s^2 acceleration + driver.setDec(900); // full steps/s^2 deceleration + + driver.setSlewRate(SR_520V_us); // faster may give more torque (but also EM noise), + // options are: 114, 220, 400, 520, 790, 980(V/us) + + driver.setOCThreshold(16); // over-current threshold for the 2.8A NEMA23 motor + // used in testing. If your motor stops working for + // no apparent reason, it's probably this. Start low + // and increase until it doesn't trip, then maybe + // add one to avoid misfires. Can prevent catastrophic + // failures caused by shorts + driver.setOCShutdown(OC_SD_ENABLE); // shutdown motor bridge on over-current event + // to protect against permanant damage + + driver.setPWMFreq(PWM_DIV_1, PWM_MUL_0_75); // 16MHz*0.75/(512*1) = 23.4375kHz + // power is supplied to stepper phases as a sin wave, + // frequency is set by two PWM modulators, + // Fpwm = Fosc*m/(512*N), N and m are set by DIV and MUL, + // options: DIV: 1, 2, 3, 4, 5, 6, 7, + // MUL: 0.625, 0.75, 0.875, 1, 1.25, 1.5, 1.75, 2 + + driver.setVoltageComp(VS_COMP_DISABLE); // no compensation for variation in Vs as + // ADC voltage divider is not populated + + driver.setSwitchMode(SW_USER); // switch doesn't trigger stop, status can be read. + // SW_HARD_STOP: TP1 causes hard stop on connection + // to GND, you get stuck on switch after homing + + driver.setOscMode(INT_16MHZ); // 16MHz internal oscillator as clock source + + // KVAL registers set the power to the motor by adjusting the PWM duty cycle, + // use a value between 0-255 where 0 = no power, 255 = full power. + // Start low and monitor the motor temperature until you find a safe balance + // between power and temperature. Only use what you need + + driver.setRunTVAL(CM_RUN_TVAL); + driver.setAccTVAL(CM_ACC_TVAL); + driver.setDecTVAL(CM_DEC_TVAL); + driver.setHoldTVAL(CM_HOLD_TVAL); + driver.setCurrentMode(); // set current mode + /* + driver.setMaxSpeed(1000.); + driver.setRunKVAL(VM_RUN_TVAL); + driver.setAccKVAL(VM_ACC_TVAL); + driver.setDecKVAL(VM_DEC_TVAL); + driver.setHoldKVAL(VM_HOLD_TVAL); + */ + + driver.setParam(ALARM_EN, 0x8F); // disable ADC UVLO (divider not populated), + // disable stall detection (not configured), + // disable switch (not using as hard stop) + + driver.getStatus(); // clears error flags + // end of stepper motor shield initialisation + + + + + // led greeting + digitalWrite(PIN_RGB_LED_R, LOW); // set all off + digitalWrite(PIN_RGB_LED_G, LOW); + digitalWrite(PIN_RGB_LED_B, LOW); + digitalWrite(PIN_RGB_LED_R, HIGH);// red on + delay(300); + digitalWrite(PIN_RGB_LED_R, LOW); // red off + digitalWrite(PIN_RGB_LED_G, HIGH);// blue on + delay(300); + digitalWrite(PIN_RGB_LED_G, LOW); // blue off + digitalWrite(PIN_RGB_LED_B, HIGH);// green on + delay(300); + digitalWrite(PIN_RGB_LED_B, LOW); // green off + + Serial.print(manual); // print instruction manual on USB UART + Serial6.print(manual); // print instruction manual on optical UART +} // end of setup() + + + + +/********************************************************************** + * loop * + **********************************************************************/ +void loop() { + if(Serial.available() != 0){ // check if uart communication is incomming on USB UART + controlUart(3); // If this is the case, read and execute incomming command + } + if(Serial6.available() != 0){ // check if uart communication is incomming on optical UART + controlUart(6); // If this is the case, execute command + } + + if(digitalRead(PIN_POS1)==HIGH){ // if TTL position 1 input is HIGH + driver.goTo(positionMotor[0]); // drive motor to saved position 1 + } + if(digitalRead(PIN_POS2)==HIGH){ // same for position 2 + driver.goTo(positionMotor[1]); + } + if(digitalRead(PIN_POS3)==HIGH){ // and position 3 + driver.goTo(positionMotor[2]); + } + +} // end of loop() + + + + + + + + +/********************************************************************** + * functions * + **********************************************************************/ +// read and execute incomming UART command +void controlUart(uint8_t port){ + String input; // String for buffering read data + String parameterStr; // STring for extracted parameter + uint32_t parameter = 0; // variable for storing command parameter + String message; // String for buffering vallidation message + String errorMessage; // String for buffering error message + + switch(port){ // Read from the port where the communication is coming from + case 3: + input = Serial.readStringUntil('\n'); // read till line feed + break; + case 6: + input = Serial6.readStringUntil('\n'); // read till line feed + break; + } + input.trim(); // remove leading and trailing whitespace + parameterStr = input.substring(5); // extract parameter: read from sign 5 + parameter = atoi(parameterStr.c_str()); // convert String into int + + // search for controll string. If found execute command. + // movl (move left): spin x steps counterclockwise + if(input.startsWith("movl")){ + driver.move(REV, parameter); + while(driver.busyCheck()); + driver.softStop(); + while(driver.busyCheck()); + message = "Motor is moved counterclockwise. Steps: "; + message += (String) parameter; + } + + // movr (move right): spin x steps clockwise + else if(input.startsWith("movr")){ + driver.move(FWD, parameter); + while(driver.busyCheck()); + driver.softStop(); + while(driver.busyCheck()); + message = "Motor is moved clockwise. Steps: "; + message += (String) parameter; + } + + // movp (move to position): move motor to position uint32_t x + else if(input.startsWith("movp")){ + driver.goTo(parameter); + while(driver.busyCheck()); + driver.softStop(); + while(driver.busyCheck()); + message = "Motor is moved to position: "; + message += (String) parameter; + } + + // gtmp (go to motor position): go to saved motor position x {0-2} + else if(input.startsWith("gtmp")){ + if(parameter0){ + message = (String) averageEncPosition(parameter); // read and build avverage of current encoder position + } + else{ + errorMessage = "Parameter out of range"; + } + } + + // seep (set EEPROM encoder position): save current encoder singleturn home position to EEPROM + else if(input.startsWith("seep")){ + int32_t currentPosition; + currentPosition = averageEncPosition(ENCODER_AV_COUNT); + EEPROM.put(eeAddress, currentPosition); + message = "Saved as encoder home position in EEPROM: "; + message += (String) currentPosition; + } + + // reep (reed EEPROM encoder position): reed current encoder singleturn home position from EEPROM + else if(input.startsWith("reep")){ + EEPROM.get(eeAddress, parameter); // reed address from EEPROM and save in parameter + message = "Encoder home position in EEPROM is: "; + message += (String) parameter; + } + + // init (initialize): move to encoder EEPROM home position and set it to position null + else if(input.startsWith("init")){ + int32_t homePosition; + int32_t currentPosition; + int32_t steps2Drive = 1; + uint8_t i = 0; // loop counter + message = "Moved steps to EEPROM encoder home position: "; + driver.softStop(); // hold motor + driver.move(FWD, 10); // move a little bit so that the later move begins at a good position + while(driver.busyCheck()); + driver.move(REV, 10); // move back so that a second init returns 0 moved steps + while(driver.busyCheck()); + driver.softStop(); // hold motor + while(driver.busyCheck()); + delay(100); // wait till motor stands still + EEPROM.get(eeAddress, homePosition); // read home position from EEPROM + + + while(steps2Drive != 0){ // move motor to position null till the position is reached, because null is not reached exactly every time + currentPosition = averageEncPosition(ENCODER_AV_COUNT); // read singleturn position + steps2Drive = int32_t ((double)(currentPosition - homePosition) / (131072.0 / (MOTORSTEPS*motorMicrosteps))); // calculate steps to move + driver.resetPos(); // set motor driver position 0 + driver.goTo(steps2Drive); // move calculated steps + while(driver.busyCheck()); + driver.softStop(); + while(driver.busyCheck()); + delay(10); + message += (String) steps2Drive; + message += ("; "); + i++; + if(i==10){ + errorMessage = ("init not possible. Check neutron switch. Maybe motor parameter must be updated"); + break; + } + } + + driver.resetPos(); // set motor driver position 0 + while(driver.busyCheck()); + } + + // help: print instruction manual + else if(input.startsWith("help")){ + message = manual; + } + + // hold: test how much hold power is needed + else if(input.startsWith("hold")){ + delay(1000); + driver.setHoldKVAL(24); + delay(2000); + driver.setHoldKVAL(16); + delay(2000); + message = ("hold test done"); + } + + // setb: test set backup register + else if(input.startsWith("setb")){ + uint32_t index = 0; + uint32_t value = 1000; + int32_t currentPosition; + currentPosition = averageEncPosition(ENCODER_AV_COUNT); + //setBackupRegister(index, value); + EEPROM.put(8, currentPosition); + + message = ("Backup register written"); + } + + // getb: test set backup register + else if(input.startsWith("getb")){ + Serial6.println(getBackupRegister(0)); + uint8_t test = 0; + message = ("Backup register:"); + for(int8_t i=10; i<20; i++){ + message += (" "); + message += EEPROM.get(i, test); + } + } + + // command unknown + else{ + errorMessage = "Command unknown or parameter is missing. Send 'help' to get the manual again"; + } + + if(message.length() != 0){ // if message available, print on the port on which command was receiverd + switch(port){ + case 3: + Serial.println(message); + break; + case 6: + Serial6.println(message); + break; + } + } + if(errorMessage.length() != 0){ // if error message available, print on the port on which command was receiverd + switch(port){ + case 3: + Serial.println(errorMessage); + break; + case 6: + Serial6.println(errorMessage); + break; + } + } + return; +} // end of controlUart() + + + +// read and build avverage of current encoder position +int32_t averageEncPosition(uint32_t count){ + int32_t encoderPositionAv = 0; + uint32_t i = 0; + for(i; i