Casey Lively / Stepper_Motor_X27168

Adds pointer acceleration to combat missed steps and losing track of position

Dependents:   Auto

Fork of Stepper_Motor_X27168 by Stepper Motor

Stepper_Motor_X27168.cpp@1:e7c0920184e4, 2016-05-02 (annotated)

Committer:
clively6
Date:
Mon May 02 15:03:35 2016 +0000
Revision:
1:e7c0920184e4
Parent:
0:c346170974bc 
Added accelleration to combat missed steps;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
agarg45 0:c346170974bc 1 #include "StepperMotor_X27168.h"
agarg45 0:c346170974bc 2
agarg45 0:c346170974bc 3 StepperMotor_X27168::StepperMotor_X27168(PinName A_f, PinName A_r, PinName B_f, PinName B_r)
agarg45 0:c346170974bc 4 :motor_control(A_f, A_r, B_f, B_r)
agarg45 0:c346170974bc 5 {
agarg45 0:c346170974bc 6 motor_control = 0x0;
agarg45 0:c346170974bc 7 max_position = MAX_POS;
agarg45 0:c346170974bc 8 speed = DEFAULT_SPEED;
clively6 1:e7c0920184e4 9 max_speed = 1000;
agarg45 0:c346170974bc 10 cur_state = STOP_MOTOR;
agarg45 0:c346170974bc 11 cur_position = 0;
agarg45 0:c346170974bc 12 }
agarg45 0:c346170974bc 13
agarg45 0:c346170974bc 14 StepperMotor_X27168::StepperMotor_X27168(PinName A_f, PinName A_r, PinName B_f, PinName B_r, int init_step_position)
agarg45 0:c346170974bc 15 :motor_control(A_f, A_r, B_f, B_r)
agarg45 0:c346170974bc 16 {
agarg45 0:c346170974bc 17 StepperMotor_X27168(A_f, A_r, B_f, B_r);
agarg45 0:c346170974bc 18 step_position(init_step_position);
agarg45 0:c346170974bc 19 }
agarg45 0:c346170974bc 20
agarg45 0:c346170974bc 21 void StepperMotor_X27168::set_speed(float s) {
agarg45 0:c346170974bc 22 speed = s;
agarg45 0:c346170974bc 23 }
agarg45 0:c346170974bc 24
clively6 1:e7c0920184e4 25 void StepperMotor_X27168::set_max_speed(float s) {
clively6 1:e7c0920184e4 26 max_speed = s;
clively6 1:e7c0920184e4 27 }
clively6 1:e7c0920184e4 28
agarg45 0:c346170974bc 29 int StepperMotor_X27168::get_speed() {
agarg45 0:c346170974bc 30 return speed;
agarg45 0:c346170974bc 31 }
agarg45 0:c346170974bc 32
clively6 1:e7c0920184e4 33 int StepperMotor_X27168::get_position() {
clively6 1:e7c0920184e4 34 return cur_position;
clively6 1:e7c0920184e4 35 }
clively6 1:e7c0920184e4 36
agarg45 0:c346170974bc 37 void StepperMotor_X27168::set_max_position(int p) {
agarg45 0:c346170974bc 38 if(p<MAX_POS) {
agarg45 0:c346170974bc 39 max_position = p;
agarg45 0:c346170974bc 40 }
agarg45 0:c346170974bc 41 }
agarg45 0:c346170974bc 42
agarg45 0:c346170974bc 43 int StepperMotor_X27168::get_max_position() {
agarg45 0:c346170974bc 44 return max_position;
agarg45 0:c346170974bc 45 }
agarg45 0:c346170974bc 46
agarg45 0:c346170974bc 47 int StepperMotor_X27168::step(int dir) {
agarg45 0:c346170974bc 48 if(dir==2)
agarg45 0:c346170974bc 49 cur_state = STOP_MOTOR;
agarg45 0:c346170974bc 50 else if(dir == 0) {
agarg45 0:c346170974bc 51 cur_state = (Polarity)((cur_state+1)%4);
agarg45 0:c346170974bc 52
agarg45 0:c346170974bc 53 if(cur_position <= MAX_POS) {
agarg45 0:c346170974bc 54 cur_position++;
agarg45 0:c346170974bc 55 }
agarg45 0:c346170974bc 56 }
agarg45 0:c346170974bc 57 else if (dir == 1) {
agarg45 0:c346170974bc 58 cur_state = (Polarity)((cur_state-1)%4);
agarg45 0:c346170974bc 59 cur_state = (Polarity)(cur_state == 255 ? cur_state + 4 : cur_state);
agarg45 0:c346170974bc 60
agarg45 0:c346170974bc 61 if(cur_position>= 0) {
agarg45 0:c346170974bc 62 cur_position--;
agarg45 0:c346170974bc 63 }
agarg45 0:c346170974bc 64 }
agarg45 0:c346170974bc 65 else
agarg45 0:c346170974bc 66 return -1;
agarg45 0:c346170974bc 67
agarg45 0:c346170974bc 68 switch (cur_state) {
agarg45 0:c346170974bc 69 case 0:
agarg45 0:c346170974bc 70 motor_control = 0x1;
agarg45 0:c346170974bc 71 break;
agarg45 0:c346170974bc 72 case 1:
agarg45 0:c346170974bc 73 motor_control = 0x4;
agarg45 0:c346170974bc 74 break;
agarg45 0:c346170974bc 75 case 2:
agarg45 0:c346170974bc 76 motor_control = 0x2;
agarg45 0:c346170974bc 77 break;
agarg45 0:c346170974bc 78 case 3:
agarg45 0:c346170974bc 79 motor_control = 0x8;
agarg45 0:c346170974bc 80 break;
agarg45 0:c346170974bc 81 case 4:
agarg45 0:c346170974bc 82 motor_control = 0x0;
agarg45 0:c346170974bc 83 break;
agarg45 0:c346170974bc 84 }
agarg45 0:c346170974bc 85 wait(1.0/speed);
agarg45 0:c346170974bc 86 return cur_position;
agarg45 0:c346170974bc 87 }
agarg45 0:c346170974bc 88
agarg45 0:c346170974bc 89 void StepperMotor_X27168::step_position(int pos) {
agarg45 0:c346170974bc 90 if(pos > max_position)
agarg45 0:c346170974bc 91 pos = max_position;
agarg45 0:c346170974bc 92 else if(pos < 0)
agarg45 0:c346170974bc 93 pos = 0;
agarg45 0:c346170974bc 94
agarg45 0:c346170974bc 95 while(cur_position < pos) {
agarg45 0:c346170974bc 96 step(0);
agarg45 0:c346170974bc 97 }
agarg45 0:c346170974bc 98 while(cur_position > pos) {
agarg45 0:c346170974bc 99 step(1);
agarg45 0:c346170974bc 100 }
agarg45 0:c346170974bc 101
agarg45 0:c346170974bc 102 step(2);
agarg45 0:c346170974bc 103 }
agarg45 0:c346170974bc 104
clively6 1:e7c0920184e4 105 void StepperMotor_X27168::set_position_dynamic(float pos){
clively6 1:e7c0920184e4 106 if(pos > max_position)
clively6 1:e7c0920184e4 107 pos = max_position;
clively6 1:e7c0920184e4 108 else if(pos < 0)
clively6 1:e7c0920184e4 109 pos = 0;
clively6 1:e7c0920184e4 110
clively6 1:e7c0920184e4 111
clively6 1:e7c0920184e4 112 int delta = abs(pos - cur_position);
clively6 1:e7c0920184e4 113 speed = 100;
clively6 1:e7c0920184e4 114
clively6 1:e7c0920184e4 115 set_speed(speed);
clively6 1:e7c0920184e4 116
clively6 1:e7c0920184e4 117
clively6 1:e7c0920184e4 118 while(cur_position < pos) {
clively6 1:e7c0920184e4 119 if(abs(pos - cur_position) > delta/2 && speed < max_speed)
clively6 1:e7c0920184e4 120 speed +=50;
clively6 1:e7c0920184e4 121 if(abs(pos - cur_position) <= delta/2&& abs(pos - cur_position) <= 25 && speed >100)
clively6 1:e7c0920184e4 122 speed -= 50;
clively6 1:e7c0920184e4 123
clively6 1:e7c0920184e4 124 step(0);
clively6 1:e7c0920184e4 125 }
clively6 1:e7c0920184e4 126 while(cur_position > pos) {
clively6 1:e7c0920184e4 127 if(abs(pos - cur_position) > delta/2 && speed < max_speed)
clively6 1:e7c0920184e4 128 speed +=50;
clively6 1:e7c0920184e4 129 if(abs(pos - cur_position) <= delta/2 && abs(pos - cur_position) <= 25 && speed >100)
clively6 1:e7c0920184e4 130 speed -= 50;
clively6 1:e7c0920184e4 131
clively6 1:e7c0920184e4 132 step(1);
clively6 1:e7c0920184e4 133 }
clively6 1:e7c0920184e4 134
clively6 1:e7c0920184e4 135 step(2);
clively6 1:e7c0920184e4 136
clively6 1:e7c0920184e4 137
clively6 1:e7c0920184e4 138
clively6 1:e7c0920184e4 139 }
clively6 1:e7c0920184e4 140
agarg45 0:c346170974bc 141 void StepperMotor_X27168::angle_position(float angle) {
clively6 1:e7c0920184e4 142 set_position_dynamic(int(angle*2));
agarg45 0:c346170974bc 143 }