With more robust driving method and calibration routine

Dependents:   Stepper_Motor_Demo Stepper_Motor_Demo_final

Fork of Stepper_Motor_X27168 by Stepper Motor

Committer:
yhbyhb4433
Date:
Wed Mar 16 18:03:32 2016 +0000
Revision:
1:406a6e6c4bd7
Parent:
0:c346170974bc
Improve driving method and add calibration routine

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;
agarg45 0:c346170974bc 9 cur_state = STOP_MOTOR;
agarg45 0:c346170974bc 10 cur_position = 0;
agarg45 0:c346170974bc 11 }
agarg45 0:c346170974bc 12
agarg45 0:c346170974bc 13 StepperMotor_X27168::StepperMotor_X27168(PinName A_f, PinName A_r, PinName B_f, PinName B_r, int init_step_position)
agarg45 0:c346170974bc 14 :motor_control(A_f, A_r, B_f, B_r)
agarg45 0:c346170974bc 15 {
agarg45 0:c346170974bc 16 StepperMotor_X27168(A_f, A_r, B_f, B_r);
agarg45 0:c346170974bc 17 step_position(init_step_position);
agarg45 0:c346170974bc 18 }
agarg45 0:c346170974bc 19
agarg45 0:c346170974bc 20 void StepperMotor_X27168::set_speed(float s) {
agarg45 0:c346170974bc 21 speed = s;
agarg45 0:c346170974bc 22 }
agarg45 0:c346170974bc 23
agarg45 0:c346170974bc 24 int StepperMotor_X27168::get_speed() {
agarg45 0:c346170974bc 25 return speed;
agarg45 0:c346170974bc 26 }
agarg45 0:c346170974bc 27
agarg45 0:c346170974bc 28 void StepperMotor_X27168::set_max_position(int p) {
agarg45 0:c346170974bc 29 if(p<MAX_POS) {
agarg45 0:c346170974bc 30 max_position = p;
agarg45 0:c346170974bc 31 }
agarg45 0:c346170974bc 32 }
agarg45 0:c346170974bc 33
agarg45 0:c346170974bc 34 int StepperMotor_X27168::get_max_position() {
agarg45 0:c346170974bc 35 return max_position;
agarg45 0:c346170974bc 36 }
agarg45 0:c346170974bc 37
agarg45 0:c346170974bc 38 int StepperMotor_X27168::step(int dir) {
agarg45 0:c346170974bc 39 if(dir==2)
agarg45 0:c346170974bc 40 cur_state = STOP_MOTOR;
agarg45 0:c346170974bc 41 else if(dir == 0) {
agarg45 0:c346170974bc 42 cur_state = (Polarity)((cur_state+1)%4);
agarg45 0:c346170974bc 43
agarg45 0:c346170974bc 44 if(cur_position <= MAX_POS) {
agarg45 0:c346170974bc 45 cur_position++;
agarg45 0:c346170974bc 46 }
agarg45 0:c346170974bc 47 }
agarg45 0:c346170974bc 48 else if (dir == 1) {
agarg45 0:c346170974bc 49 cur_state = (Polarity)((cur_state-1)%4);
agarg45 0:c346170974bc 50 cur_state = (Polarity)(cur_state == 255 ? cur_state + 4 : cur_state);
agarg45 0:c346170974bc 51
agarg45 0:c346170974bc 52 if(cur_position>= 0) {
agarg45 0:c346170974bc 53 cur_position--;
agarg45 0:c346170974bc 54 }
agarg45 0:c346170974bc 55 }
agarg45 0:c346170974bc 56 else
agarg45 0:c346170974bc 57 return -1;
agarg45 0:c346170974bc 58
yhbyhb4433 1:406a6e6c4bd7 59 switch (cur_state) { //improve 2 phase drive method. Original method commented out
agarg45 0:c346170974bc 60 case 0:
yhbyhb4433 1:406a6e6c4bd7 61 motor_control = 0x9;//0x1;
agarg45 0:c346170974bc 62 break;
agarg45 0:c346170974bc 63 case 1:
yhbyhb4433 1:406a6e6c4bd7 64 motor_control = 0x5;//0x4;
agarg45 0:c346170974bc 65 break;
agarg45 0:c346170974bc 66 case 2:
yhbyhb4433 1:406a6e6c4bd7 67 motor_control = 0x6;//0x2;
agarg45 0:c346170974bc 68 break;
agarg45 0:c346170974bc 69 case 3:
yhbyhb4433 1:406a6e6c4bd7 70 motor_control = 0xA;//0x8;
agarg45 0:c346170974bc 71 break;
agarg45 0:c346170974bc 72 case 4:
agarg45 0:c346170974bc 73 motor_control = 0x0;
agarg45 0:c346170974bc 74 break;
agarg45 0:c346170974bc 75 }
agarg45 0:c346170974bc 76 wait(1.0/speed);
agarg45 0:c346170974bc 77 return cur_position;
agarg45 0:c346170974bc 78 }
agarg45 0:c346170974bc 79
agarg45 0:c346170974bc 80 void StepperMotor_X27168::step_position(int pos) {
agarg45 0:c346170974bc 81 if(pos > max_position)
agarg45 0:c346170974bc 82 pos = max_position;
agarg45 0:c346170974bc 83 else if(pos < 0)
agarg45 0:c346170974bc 84 pos = 0;
agarg45 0:c346170974bc 85
agarg45 0:c346170974bc 86 while(cur_position < pos) {
agarg45 0:c346170974bc 87 step(0);
agarg45 0:c346170974bc 88 }
agarg45 0:c346170974bc 89 while(cur_position > pos) {
agarg45 0:c346170974bc 90 step(1);
agarg45 0:c346170974bc 91 }
agarg45 0:c346170974bc 92
agarg45 0:c346170974bc 93 step(2);
agarg45 0:c346170974bc 94 }
agarg45 0:c346170974bc 95
agarg45 0:c346170974bc 96 void StepperMotor_X27168::angle_position(float angle) {
agarg45 0:c346170974bc 97 step_position(int(angle*2));
yhbyhb4433 1:406a6e6c4bd7 98 }
yhbyhb4433 1:406a6e6c4bd7 99
yhbyhb4433 1:406a6e6c4bd7 100 void StepperMotor_X27168::init()
yhbyhb4433 1:406a6e6c4bd7 101 {
yhbyhb4433 1:406a6e6c4bd7 102 set_speed(400); //if too fast(>400) the motor will slip and go weird
yhbyhb4433 1:406a6e6c4bd7 103
yhbyhb4433 1:406a6e6c4bd7 104 step_position(MAX_POS); //sweep to MAX position
yhbyhb4433 1:406a6e6c4bd7 105 wait(0.5);
yhbyhb4433 1:406a6e6c4bd7 106
yhbyhb4433 1:406a6e6c4bd7 107 step_position(0); //sweep to zero position
yhbyhb4433 1:406a6e6c4bd7 108 wait(0.5);
agarg45 0:c346170974bc 109 }