@@ -911,128 +911,123 @@ void FastAccelStepper::backwardStep(bool blocking) {
911
911
int32_t FastAccelStepper::getCurrentPosition () {
912
912
return fas_queue[_queue_num].getCurrentPosition ();
913
913
}
914
- int8_t FastAccelStepper::moveTimed (int16_t steps, uint32_t duration, uint32_t &actual_duration) {
914
+ int8_t FastAccelStepper::moveTimed (int16_t steps, uint32_t duration,
915
+ uint32_t & actual_duration) {
915
916
// For now only a very crude implementation
916
917
uint8_t freeEntries = QUEUE_LEN - queueEntries ();
917
918
actual_duration = 0 ;
918
- struct stepper_command_s cmd = {
919
- .ticks = 0 , .steps = 0 , .count_up = true };
919
+ struct stepper_command_s cmd = {.ticks = 0 , .steps = 0 , .count_up = true };
920
920
if (steps == 0 ) {
921
- if ((duration >> 16 ) >= QUEUE_LEN) {
922
- return MOVE_TIMED_TOO_LARGE_ERROR;
923
- }
924
- if ((duration >> 16 ) >= freeEntries) {
925
- return MOVE_TIMED_BUSY;
926
- }
927
- // Should fit
928
- while (duration > 0 ) {
929
- if (duration <= 65535 ) {
930
- // done using one command
931
- cmd.ticks = duration;
932
- }
933
- else if (duration >= 131072 ) {
934
- // need more than one command
935
- cmd.ticks = 65535 ;
936
- }
937
- else {
938
- // just use half of the duration now, and the other half in the next cmd.
939
- cmd.ticks = duration >> 1 ;
940
- }
941
- uint8_t ret = addQueueEntry (&cmd);
942
- if (ret != 0 ) {
943
- // unexpected
944
- return ret;
945
- }
946
- actual_duration += cmd.ticks ;
947
- duration -= cmd.ticks ;
948
- }
949
- return MOVE_TIMED_OK;
921
+ if ((duration >> 16 ) >= QUEUE_LEN) {
922
+ return MOVE_TIMED_TOO_LARGE_ERROR;
923
+ }
924
+ if ((duration >> 16 ) >= freeEntries) {
925
+ return MOVE_TIMED_BUSY;
926
+ }
927
+ // Should fit
928
+ while (duration > 0 ) {
929
+ if (duration <= 65535 ) {
930
+ // done using one command
931
+ cmd.ticks = duration;
932
+ } else if (duration >= 131072 ) {
933
+ // need more than one command
934
+ cmd.ticks = 65535 ;
935
+ } else {
936
+ // just use half of the duration now, and the other half in the next
937
+ // cmd.
938
+ cmd.ticks = duration >> 1 ;
939
+ }
940
+ uint8_t ret = addQueueEntry (&cmd);
941
+ if (ret != 0 ) {
942
+ // unexpected
943
+ return ret;
944
+ }
945
+ actual_duration += cmd.ticks ;
946
+ duration -= cmd.ticks ;
947
+ }
948
+ return MOVE_TIMED_OK;
950
949
}
951
950
952
951
// let's evaluate the direction
953
952
if (steps < 0 ) {
954
- cmd.count_up = false ;
955
- steps = -steps;
953
+ cmd.count_up = false ;
954
+ steps = -steps;
956
955
}
957
956
958
957
// There are steps to execute
959
958
// Let's first calculate the step rate
960
959
uint32_t rate = duration;
961
960
rate /= steps;
962
961
if (rate > 65535 ) {
963
- // we need pauses, so only few steps can be executed
964
- uint16_t cmds_per_step = rate >> 16 ; // bit too small
965
- if (cmds_per_step >= QUEUE_LEN) {
966
- return MOVE_TIMED_TOO_LARGE_ERROR;
967
- }
968
- if (steps >= QUEUE_LEN) {
969
- return MOVE_TIMED_TOO_LARGE_ERROR;
970
- }
971
- uint8_t cmds = steps * cmds_per_step;
972
- if (cmds >= QUEUE_LEN) {
973
- return MOVE_TIMED_TOO_LARGE_ERROR;
974
- }
975
- if (cmds > freeEntries) {
976
- return MOVE_TIMED_BUSY;
977
- }
978
- // Should fit into the queue.
979
- for (uint8_t s = 0 ;s < steps;s++) {
980
- uint32_t this_duration = rate;
981
- cmd.steps = 1 ;
982
- while (this_duration) {
983
- if (this_duration > 131072 ) {
984
- cmd.ticks = 65535 ;
985
- }
986
- else if (this_duration > 65535 ) {
987
- cmd.ticks = this_duration/2 ;
988
- }
989
- else {
990
- cmd.ticks = this_duration;
991
- }
992
- this_duration -= cmd.ticks ;
993
-
994
- uint8_t ret = addQueueEntry (&cmd);
995
- if (ret != 0 ) {
996
- // unexpected
997
- return ret;
998
- }
999
- actual_duration += cmd.ticks ;
1000
- // remaining are pauses
1001
- cmd.steps = 0 ;
1002
- }
1003
- }
1004
- return MOVE_TIMED_OK;
1005
- }
1006
- // Now we need to run steps at "high" speed.
1007
- if (steps > QUEUE_LEN * 255 ) {
962
+ // we need pauses, so only few steps can be executed
963
+ uint16_t cmds_per_step = rate >> 16 ; // bit too small
964
+ if (cmds_per_step >= QUEUE_LEN) {
965
+ return MOVE_TIMED_TOO_LARGE_ERROR;
966
+ }
967
+ if (steps >= QUEUE_LEN) {
1008
968
return MOVE_TIMED_TOO_LARGE_ERROR;
1009
969
}
1010
- if (steps > freeEntries * 255 ) {
970
+ uint8_t cmds = steps * cmds_per_step;
971
+ if (cmds >= QUEUE_LEN) {
972
+ return MOVE_TIMED_TOO_LARGE_ERROR;
973
+ }
974
+ if (cmds > freeEntries) {
1011
975
return MOVE_TIMED_BUSY;
1012
976
}
1013
- // The steps should fit in
1014
- while (steps > 0 ) {
1015
- cmd.ticks = rate;
1016
- if (steps > 510 ) {
1017
- cmd.steps = 255 ;
1018
- }
1019
- else if (steps > 255 ) {
1020
- cmd.steps = steps/2 ;
1021
- }
1022
- else {
1023
- cmd.steps = steps;
1024
- }
1025
- uint8_t ret = addQueueEntry (&cmd);
1026
- if (ret != 0 ) {
1027
- // unexpected
1028
- return ret;
977
+ // Should fit into the queue.
978
+ for (uint8_t s = 0 ; s < steps; s++) {
979
+ uint32_t this_duration = rate;
980
+ cmd.steps = 1 ;
981
+ while (this_duration) {
982
+ if (this_duration > 131072 ) {
983
+ cmd.ticks = 65535 ;
984
+ } else if (this_duration > 65535 ) {
985
+ cmd.ticks = this_duration / 2 ;
986
+ } else {
987
+ cmd.ticks = this_duration;
988
+ }
989
+ this_duration -= cmd.ticks ;
990
+
991
+ uint8_t ret = addQueueEntry (&cmd);
992
+ if (ret != 0 ) {
993
+ // unexpected
994
+ return ret;
995
+ }
996
+ actual_duration += cmd.ticks ;
997
+ // remaining are pauses
998
+ cmd.steps = 0 ;
1029
999
}
1030
- uint32_t cmd_duration = cmd.ticks ;
1031
- cmd_duration *= cmd.steps ;
1032
- actual_duration += cmd_duration;
1033
- steps -= cmd.steps ;
1034
1000
}
1035
1001
return MOVE_TIMED_OK;
1002
+ }
1003
+ // Now we need to run steps at "high" speed.
1004
+ if (steps > QUEUE_LEN * 255 ) {
1005
+ return MOVE_TIMED_TOO_LARGE_ERROR;
1006
+ }
1007
+ if (steps > freeEntries * 255 ) {
1008
+ return MOVE_TIMED_BUSY;
1009
+ }
1010
+ // The steps should fit in
1011
+ while (steps > 0 ) {
1012
+ cmd.ticks = rate;
1013
+ if (steps > 510 ) {
1014
+ cmd.steps = 255 ;
1015
+ } else if (steps > 255 ) {
1016
+ cmd.steps = steps / 2 ;
1017
+ } else {
1018
+ cmd.steps = steps;
1019
+ }
1020
+ uint8_t ret = addQueueEntry (&cmd);
1021
+ if (ret != 0 ) {
1022
+ // unexpected
1023
+ return ret;
1024
+ }
1025
+ uint32_t cmd_duration = cmd.ticks ;
1026
+ cmd_duration *= cmd.steps ;
1027
+ actual_duration += cmd_duration;
1028
+ steps -= cmd.steps ;
1029
+ }
1030
+ return MOVE_TIMED_OK;
1036
1031
}
1037
1032
void FastAccelStepper::detachFromPin () { fas_queue[_queue_num].disconnect (); }
1038
1033
void FastAccelStepper::reAttachToPin () { fas_queue[_queue_num].connect (); }
0 commit comments