EW
/
M034_LEPP-neutron-switch
2
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Uses encoder for movement. 

All 3 encoder positions are now backuped in EEPROM.
Much more things changed
master
Steffen Brucker 7 months ago
parent
189291d597
commit
5d33516779
3 changed files with 186 additions and 78 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 3
      .vscode/settings.json
  2. 22
      include/Pin_Definitions.h
  3. 231
      src/main.cpp

3
.vscode/settings.json
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_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
int const PIN_POS1 = A3; // CN9:7, PF3
int const PIN_POS2 = A4; // CN9:9, PF5
int const PIN_POS3 = A5; // CN9:11, PF10
int const PIN_LEMO_POS1 = D70; // CN9:17, PF2
int const PIN_LEMO_POS2 = 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
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
// 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

231
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"
"commands (in small letters):\n"
"help: print instruction manual\n"
"\n"
"Basic Motor Instructions (without encoder):\n"
"movl (move left): spin x steps counterclockwise\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"
"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"
"setn (set null): set current position as motor null position (home)\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"
"gtmp (go to motor position): go to saved motor position x {0-2}\n"
"\n"
"Motor configuration:\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"
"\n"
"Encoder Instructions:\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"
"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): move to encoder EEPROM home position and set it to position null\n"
"\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)
// const
// small neutron-switch
// 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;
@ -62,22 +75,39 @@ const byte VM_DEC_TVAL = 24;
const byte VM_HOLD_TVAL = 28;
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 *
**********************************************************************/
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
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 *
**********************************************************************/
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_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
pinMode(PIN_BUTTON_POS1, INPUT_FLOATING);// Push button for position controll
pinMode(PIN_BUTTON_POS2, INPUT_FLOATING);// Push button 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
Serial6.begin(SERIAL_BAUD_RATE); // Start serial UART on optical links
@ -210,24 +246,27 @@ void setup() {
// end of stepper motor shield initialisation
// init
EEPROM.get(eeAddress, positionEncoder); // restore encoderpositions from EEPROM
// 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
digitalWrite(PIN_RGB_LED_R, HIGH); // set all off (common anode)
digitalWrite(PIN_RGB_LED_G, HIGH);
digitalWrite(PIN_RGB_LED_B, HIGH);
digitalWrite(PIN_RGB_LED_R, LOW);// red on
delay(300);
digitalWrite(PIN_RGB_LED_R, LOW); // red off
digitalWrite(PIN_RGB_LED_G, HIGH);// blue on
digitalWrite(PIN_RGB_LED_R, HIGH); // red off
digitalWrite(PIN_RGB_LED_G, LOW);// blue on
delay(300);
digitalWrite(PIN_RGB_LED_G, LOW); // blue off
digitalWrite(PIN_RGB_LED_B, HIGH);// green on
digitalWrite(PIN_RGB_LED_G, HIGH); // blue off
digitalWrite(PIN_RGB_LED_B, LOW);// green on
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
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()
@ -244,16 +283,28 @@ void loop() {
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_LEMO_POS1)==HIGH){ // if TTL position 1 input is HIGH
goToEncoderPosition(0); // drive motor to saved encoder position 1
}
if(digitalRead(PIN_POS2)==HIGH){ // same for position 2
driver.goTo(positionMotor[1]);
if(digitalRead(PIN_LEMO_POS2)==HIGH){ // same for position 2
goToEncoderPosition(1);
}
if(digitalRead(PIN_POS3)==HIGH){ // and position 3
driver.goTo(positionMotor[2]);
if(digitalRead(PIN_LEMO_POS3)==HIGH){ // and position 3
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()
@ -271,8 +322,7 @@ 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:
@ -335,13 +385,16 @@ void controlUart(uint8_t port){
// gtep (go to encoder position): go to saved encoder position x {0-2}
else if(input.startsWith("gtep")){
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 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
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[parameter]) / (131072.0 / (MOTORSTEPS*motorMicrosteps))); // calculate steps to move
if(steps2Drive<0){
@ -362,6 +415,7 @@ void controlUart(uint8_t port){
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")){
long position = driver.getPos();
message = "Current position is: ";
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")){
driver.resetPos();
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
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;
// sepe (save encoder positions to EEPROM): backup saved encoder positions in EEPROM
else if(input.startsWith("sepe")){
EEPROM.put(eeAddress, positionEncoder); // put data to EEPROM
message = "Encoder positions writen to EEPROM: "; // build output string
for(int i =0; i<3; i++){
message += (String) positionEncoder[i];
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")){
EEPROM.get(eeAddress, parameter); // reed address from EEPROM and save in parameter
message = "Encoder home position in EEPROM is: ";
message += (String) parameter;
int32_t positionEncoderEeprom[3] = {0,0,0}; // for read EEPROM data
EEPROM.get(eeAddress, positionEncoderEeprom); // reed encoder positions from EEPROM
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")){
int32_t homePosition;
int32_t currentPosition;
@ -521,7 +580,9 @@ void controlUart(uint8_t port){
driver.softStop(); // hold motor
while(driver.busyCheck());
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
@ -533,7 +594,7 @@ void controlUart(uint8_t port){
driver.softStop();
while(driver.busyCheck());
delay(10);
message += (String) steps2Drive;
message += (String) steps2Drive; // build output string
message += ("; ");
i++;
if(i==10){
@ -561,29 +622,6 @@ void controlUart(uint8_t port){
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";
@ -630,3 +668,54 @@ int32_t averageEncPosition(uint32_t count){
encoderPositionAv = encoderPositionAv / i; // build average
return encoderPositionAv;
} // 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()
Write Preview
Loading…
Cancel
Save