Michael Marzano / Mbed 2 deprecated Linear_Stepper_Motor_Nema17

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

lin_step_mtr.cpp@3:2138b69ee3bd, 2020-04-20 (annotated)

Committer:
mikermarza
Date:
Mon Apr 20 03:54:54 2020 +0000
Revision:
3:2138b69ee3bd
Parent:
2:6c324fded7c1
Child:
4:1dc350268172
linear stepper motor library complete

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mikermarza 0:54c5be5f26f4 1 // Code for the lin_step_mtr driver
mikermarza 0:54c5be5f26f4 2
mikermarza 0:54c5be5f26f4 3 #include "lin_step_mtr.h"
mikermarza 0:54c5be5f26f4 4 #include "debug.h"
mikermarza 0:54c5be5f26f4 5
mikermarza 0:54c5be5f26f4 6 //Construtor
mikermarza 3:2138b69ee3bd 7 LinStepMtr::LinStepMtr(PinName A_f, PinName A_r, PinName B_f, PinName B_r, int m_rpm=MAX_RPM)
mikermarza 3:2138b69ee3bd 8 :mtr_ctrl(B_r, A_r, B_f,A_f), max_speed((m_rpm > MAX_RPM) ? floor((float)MAX_RPM * 10/3):floor((float)m_rpm * 10/3)),
mikermarza 3:2138b69ee3bd 9 max_rpm(m_rpm)
mikermarza 0:54c5be5f26f4 10 {
mikermarza 0:54c5be5f26f4 11 mtr_ctrl = 0x0;
mikermarza 3:2138b69ee3bd 12 speed = floor((float)DEFAULT_RPM * 10 / 3);
mikermarza 0:54c5be5f26f4 13 dir = CW;
mikermarza 1:757a52db1604 14 cur_step = ONE;
mikermarza 1:757a52db1604 15
mikermarza 1:757a52db1604 16 stop_mtr = true;
mikermarza 1:757a52db1604 17 terminate = false;
mikermarza 1:757a52db1604 18 rotate_th = NULL;
mikermarza 1:757a52db1604 19
mikermarza 1:757a52db1604 20 cur_state = STOP_MOTOR;
mikermarza 1:757a52db1604 21 }
mikermarza 1:757a52db1604 22
mikermarza 1:757a52db1604 23 LinStepMtr::LinStepMtr(PinName A_f, PinName A_r, PinName B_f, PinName B_r)
mikermarza 3:2138b69ee3bd 24 :mtr_ctrl(B_r,A_r,B_f,A_f), max_speed(floor((float)MAX_RPM * 10/3)), max_rpm(MAX_RPM)
mikermarza 1:757a52db1604 25 {
mikermarza 1:757a52db1604 26 mtr_ctrl = 0x0;
mikermarza 3:2138b69ee3bd 27 speed = floor((double)DEFAULT_RPM * 10 / 3);
mikermarza 1:757a52db1604 28 dir = CW;
mikermarza 1:757a52db1604 29 cur_step = ONE;
mikermarza 0:54c5be5f26f4 30
mikermarza 0:54c5be5f26f4 31 stop_mtr = true;
mikermarza 0:54c5be5f26f4 32 terminate = false;
mikermarza 0:54c5be5f26f4 33
mikermarza 0:54c5be5f26f4 34 cur_state = STOP_MOTOR;
mikermarza 0:54c5be5f26f4 35 }
mikermarza 0:54c5be5f26f4 36
mikermarza 1:757a52db1604 37 LinStepMtr::~LinStepMtr()
mikermarza 1:757a52db1604 38 {
mikermarza 1:757a52db1604 39 this->end();
mikermarza 1:757a52db1604 40 }
mikermarza 1:757a52db1604 41
mikermarza 0:54c5be5f26f4 42
mikermarza 0:54c5be5f26f4 43 float LinStepMtr::get_speed()
mikermarza 0:54c5be5f26f4 44 {
mikermarza 0:54c5be5f26f4 45 return (float) speed * 3 / 10;
mikermarza 0:54c5be5f26f4 46 }
mikermarza 0:54c5be5f26f4 47
mikermarza 0:54c5be5f26f4 48 LinStepMtr::Direction LinStepMtr::get_dir()
mikermarza 0:54c5be5f26f4 49 {
mikermarza 0:54c5be5f26f4 50 return dir;
mikermarza 0:54c5be5f26f4 51 }
mikermarza 0:54c5be5f26f4 52
mikermarza 1:757a52db1604 53 void LinStepMtr::init(double rpm, Direction d)
mikermarza 1:757a52db1604 54 {
mikermarza 1:757a52db1604 55 terminate = false;
mikermarza 1:757a52db1604 56 speed = floor(rpm * 10 / 3);
mikermarza 1:757a52db1604 57 dir = d;
mikermarza 1:757a52db1604 58 if (!rotate_th) {
mikermarza 1:757a52db1604 59 rotate_th = new Thread(LinStepMtr::rotate_help, this);
mikermarza 1:757a52db1604 60 }
mikermarza 1:757a52db1604 61 }
mikermarza 1:757a52db1604 62
mikermarza 1:757a52db1604 63 void LinStepMtr::end() {
mikermarza 1:757a52db1604 64 terminate = true;
mikermarza 1:757a52db1604 65 Thread::wait(100);
mikermarza 1:757a52db1604 66 if (rotate_th) {
mikermarza 1:757a52db1604 67 delete rotate_th;
mikermarza 1:757a52db1604 68 rotate_th = NULL;
mikermarza 1:757a52db1604 69 }
mikermarza 1:757a52db1604 70 }
mikermarza 1:757a52db1604 71
mikermarza 1:757a52db1604 72 void LinStepMtr::start() {
mikermarza 3:2138b69ee3bd 73 stop_mtr = false;
mikermarza 3:2138b69ee3bd 74 spin_up();
mikermarza 1:757a52db1604 75 }
mikermarza 1:757a52db1604 76
mikermarza 1:757a52db1604 77 void LinStepMtr::stop() {
mikermarza 3:2138b69ee3bd 78 int s = spin_down();
mikermarza 2:6c324fded7c1 79 stop_mtr = true;
mikermarza 3:2138b69ee3bd 80 speed = s;
mikermarza 1:757a52db1604 81 }
mikermarza 1:757a52db1604 82
mikermarza 1:757a52db1604 83 void LinStepMtr::rotate_help(void const *args)
mikermarza 1:757a52db1604 84 {
mikermarza 1:757a52db1604 85 LinStepMtr *instPtr = static_cast<LinStepMtr *>(const_cast<void *>(args));
mikermarza 1:757a52db1604 86
mikermarza 1:757a52db1604 87 instPtr->rotate();
mikermarza 1:757a52db1604 88 }
mikermarza 1:757a52db1604 89
mikermarza 3:2138b69ee3bd 90 void LinStepMtr::change_dir(Direction d)
mikermarza 3:2138b69ee3bd 91 {
mikermarza 3:2138b69ee3bd 92 if(dir != d) {
mikermarza 3:2138b69ee3bd 93 if(stop_mtr) {
mikermarza 3:2138b69ee3bd 94 dir = d;
mikermarza 3:2138b69ee3bd 95 } else {
mikermarza 3:2138b69ee3bd 96 int s = spin_down();
mikermarza 3:2138b69ee3bd 97 stop_mtr = true;
mikermarza 3:2138b69ee3bd 98 dir = d;
mikermarza 3:2138b69ee3bd 99 stop_mtr = false;
mikermarza 3:2138b69ee3bd 100 spin_up(s);
mikermarza 3:2138b69ee3bd 101 }
mikermarza 3:2138b69ee3bd 102 }
mikermarza 3:2138b69ee3bd 103 }
mikermarza 3:2138b69ee3bd 104
mikermarza 3:2138b69ee3bd 105 void LinStepMtr::change_speed(float rpm)
mikermarza 3:2138b69ee3bd 106 {
mikermarza 3:2138b69ee3bd 107 speed = floor(rpm * 10 / 3);
mikermarza 3:2138b69ee3bd 108 }
mikermarza 3:2138b69ee3bd 109
mikermarza 0:54c5be5f26f4 110 void LinStepMtr::rotate()
mikermarza 0:54c5be5f26f4 111 {
mikermarza 1:757a52db1604 112 pc.printf("Called rotate()\n");
mikermarza 1:757a52db1604 113 /*
mikermarza 1:757a52db1604 114 while(1) {
mikermarza 1:757a52db1604 115
mikermarza 1:757a52db1604 116 mtr_ctrl = ++cur_step;
mikermarza 1:757a52db1604 117 rev_cnt +=.005;
mikermarza 1:757a52db1604 118 wait(pause);
mikermarza 1:757a52db1604 119 }
mikermarza 1:757a52db1604 120 */
mikermarza 1:757a52db1604 121
mikermarza 0:54c5be5f26f4 122 while(!terminate) {
mikermarza 0:54c5be5f26f4 123 if(!stop_mtr){
mikermarza 0:54c5be5f26f4 124 switch(dir) {
mikermarza 0:54c5be5f26f4 125 case CW:
mikermarza 0:54c5be5f26f4 126 mtr_ctrl = ++cur_step;
mikermarza 0:54c5be5f26f4 127 rev_cnt +=.005;
mikermarza 0:54c5be5f26f4 128 break;
mikermarza 0:54c5be5f26f4 129 case CCW:
mikermarza 0:54c5be5f26f4 130 mtr_ctrl = --cur_step;
mikermarza 0:54c5be5f26f4 131 rev_cnt-=.005;
mikermarza 0:54c5be5f26f4 132 break;
mikermarza 0:54c5be5f26f4 133 }
mikermarza 1:757a52db1604 134 wait(1/ (float) speed);
mikermarza 0:54c5be5f26f4 135 } else {
mikermarza 0:54c5be5f26f4 136 mtr_ctrl = STOP;
mikermarza 0:54c5be5f26f4 137 Thread::yield();
mikermarza 0:54c5be5f26f4 138 }
mikermarza 0:54c5be5f26f4 139 }
mikermarza 1:757a52db1604 140
mikermarza 0:54c5be5f26f4 141 }
mikermarza 0:54c5be5f26f4 142
mikermarza 3:2138b69ee3bd 143 #define SPIN_INCR 83 // steps
mikermarza 3:2138b69ee3bd 144 #define SPIN_WAIT 10 // ms
mikermarza 3:2138b69ee3bd 145 void LinStepMtr::spin_up(float rpm) {
mikermarza 3:2138b69ee3bd 146 int end_speed;
mikermarza 3:2138b69ee3bd 147
mikermarza 3:2138b69ee3bd 148 if(rpm == -1) {
mikermarza 3:2138b69ee3bd 149 end_speed = speed;
mikermarza 3:2138b69ee3bd 150 } else {
mikermarza 3:2138b69ee3bd 151 end_speed = (rpm > max_rpm) ? max_speed:floor(rpm * 10 / 3);
mikermarza 3:2138b69ee3bd 152 }
mikermarza 3:2138b69ee3bd 153 for(int i = min_speed; i < end_speed-SPIN_INCR; i+=SPIN_INCR){
mikermarza 3:2138b69ee3bd 154 speed = i;
mikermarza 3:2138b69ee3bd 155 Thread::wait(SPIN_WAIT);
mikermarza 3:2138b69ee3bd 156 }
mikermarza 3:2138b69ee3bd 157 speed = end_speed;
mikermarza 3:2138b69ee3bd 158
mikermarza 3:2138b69ee3bd 159 }
mikermarza 3:2138b69ee3bd 160
mikermarza 3:2138b69ee3bd 161 int LinStepMtr::spin_down(float rpm) {
mikermarza 3:2138b69ee3bd 162 int end_speed;
mikermarza 3:2138b69ee3bd 163 int s = speed;
mikermarza 3:2138b69ee3bd 164
mikermarza 3:2138b69ee3bd 165 if(rpm == -1) {
mikermarza 3:2138b69ee3bd 166 end_speed = min_speed;
mikermarza 3:2138b69ee3bd 167 } else {
mikermarza 3:2138b69ee3bd 168 end_speed = (rpm < min_rpm) ? min_speed:floor(rpm * 10 / 3);
mikermarza 3:2138b69ee3bd 169 }
mikermarza 3:2138b69ee3bd 170
mikermarza 3:2138b69ee3bd 171 for(int i = speed; i > end_speed+SPIN_INCR; i-=SPIN_INCR){
mikermarza 3:2138b69ee3bd 172 speed = i;
mikermarza 3:2138b69ee3bd 173 Thread::wait(SPIN_WAIT);
mikermarza 3:2138b69ee3bd 174 }
mikermarza 3:2138b69ee3bd 175 speed = end_speed;
mikermarza 3:2138b69ee3bd 176 return s;
mikermarza 3:2138b69ee3bd 177 }
mikermarza 3:2138b69ee3bd 178
mikermarza 0:54c5be5f26f4 179 // Private Step Class functions
mikermarza 0:54c5be5f26f4 180 /*
mikermarza 0:54c5be5f26f4 181 LinStepMtr::Step::Step() : cur_step(ONE)
mikermarza 0:54c5be5f26f4 182 {
mikermarza 0:54c5be5f26f4 183 pc.printf("\n\nCalled Constructor\n cur_step: %x\n",cur_step);
mikermarza 0:54c5be5f26f4 184
mikermarza 0:54c5be5f26f4 185 pc.printf("Step_Num val:\n ONE = %x\n TWO = %x\n THREE = %x\n FOUR = %x\n\n", ONE, TWO, THREE, FOUR);
mikermarza 0:54c5be5f26f4 186 };
mikermarza 0:54c5be5f26f4 187 */
mikermarza 0:54c5be5f26f4 188 LinStepMtr::Step_Num LinStepMtr::Step::get_cur_step()
mikermarza 0:54c5be5f26f4 189 {
mikermarza 0:54c5be5f26f4 190 return cur_step;
mikermarza 0:54c5be5f26f4 191 }
mikermarza 0:54c5be5f26f4 192
mikermarza 0:54c5be5f26f4 193 LinStepMtr::Step_Num LinStepMtr::Step::operator++()
mikermarza 0:54c5be5f26f4 194 {
mikermarza 0:54c5be5f26f4 195 switch(cur_step){
mikermarza 0:54c5be5f26f4 196 case ONE:
mikermarza 0:54c5be5f26f4 197 cur_step=TWO;
mikermarza 0:54c5be5f26f4 198 break;
mikermarza 0:54c5be5f26f4 199 case TWO:
mikermarza 0:54c5be5f26f4 200 cur_step = THREE;
mikermarza 0:54c5be5f26f4 201 break;
mikermarza 0:54c5be5f26f4 202 case THREE:
mikermarza 0:54c5be5f26f4 203 cur_step = FOUR;
mikermarza 0:54c5be5f26f4 204 break;
mikermarza 0:54c5be5f26f4 205 case FOUR:
mikermarza 0:54c5be5f26f4 206 cur_step = ONE;
mikermarza 0:54c5be5f26f4 207 break;
mikermarza 0:54c5be5f26f4 208 }
mikermarza 1:757a52db1604 209 //pc.printf(" CUR_STEP = %x\n",cur_step);
mikermarza 0:54c5be5f26f4 210 return cur_step;
mikermarza 0:54c5be5f26f4 211 }
mikermarza 0:54c5be5f26f4 212
mikermarza 0:54c5be5f26f4 213 LinStepMtr::Step_Num LinStepMtr::Step::operator--()
mikermarza 0:54c5be5f26f4 214 {
mikermarza 0:54c5be5f26f4 215 switch(cur_step){
mikermarza 0:54c5be5f26f4 216 case ONE:
mikermarza 0:54c5be5f26f4 217 cur_step=FOUR;
mikermarza 0:54c5be5f26f4 218 break;
mikermarza 0:54c5be5f26f4 219 case TWO:
mikermarza 0:54c5be5f26f4 220 cur_step = ONE;
mikermarza 0:54c5be5f26f4 221 break;
mikermarza 0:54c5be5f26f4 222 case THREE:
mikermarza 0:54c5be5f26f4 223 cur_step = TWO;
mikermarza 0:54c5be5f26f4 224 break;
mikermarza 0:54c5be5f26f4 225 case FOUR:
mikermarza 0:54c5be5f26f4 226 cur_step = THREE;
mikermarza 0:54c5be5f26f4 227 break;
mikermarza 0:54c5be5f26f4 228 }
mikermarza 0:54c5be5f26f4 229 return cur_step;
mikermarza 1:757a52db1604 230 }
mikermarza 1:757a52db1604 231
mikermarza 1:757a52db1604 232 void LinStepMtr::Step::operator=(LinStepMtr::Step_Num s)
mikermarza 1:757a52db1604 233 {
mikermarza 1:757a52db1604 234 cur_step = s;
mikermarza 0:54c5be5f26f4 235 }