EW/M034_LEPP-neutron-switch
2
0
Fork 0
Code Issues Pull Requests repo.project_board Releases Wiki Activity

Uses encoder for movement.

Browse Source 
All 3 encoder positions are now backuped in EEPROM.
Much more things changed
This commit is contained in:
Steffen Brucker 2024-03-13 20:34:32 +01:00
parent 189291d597
commit 5d33516779
3 changed files with 187 additions and 79 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -0,0 +1,3 @@
{
"cmake.configureOnOpen": false
}

22
include/Pin_Definitions.h
View File

@ -53,14 +53,30 @@ int const PIN_RGB_LED_B = D3; // CN10:10, PE13
int const PIN_OPTICAL_RX = D0; // CN10:16, PG9, USART6 int const PIN_OPTICAL_RX = D0; // CN10:16, PG9, USART6
int const PIN_OPTICAL_TX = D1; // CN10:14, PG14, USART6 int const PIN_OPTICAL_TX = D1; // CN10:14, PG14, USART6
// Push button for position controll
int const PIN_BUTTON_POS1 = A6; // CN10:7, PB1
int const PIN_BUTTON_POS2 = A7; // CN10:9, PC2
int const PIN_BUTTON_POS3 = A8; // CN10:11, PF4
// TTL input on Lemo for position controll // TTL input on Lemo for position controll
int const PIN_POS1 = A3; // CN9:7, PF3 int const PIN_LEMO_POS1 = D70; // CN9:17, PF2
int const PIN_POS2 = A4; // CN9:9, PF5 int const PIN_LEMO_POS2 = A5; // CN9:11, PF10
int const PIN_POS3 = A5; // CN9:11, PF10 int const PIN_LEMO_POS3 = A4; // CN9:9, PF5
// TTL output on Lemo for Ready Signal when Positioning finished
int const PIN_LEMO_READY = A3; //CN9:7, PF3
// ADC inputs // ADC inputs
int const PIN_ADC_5V = A1; // CN9:3, PC0 int const PIN_ADC_5V = A1; // CN9:3, PC0
int const PIN_ADV_VIN = A2; // CN9:5, PC3 int const PIN_ADV_VIN = A2; // CN9:5, PC3
int const PIN_ADC_I_M = A0; // CN9:1, PA3 int const PIN_ADC_I_M = A0; // CN9:1, PA3
// old for Version 0
/*
// 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
*/
#endif //M034_PIN_DEFINITIONS_H #endif //M034_PIN_DEFINITIONS_H

241
src/main.cpp
View File

@ -14,26 +14,38 @@ const String manual = "\nM034 stepper motor controll\n"
"protocol structure: command parameter e.g. movl 200\n" "protocol structure: command parameter e.g. movl 200\n"
"commands (in small letters):\n" "commands (in small letters):\n"
"help: print instruction manual\n" "help: print instruction manual\n"
"\n"
"Basic Motor Instructions (without encoder):\n"
"movl (move left): spin x steps counterclockwise\n" "movl (move left): spin x steps counterclockwise\n"
"movr (move right): spin x steps clockwise\n" "movr (move right): spin x steps clockwise\n"
"\n"
"Other motor Instructions (without encoder):\n"
"getp (get position): get current motor position\n"
"movp (move to position): move motor to position uint32_t x\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" "setn (set null): set current position as motor null position (home)\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" "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" "gtmp (go to motor position): go to saved motor position x {0-2}\n"
"getp (get position): get current position\n" "\n"
"setn (set null): set current position as null (home)\n"
"Motor configuration:\n"
"setm (set mode): set mode to: 0 - voltage mode; 1 - current mode\n" "setm (set mode): set mode to: 0 - voltage mode; 1 - current mode\n"
"sets (set microstep size):\n" "sets (set microstep size):\n"
"\tOptions: 1, 1/2, 1/4, 1/8, 1/16, 1/32, 1/64, 1/128\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" "\tSetS parameter = 1 for 1; 2 for 1/2; ...\n"
"\tIn current mode the maximum microstepping resolution is 1/16\n" "\tIn current mode the maximum microstepping resolution is 1/16\n"
"\tIf changed, Null and saved positions get lost\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" "\n"
"Encoder Instructions:\n"
"gete (get encoder position): get current encoder singleturn position (average over x)\n"
"sepo (set encoder position): save current encoder position as position x {0-2}\n"
"gtep (go to encoder position): go to saved encoder position x {0-2}\n"
"sepe (save encoder positions to EEPROM): backup saved encoder positions in EEPROM\n"
"reep (reed EEPROM encoder position): reed current encoder singleturn home position from EEPROM\n"
"init (initialize): load encoder positions from EEPROM, move to encoder 0 position and set it to motor position null\n"
"\n\n"
; ;
@ -50,6 +62,7 @@ const uint32_t MOTORSTEPS = 200; // number of full steps wich the mot
// EEPROM (test) // EEPROM (test)
// const // const
// small neutron-switch
// motor power settings. Complete explanation in setup. Tested with 16 microsteps // 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_RUN_TVAL = 20; // motor power settings for current mode
const byte CM_ACC_TVAL = 32; const byte CM_ACC_TVAL = 32;
@ -61,23 +74,40 @@ const byte VM_ACC_TVAL = 24;
const byte VM_DEC_TVAL = 24; const byte VM_DEC_TVAL = 24;
const byte VM_HOLD_TVAL = 28; const byte VM_HOLD_TVAL = 28;
const float VM_MAX_SPEED = 1000.0; const float VM_MAX_SPEED = 1000.0;
// big neutron-switch
// motor power settings. Complete explanation in setup. Tested with 16 microsteps
/*
const byte CM_RUN_TVAL = 30; // motor power settings for current mode
const byte CM_ACC_TVAL = 40;
const byte CM_DEC_TVAL = 40;
const byte CM_HOLD_TVAL = 24;
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 * * global variables *
**********************************************************************/ **********************************************************************/
long positionMotor[POSITION_COUNT] = {(long)0, (long)1062, (long)-1067}; // array to save motor positions 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 int32_t positionEncoder[POSITION_COUNT] = {}; // array to save encoder positions
uint16_t eeAddress = 0; // Location of next free EEPROM address uint16_t eeAddress = 0; // Location of next free EEPROM address
uint32_t motorMicrosteps = 16; // number of microsteps wich currently used uint32_t motorMicrosteps = 16; // number of microsteps wich currently used
String message; // String for buffering vallidation message
String errorMessage; // String for buffering error message
/********************************************************************** /**********************************************************************
* function declaration * * 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 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 int32_t averageEncPosition(uint32_t count); // read and build avverage of current encoder position
void goToEncoderPosition(uint32_t encoderPosition); // gtep (go to encoder position): go to saved encoder position x {0-2}
/********************************************************************** /**********************************************************************
@ -116,9 +146,15 @@ void setup() {
pinMode(PIN_RGB_LED_G, OUTPUT); // Green 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_RGB_LED_B, OUTPUT); // Blue part of the RGB LED
pinMode(PIN_POS1, INPUT_PULLDOWN);// TTL Input for position controll pinMode(PIN_BUTTON_POS1, INPUT_FLOATING);// Push button for position controll
pinMode(PIN_POS2, INPUT_PULLDOWN);// TTL Input for position controll pinMode(PIN_BUTTON_POS2, INPUT_FLOATING);// Push button for position controll
pinMode(PIN_POS3, INPUT_PULLDOWN);// TTL Input for position controll pinMode(PIN_BUTTON_POS3, INPUT_FLOATING);// Push button for position controll
pinMode(PIN_LEMO_POS1, INPUT_PULLDOWN);// TTL input on Lemo for position controll
pinMode(PIN_LEMO_POS2, INPUT_PULLDOWN);// TTL input on Lemo for position controll
pinMode(PIN_LEMO_POS3, INPUT_PULLDOWN);// TTL input on Lemo for position controll
pinMode(PIN_LEMO_READY, OUTPUT); // TTL output for Ready Signal when Positioning finished
Serial.begin(SERIAL_BAUD_RATE); // Start serial UART on USB Serial.begin(SERIAL_BAUD_RATE); // Start serial UART on USB
Serial6.begin(SERIAL_BAUD_RATE); // Start serial UART on optical links Serial6.begin(SERIAL_BAUD_RATE); // Start serial UART on optical links
@ -210,24 +246,27 @@ void setup() {
// end of stepper motor shield initialisation // end of stepper motor shield initialisation
// init
EEPROM.get(eeAddress, positionEncoder); // restore encoderpositions from EEPROM
// led greeting // led greeting
digitalWrite(PIN_RGB_LED_R, LOW); // set all off digitalWrite(PIN_RGB_LED_R, HIGH); // set all off (common anode)
digitalWrite(PIN_RGB_LED_G, LOW); digitalWrite(PIN_RGB_LED_G, HIGH);
digitalWrite(PIN_RGB_LED_B, LOW); digitalWrite(PIN_RGB_LED_B, HIGH);
digitalWrite(PIN_RGB_LED_R, HIGH);// red on digitalWrite(PIN_RGB_LED_R, LOW);// red on
delay(300); delay(300);
digitalWrite(PIN_RGB_LED_R, LOW); // red off digitalWrite(PIN_RGB_LED_R, HIGH); // red off
digitalWrite(PIN_RGB_LED_G, HIGH);// blue on digitalWrite(PIN_RGB_LED_G, LOW);// blue on
delay(300); delay(300);
digitalWrite(PIN_RGB_LED_G, LOW); // blue off digitalWrite(PIN_RGB_LED_G, HIGH); // blue off
digitalWrite(PIN_RGB_LED_B, HIGH);// green on digitalWrite(PIN_RGB_LED_B, LOW);// green on
delay(300); delay(300);
digitalWrite(PIN_RGB_LED_B, LOW); // green off digitalWrite(PIN_RGB_LED_B, HIGH); // green off
Serial.print(manual); // print instruction manual on USB UART Serial.print(manual); // print instruction manual on USB UART
Serial6.print(manual); // print instruction manual on optical UART Serial6.print(manual); // print instruction manual on optical UART
digitalWrite(PIN_RGB_LED_G, LOW); // Turn green LED on (display setup done)
} // end of setup() } // end of setup()
@ -244,16 +283,28 @@ void loop() {
controlUart(6); // If this is the case, execute command controlUart(6); // If this is the case, execute command
} }
if(digitalRead(PIN_POS1)==HIGH){ // if TTL position 1 input is HIGH if(digitalRead(PIN_LEMO_POS1)==HIGH){ // if TTL position 1 input is HIGH
driver.goTo(positionMotor[0]); // drive motor to saved position 1 goToEncoderPosition(0); // drive motor to saved encoder position 1
} }
if(digitalRead(PIN_POS2)==HIGH){ // same for position 2 if(digitalRead(PIN_LEMO_POS2)==HIGH){ // same for position 2
driver.goTo(positionMotor[1]); goToEncoderPosition(1);
} }
if(digitalRead(PIN_POS3)==HIGH){ // and position 3 if(digitalRead(PIN_LEMO_POS3)==HIGH){ // and position 3
driver.goTo(positionMotor[2]); goToEncoderPosition(2);
} }
if(digitalRead(PIN_BUTTON_POS1)==HIGH){ // if button position 1 is HIGH
delay(200); // wait, so that reset could be done before motor is moved
goToEncoderPosition(0); // drive motor to saved encoder position 1
}
if(digitalRead(PIN_BUTTON_POS2)==HIGH){ // same for position 2
delay(200);
goToEncoderPosition(1);
}
if(digitalRead(PIN_BUTTON_POS3)==HIGH){ // same for position 3
delay(200);
goToEncoderPosition(2);
}
} // end of loop() } // end of loop()
@ -271,8 +322,7 @@ void controlUart(uint8_t port){
String input; // String for buffering read data String input; // String for buffering read data
String parameterStr; // STring for extracted parameter String parameterStr; // STring for extracted parameter
uint32_t parameter = 0; // variable for storing command 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 switch(port){ // Read from the port where the communication is coming from
case 3: case 3:
@ -335,13 +385,16 @@ void controlUart(uint8_t port){
// gtep (go to encoder position): go to saved encoder position x {0-2} // gtep (go to encoder position): go to saved encoder position x {0-2}
else if(input.startsWith("gtep")){ else if(input.startsWith("gtep")){
if(parameter<POSITION_COUNT){ if(parameter<POSITION_COUNT){
goToEncoderPosition(parameter); // Separated so that this can also be called up via the buttons and the TTL inputs.
/*
int32_t currentPosition; int32_t currentPosition;
int32_t steps2Drive = 1; int32_t steps2Drive = 1;
uint8_t i = 0; // loop counter uint8_t i = 0; // loop counter
message = "Motor is moved to saved position: "; message = "Motor is moved to saved position: ";
message += (String) parameter; message += (String) parameter;
message += ". Moved steps: "; message += ". Moved steps: ";
while(steps2Drive != 0){ // move motor to position null till the position is reached, because null is not reached exactly every time while(steps2Drive != 0){ // move motor to position till the position is reached, because it is not reached exactly every time
currentPosition = averageEncPosition(ENCODER_AV_COUNT); // read singleturn position currentPosition = averageEncPosition(ENCODER_AV_COUNT); // read singleturn position
steps2Drive = int32_t ((double)(currentPosition - positionEncoder[parameter]) / (131072.0 / (MOTORSTEPS*motorMicrosteps))); // calculate steps to move steps2Drive = int32_t ((double)(currentPosition - positionEncoder[parameter]) / (131072.0 / (MOTORSTEPS*motorMicrosteps))); // calculate steps to move
if(steps2Drive<0){ if(steps2Drive<0){
@ -362,6 +415,7 @@ void controlUart(uint8_t port){
break; break;
} }
} }
*/
} }
} }
@ -389,14 +443,14 @@ void controlUart(uint8_t port){
} }
} }
// getp (get position): get current position // getp (get position): get current motor position
else if(input.startsWith("getp")){ else if(input.startsWith("getp")){
long position = driver.getPos(); long position = driver.getPos();
message = "Current position is: "; message = "Current position is: ";
message = (String) position; message = (String) position;
} }
// setn (set null): set current position as null (home) // setn (set null): set current position as motor null position (home)
else if(input.startsWith("setn")){ else if(input.startsWith("setn")){
driver.resetPos(); driver.resetPos();
message = "Current position is set to 0"; message = "Current position is set to 0";
@ -490,23 +544,28 @@ void controlUart(uint8_t port){
} }
} }
// seep (set EEPROM encoder position): save current encoder singleturn home position to EEPROM // sepe (save encoder positions to EEPROM): backup saved encoder positions in EEPROM
else if(input.startsWith("seep")){ else if(input.startsWith("sepe")){
int32_t currentPosition; EEPROM.put(eeAddress, positionEncoder); // put data to EEPROM
currentPosition = averageEncPosition(ENCODER_AV_COUNT); message = "Encoder positions writen to EEPROM: "; // build output string
EEPROM.put(eeAddress, currentPosition); for(int i =0; i<3; i++){
message = "Saved as encoder home position in EEPROM: "; message += (String) positionEncoder[i];
message += (String) currentPosition; message += ("; ");
}
} }
// reep (reed EEPROM encoder position): reed current encoder singleturn home position from EEPROM // reep (reed EEPROM encoder positions): reed encoder singleturn positions from EEPROM
else if(input.startsWith("reep")){ else if(input.startsWith("reep")){
EEPROM.get(eeAddress, parameter); // reed address from EEPROM and save in parameter int32_t positionEncoderEeprom[3] = {0,0,0}; // for read EEPROM data
message = "Encoder home position in EEPROM is: "; EEPROM.get(eeAddress, positionEncoderEeprom); // reed encoder positions from EEPROM
message += (String) parameter; message = "Encoder positions in EEPROM: "; // build output string
for(int i =0; i<3; i++){
message += (String) positionEncoderEeprom[i];
message += ("; ");
}
} }
// init (initialize): move to encoder EEPROM home position and set it to position null // init (initialize): load encoder positions from EEPROM, move to encoder 0 position and set it to motor position null
else if(input.startsWith("init")){ else if(input.startsWith("init")){
int32_t homePosition; int32_t homePosition;
int32_t currentPosition; int32_t currentPosition;
@ -521,7 +580,9 @@ void controlUart(uint8_t port){
driver.softStop(); // hold motor driver.softStop(); // hold motor
while(driver.busyCheck()); while(driver.busyCheck());
delay(100); // wait till motor stands still delay(100); // wait till motor stands still
EEPROM.get(eeAddress, homePosition); // read home position from EEPROM //EEPROM.get(eeAddress, homePosition); // (old) read home position from EEPROM
EEPROM.get(eeAddress, positionEncoder); // restore encoderpositions from EEPROM
homePosition = positionEncoder[0]; // copy position 0 to homePosition for manipulation
while(steps2Drive != 0){ // move motor to position null till the position is reached, because null is not reached exactly every time while(steps2Drive != 0){ // move motor to position null till the position is reached, because null is not reached exactly every time
@ -533,7 +594,7 @@ void controlUart(uint8_t port){
driver.softStop(); driver.softStop();
while(driver.busyCheck()); while(driver.busyCheck());
delay(10); delay(10);
message += (String) steps2Drive; message += (String) steps2Drive; // build output string
message += ("; "); message += ("; ");
i++; i++;
if(i==10){ if(i==10){
@ -560,29 +621,6 @@ void controlUart(uint8_t port){
delay(2000); delay(2000);
message = ("hold test done"); 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 // command unknown
else{ else{
@ -629,4 +667,55 @@ int32_t averageEncPosition(uint32_t count){
} }
encoderPositionAv = encoderPositionAv / i; // build average encoderPositionAv = encoderPositionAv / i; // build average
return encoderPositionAv; return encoderPositionAv;
} // end of averageEncPosition() } // end of averageEncPosition()
// gtep (go to encoder position): go to saved encoder position x {0-2}
// Separated so that this can also be called up via the buttons and the TTL inputs.
void goToEncoderPosition(uint32_t encoderPosition){
int32_t currentPosition;
int32_t steps2Drive = 1;
uint8_t i = 0; // loop counter
uint32_t failure = 0; // set if failure happen
digitalWrite(PIN_LEMO_READY, HIGH); // Set ready HIGH (display positioning in progress)
digitalWrite(PIN_RGB_LED_B, LOW); // Turn blue LED on (display positioning in progress)
digitalWrite(PIN_RGB_LED_R, HIGH); // Turn red LED off
digitalWrite(PIN_RGB_LED_G, HIGH); // Turn green LED off
message = "Motor is moved to saved position: ";
message += (String) encoderPosition;
message += ". Moved steps: ";
while(steps2Drive != 0){ // move motor to position till the position is reached, because it is not reached exactly every time
currentPosition = averageEncPosition(ENCODER_AV_COUNT); // read singleturn position
steps2Drive = int32_t ((double)(currentPosition - positionEncoder[encoderPosition]) / (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");
failure = 1;
break;
}
}
if(failure == 0){ // without failure
digitalWrite(PIN_LEMO_READY, LOW); // Set ready LOW (display positioning done)
digitalWrite(PIN_RGB_LED_G, LOW); // Turn green LED on (display positioning done)
digitalWrite(PIN_RGB_LED_B, HIGH); // Turn blue LED off
}
else{ // with failure
//digitalWrite(PIN_LEMO_READY, LOW); // Set ready LOW (display positioning done)
digitalWrite(PIN_RGB_LED_R, LOW); // Turn red LED on (display failure)
digitalWrite(PIN_RGB_LED_B, HIGH); // Turn blue LED off
}
} // end of goToEncoderPosition()