Steffen Brucker
2 years ago
commit
4bc7e99780
10 changed files with 921 additions and 0 deletions
-
5.gitignore
-
10.vscode/extensions.json
-
38include/M034.h
-
66include/Pin_Definitions.h
-
39include/README
-
46lib/README
-
16platformio.ini
-
58src/M034.cpp
-
632src/main.cpp
-
11test/README
@ -0,0 +1,5 @@ |
|||
.pio |
|||
.vscode/.browse.c_cpp.db* |
|||
.vscode/c_cpp_properties.json |
|||
.vscode/launch.json |
|||
.vscode/ipch |
@ -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" |
|||
] |
|||
} |
@ -0,0 +1,38 @@ |
|||
/* |
|||
* M034.h - Library for M034 |
|||
* Created by brucker 22.03.2023 |
|||
*/ |
|||
|
|||
#ifndef M034_H |
|||
#define M034_H |
|||
|
|||
#include <Arduino.h> // main arduino library |
|||
#include <SPI.h> // Arduino SPI library |
|||
/* |
|||
#include "Pin_Definitions.h" // pin definitions |
|||
#include <Ponoor_PowerSTEP01Library.h> // stepper motor shield library |
|||
#include <SPI.h> // Arduino SPI library |
|||
#include <EEPROM.h> // 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 |
@ -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 |
@ -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 |
@ -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 <Foo.h> |
|||
#include <Bar.h> |
|||
|
|||
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 |
@ -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 |
@ -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
|
|||
} |
@ -0,0 +1,632 @@ |
|||
/*
|
|||
* short explanation |
|||
*/ |
|||
|
|||
#include <Arduino.h> // main arduino library
|
|||
#include <Ponoor_PowerSTEP01Library.h> // stepper motor shield library
|
|||
#include <SPI.h> // Arduino SPI library
|
|||
#include <EEPROM.h> // 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(parameter<POSITION_COUNT){ |
|||
driver.goTo(positionMotor[parameter]); |
|||
while(driver.busyCheck()); |
|||
driver.softStop(); |
|||
while(driver.busyCheck()); |
|||
message = "Motor is moved to saved position: "; |
|||
message += (String) parameter; |
|||
} |
|||
else{ |
|||
errorMessage = "Parameter out of range"; |
|||
} |
|||
} |
|||
|
|||
// gtep (go to encoder position): go to saved encoder position x {0-2}
|
|||
else if(input.startsWith("gtep")){ |
|||
if(parameter<POSITION_COUNT){ |
|||
int32_t currentPosition; |
|||
int32_t steps2Drive = 1; |
|||
uint8_t i = 0; // loop counter
|
|||
message = "Motor is moved to saved position: "; |
|||
message += (String) parameter; |
|||
message += ". Moved steps: "; |
|||
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 - positionEncoder[parameter]) / (131072.0 / (MOTORSTEPS*motorMicrosteps))); // calculate steps to move
|
|||
if(steps2Drive<0){ |
|||
driver.move(REV, ((unsigned long) (steps2Drive * -1))); |
|||
} |
|||
else{ |
|||
driver.move(FWD, (unsigned long) steps2Drive); |
|||
} |
|||
while(driver.busyCheck()); |
|||
driver.softStop(); |
|||
while(driver.busyCheck()); |
|||
delay(10); |
|||
message += (String) steps2Drive; |
|||
message += ("; "); |
|||
i++; |
|||
if(i==10){ |
|||
errorMessage = ("positioning not possible. Check neutron switch. Maybe motor parameter must be updated"); |
|||
break; |
|||
} |
|||
} |
|||
} |
|||
} |
|||
|
|||
// smpo (set motor position): save current motor position (moved steps) as position x {0-2}
|
|||
else if(input.startsWith("smpo")){ |
|||
if(parameter<POSITION_COUNT){ |
|||
positionMotor[parameter] = driver.getPos(); |
|||
message = "Current position is saved as motor position: "; |
|||
message += (String) parameter; |
|||
} |
|||
else{ |
|||
errorMessage = "Parameter out of range"; |
|||
} |
|||
} |
|||
|
|||
// sepo (set encoder position): save current encoder position as position x {0-2}
|
|||
else if(input.startsWith("sepo")){ |
|||
if(parameter<POSITION_COUNT){ |
|||
positionEncoder[parameter] = averageEncPosition(ENCODER_AV_COUNT); // read singleturn position
|
|||
message = "Current position is saved as encoder position: "; |
|||
message += (String) parameter; |
|||
} |
|||
else{ |
|||
errorMessage = "Parameter out of range"; |
|||
} |
|||
} |
|||
|
|||
// getp (get position): get current position
|
|||
else if(input.startsWith("getp")){ |
|||
long position = driver.getPos(); |
|||
message = "Current position is: "; |
|||
message = (String) position; |
|||
} |
|||
|
|||
// setn (set null): set current position as null (home)
|
|||
else if(input.startsWith("setn")){ |
|||
driver.resetPos(); |
|||
message = "Current position is set to 0"; |
|||
} |
|||
|
|||
// setm (set mode): set mode to: 0 - voltage mode; 1 - current mode
|
|||
else if(input.startsWith("setm")){ |
|||
if(parameter==0){ |
|||
// Settings at 24V
|
|||
driver.softHiZ(); |
|||
driver.setMaxSpeed(VM_MAX_SPEED); |
|||
driver.setRunTVAL(VM_RUN_TVAL); |
|||
driver.setAccTVAL(VM_ACC_TVAL); |
|||
driver.setDecTVAL(VM_DEC_TVAL); |
|||
driver.setHoldTVAL(VM_HOLD_TVAL); |
|||
driver.setVoltageMode(); |
|||
message = ("Voltage mode is set"); |
|||
} |
|||
else if(parameter==1){ |
|||
driver.softHiZ(); |
|||
driver.setMaxSpeed(CM_MAX_SPEED); |
|||
driver.setRunKVAL(CM_RUN_TVAL); |
|||
driver.setAccKVAL(CM_ACC_TVAL); |
|||
driver.setDecKVAL(CM_DEC_TVAL); |
|||
driver.setHoldKVAL(CM_HOLD_TVAL); |
|||
driver.setCurrentMode(); |
|||
message = ("Current mode is set"); |
|||
} |
|||
else errorMessage = "Parameter out of range"; |
|||
} |
|||
|
|||
// sets (set microstep size):
|
|||
// Options: 1, 1/2, 1/4, 1/8, 1/16, 1/32, 1/64, 1/128
|
|||
// SetS parameter = 1 for 1; 2 for 1/2; ...
|
|||
// In current mode the maximum microstepping resolution is 1/16
|
|||
// If changed, Null and saved positions get lost
|
|||
else if(input.startsWith("sets")){ |
|||
if(parameter==1){ |
|||
driver.softHiZ(); |
|||
driver.configStepMode(STEP_FS); // full steps
|
|||
message = "Full step is set"; |
|||
} |
|||
else if(parameter==2){ |
|||
driver.softHiZ(); |
|||
driver.configStepMode(STEP_FS_2); // 2 microsteps
|
|||
message = "microstep size 1/2 is set"; |
|||
} |
|||
else if(parameter==4){ |
|||
driver.softHiZ(); |
|||
driver.configStepMode(STEP_FS_4); // 4 microsteps
|
|||
message = "microstep size 1/4 is set"; |
|||
} |
|||
else if(parameter==8){ |
|||
driver.softHiZ(); |
|||
driver.configStepMode(STEP_FS_8); // 8 microsteps
|
|||
message = "microstep size 1/8 is set"; |
|||
} |
|||
else if(parameter==16){ |
|||
driver.softHiZ(); |
|||
driver.configStepMode(STEP_FS_16); // 16 microsteps
|
|||
message = "microstep size 1/16 is set"; |
|||
} |
|||
else if(parameter==32){ |
|||
driver.softHiZ(); |
|||
driver.configStepMode(STEP_FS_32); // 32 microsteps
|
|||
message = "microstep size 1/32 is set"; |
|||
} |
|||
else if(parameter==64){ |
|||
driver.softHiZ(); |
|||
driver.configStepMode(STEP_FS_64); // 64 microsteps
|
|||
message = "microstep size 1/64 is set"; |
|||
} |
|||
else if(parameter==128){ |
|||
driver.softHiZ(); |
|||
driver.configStepMode(STEP_FS_128); // 128 microsteps
|
|||
message = "microstep size 1/128 is set"; |
|||
} |
|||
else{ |
|||
errorMessage = "wrong parameter for sets"; |
|||
} |
|||
message += "\nposition lost, new initialisation needed"; |
|||
} |
|||
|
|||
// gete (get encoder position): get current encoder singleturn position (average over x)
|
|||
else if(input.startsWith("gete")){ |
|||
if(parameter>0){ |
|||
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<count; i++){ |
|||
encoder.readPosition(); // read position from encoder
|
|||
if(encoder.getStatus()==0){ // if status is good,
|
|||
encoderPositionAv += encoder.getSingleturn(); //sum up positions
|
|||
} |
|||
else{ // if status is not good,
|
|||
i--; // count down i so that the count is correct
|
|||
} |
|||
} |
|||
encoderPositionAv = encoderPositionAv / i; // build average
|
|||
return encoderPositionAv; |
|||
} // end of averageEncPosition()
|
@ -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 |
Write
Preview
Loading…
Cancel
Save
Reference in new issue