Uses encoder for movement.
All 3 encoder positions are now backuped in EEPROM. Much more things changed
This commit is contained in:
parent
189291d597
commit
5d33516779
3
.vscode/settings.json
vendored
Normal file
3
.vscode/settings.json
vendored
Normal file
@ -0,0 +1,3 @@
|
||||
{
|
||||
"cmake.configureOnOpen": false
|
||||
}
|
||||
@ -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
|
||||
|
||||
241
src/main.cpp
241
src/main.cpp
@ -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"
|
||||
"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"
|
||||
|
||||
"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)
|
||||
// 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;
|
||||
@ -61,23 +74,40 @@ 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;
|
||||
|
||||
|
||||
// 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){
|
||||
@ -560,29 +621,6 @@ void controlUart(uint8_t port){
|
||||
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{
|
||||
@ -629,4 +667,55 @@ int32_t averageEncPosition(uint32_t count){
|
||||
}
|
||||
encoderPositionAv = encoderPositionAv / i; // build average
|
||||
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()
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user