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@5:955bbc08ee78, 2020-04-27 (annotated)

Committer:
mikermarza
Date:
Mon Apr 27 01:01:59 2020 +0000
Revision:
5:955bbc08ee78
Parent:
4:1dc350268172
Child:
6:0c5418e05145
Added code for starting in a given direction

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 5:955bbc08ee78 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 5:955bbc08ee78 77 void LinStepMtr::start(Direction d) {
mikermarza 5:955bbc08ee78 78 if(dir != d){
mikermarza 5:955bbc08ee78 79 dir = d;
mikermarza 5:955bbc08ee78 80 }
mikermarza 5:955bbc08ee78 81 stop_mtr = false;
mikermarza 5:955bbc08ee78 82 spin_up();
mikermarza 5:955bbc08ee78 83 }
mikermarza 5:955bbc08ee78 84
mikermarza 1:757a52db1604 85 void LinStepMtr::stop() {
mikermarza 3:2138b69ee3bd 86 int s = spin_down();
mikermarza 2:6c324fded7c1 87 stop_mtr = true;
mikermarza 3:2138b69ee3bd 88 speed = s;
mikermarza 1:757a52db1604 89 }
mikermarza 1:757a52db1604 90
mikermarza 1:757a52db1604 91 void LinStepMtr::rotate_help(void const *args)
mikermarza 1:757a52db1604 92 {
mikermarza 1:757a52db1604 93 LinStepMtr *instPtr = static_cast<LinStepMtr *>(const_cast<void *>(args));
mikermarza 1:757a52db1604 94
mikermarza 1:757a52db1604 95 instPtr->rotate();
mikermarza 1:757a52db1604 96 }
mikermarza 1:757a52db1604 97
mikermarza 3:2138b69ee3bd 98 void LinStepMtr::change_dir(Direction d)
mikermarza 3:2138b69ee3bd 99 {
mikermarza 3:2138b69ee3bd 100 if(dir != d) {
mikermarza 3:2138b69ee3bd 101 if(stop_mtr) {
mikermarza 3:2138b69ee3bd 102 dir = d;
mikermarza 3:2138b69ee3bd 103 } else {
mikermarza 3:2138b69ee3bd 104 int s = spin_down();
mikermarza 3:2138b69ee3bd 105 stop_mtr = true;
mikermarza 3:2138b69ee3bd 106 dir = d;
mikermarza 3:2138b69ee3bd 107 stop_mtr = false;
mikermarza 3:2138b69ee3bd 108 spin_up(s);
mikermarza 3:2138b69ee3bd 109 }
mikermarza 3:2138b69ee3bd 110 }
mikermarza 3:2138b69ee3bd 111 }
mikermarza 3:2138b69ee3bd 112
mikermarza 3:2138b69ee3bd 113 void LinStepMtr::change_speed(float rpm)
mikermarza 3:2138b69ee3bd 114 {
mikermarza 3:2138b69ee3bd 115 speed = floor(rpm * 10 / 3);
mikermarza 3:2138b69ee3bd 116 }
mikermarza 3:2138b69ee3bd 117
mikermarza 0:54c5be5f26f4 118 void LinStepMtr::rotate()
mikermarza 0:54c5be5f26f4 119 {
mikermarza 5:955bbc08ee78 120 pc.printf("Called rotate()\n");
mikermarza 1:757a52db1604 121 /*
mikermarza 1:757a52db1604 122 while(1) {
mikermarza 1:757a52db1604 123
mikermarza 1:757a52db1604 124 mtr_ctrl = ++cur_step;
mikermarza 1:757a52db1604 125 rev_cnt +=.005;
mikermarza 1:757a52db1604 126 wait(pause);
mikermarza 1:757a52db1604 127 }
mikermarza 1:757a52db1604 128 */
mikermarza 1:757a52db1604 129
mikermarza 0:54c5be5f26f4 130 while(!terminate) {
mikermarza 0:54c5be5f26f4 131 if(!stop_mtr){
mikermarza 0:54c5be5f26f4 132 switch(dir) {
mikermarza 0:54c5be5f26f4 133 case CW:
mikermarza 0:54c5be5f26f4 134 mtr_ctrl = ++cur_step;
mikermarza 0:54c5be5f26f4 135 rev_cnt +=.005;
mikermarza 0:54c5be5f26f4 136 break;
mikermarza 0:54c5be5f26f4 137 case CCW:
mikermarza 0:54c5be5f26f4 138 mtr_ctrl = --cur_step;
mikermarza 0:54c5be5f26f4 139 rev_cnt-=.005;
mikermarza 0:54c5be5f26f4 140 break;
mikermarza 0:54c5be5f26f4 141 }
mikermarza 1:757a52db1604 142 wait(1/ (float) speed);
mikermarza 0:54c5be5f26f4 143 } else {
mikermarza 0:54c5be5f26f4 144 mtr_ctrl = STOP;
mikermarza 0:54c5be5f26f4 145 Thread::yield();
mikermarza 0:54c5be5f26f4 146 }
mikermarza 0:54c5be5f26f4 147 }
mikermarza 1:757a52db1604 148
mikermarza 0:54c5be5f26f4 149 }
mikermarza 0:54c5be5f26f4 150
mikermarza 3:2138b69ee3bd 151 #define SPIN_INCR 83 // steps
mikermarza 3:2138b69ee3bd 152 #define SPIN_WAIT 10 // ms
mikermarza 3:2138b69ee3bd 153 void LinStepMtr::spin_up(float rpm) {
mikermarza 3:2138b69ee3bd 154 int end_speed;
mikermarza 3:2138b69ee3bd 155
mikermarza 3:2138b69ee3bd 156 if(rpm == -1) {
mikermarza 3:2138b69ee3bd 157 end_speed = speed;
mikermarza 3:2138b69ee3bd 158 } else {
mikermarza 3:2138b69ee3bd 159 end_speed = (rpm > max_rpm) ? max_speed:floor(rpm * 10 / 3);
mikermarza 3:2138b69ee3bd 160 }
mikermarza 3:2138b69ee3bd 161 for(int i = min_speed; i < end_speed-SPIN_INCR; i+=SPIN_INCR){
mikermarza 3:2138b69ee3bd 162 speed = i;
mikermarza 3:2138b69ee3bd 163 Thread::wait(SPIN_WAIT);
mikermarza 3:2138b69ee3bd 164 }
mikermarza 3:2138b69ee3bd 165 speed = end_speed;
mikermarza 3:2138b69ee3bd 166
mikermarza 3:2138b69ee3bd 167 }
mikermarza 3:2138b69ee3bd 168
mikermarza 3:2138b69ee3bd 169 int LinStepMtr::spin_down(float rpm) {
mikermarza 3:2138b69ee3bd 170 int end_speed;
mikermarza 3:2138b69ee3bd 171 int s = speed;
mikermarza 3:2138b69ee3bd 172
mikermarza 3:2138b69ee3bd 173 if(rpm == -1) {
mikermarza 3:2138b69ee3bd 174 end_speed = min_speed;
mikermarza 3:2138b69ee3bd 175 } else {
mikermarza 3:2138b69ee3bd 176 end_speed = (rpm < min_rpm) ? min_speed:floor(rpm * 10 / 3);
mikermarza 3:2138b69ee3bd 177 }
mikermarza 3:2138b69ee3bd 178
mikermarza 3:2138b69ee3bd 179 for(int i = speed; i > end_speed+SPIN_INCR; i-=SPIN_INCR){
mikermarza 3:2138b69ee3bd 180 speed = i;
mikermarza 3:2138b69ee3bd 181 Thread::wait(SPIN_WAIT);
mikermarza 3:2138b69ee3bd 182 }
mikermarza 3:2138b69ee3bd 183 speed = end_speed;
mikermarza 3:2138b69ee3bd 184 return s;
mikermarza 3:2138b69ee3bd 185 }
mikermarza 3:2138b69ee3bd 186
mikermarza 0:54c5be5f26f4 187 // Private Step Class functions
mikermarza 0:54c5be5f26f4 188 /*
mikermarza 0:54c5be5f26f4 189 LinStepMtr::Step::Step() : cur_step(ONE)
mikermarza 0:54c5be5f26f4 190 {
mikermarza 0:54c5be5f26f4 191 pc.printf("\n\nCalled Constructor\n cur_step: %x\n",cur_step);
mikermarza 0:54c5be5f26f4 192
mikermarza 0:54c5be5f26f4 193 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 194 };
mikermarza 0:54c5be5f26f4 195 */
mikermarza 0:54c5be5f26f4 196 LinStepMtr::Step_Num LinStepMtr::Step::get_cur_step()
mikermarza 0:54c5be5f26f4 197 {
mikermarza 0:54c5be5f26f4 198 return cur_step;
mikermarza 0:54c5be5f26f4 199 }
mikermarza 0:54c5be5f26f4 200
mikermarza 0:54c5be5f26f4 201 LinStepMtr::Step_Num LinStepMtr::Step::operator++()
mikermarza 0:54c5be5f26f4 202 {
mikermarza 0:54c5be5f26f4 203 switch(cur_step){
mikermarza 0:54c5be5f26f4 204 case ONE:
mikermarza 0:54c5be5f26f4 205 cur_step=TWO;
mikermarza 0:54c5be5f26f4 206 break;
mikermarza 0:54c5be5f26f4 207 case TWO:
mikermarza 0:54c5be5f26f4 208 cur_step = THREE;
mikermarza 0:54c5be5f26f4 209 break;
mikermarza 0:54c5be5f26f4 210 case THREE:
mikermarza 0:54c5be5f26f4 211 cur_step = FOUR;
mikermarza 0:54c5be5f26f4 212 break;
mikermarza 0:54c5be5f26f4 213 case FOUR:
mikermarza 0:54c5be5f26f4 214 cur_step = ONE;
mikermarza 0:54c5be5f26f4 215 break;
mikermarza 0:54c5be5f26f4 216 }
mikermarza 1:757a52db1604 217 //pc.printf(" CUR_STEP = %x\n",cur_step);
mikermarza 0:54c5be5f26f4 218 return cur_step;
mikermarza 0:54c5be5f26f4 219 }
mikermarza 0:54c5be5f26f4 220
mikermarza 0:54c5be5f26f4 221 LinStepMtr::Step_Num LinStepMtr::Step::operator--()
mikermarza 0:54c5be5f26f4 222 {
mikermarza 0:54c5be5f26f4 223 switch(cur_step){
mikermarza 0:54c5be5f26f4 224 case ONE:
mikermarza 0:54c5be5f26f4 225 cur_step=FOUR;
mikermarza 0:54c5be5f26f4 226 break;
mikermarza 0:54c5be5f26f4 227 case TWO:
mikermarza 0:54c5be5f26f4 228 cur_step = ONE;
mikermarza 0:54c5be5f26f4 229 break;
mikermarza 0:54c5be5f26f4 230 case THREE:
mikermarza 0:54c5be5f26f4 231 cur_step = TWO;
mikermarza 0:54c5be5f26f4 232 break;
mikermarza 0:54c5be5f26f4 233 case FOUR:
mikermarza 0:54c5be5f26f4 234 cur_step = THREE;
mikermarza 0:54c5be5f26f4 235 break;
mikermarza 0:54c5be5f26f4 236 }
mikermarza 0:54c5be5f26f4 237 return cur_step;
mikermarza 1:757a52db1604 238 }
mikermarza 1:757a52db1604 239
mikermarza 1:757a52db1604 240 void LinStepMtr::Step::operator=(LinStepMtr::Step_Num s)
mikermarza 1:757a52db1604 241 {
mikermarza 1:757a52db1604 242 cur_step = s;
mikermarza 0:54c5be5f26f4 243 }