Stepper_Motor_X27168.cpp

00001 #include "StepperMotor_X27168.h"
00002 
00003 StepperMotor_X27168::StepperMotor_X27168(PinName A_f, PinName A_r, PinName B_f, PinName B_r) 
00004             :motor_control(A_f, A_r, B_f, B_r)
00005 {
00006         motor_control = 0x0;
00007         max_position = MAX_POS;
00008         speed = DEFAULT_SPEED;
00009         cur_state = STOP_MOTOR;
00010         cur_position = 0;
00011 }
00012 
00013 StepperMotor_X27168::StepperMotor_X27168(PinName A_f, PinName A_r, PinName B_f, PinName B_r, int init_step_position)
00014             :motor_control(A_f, A_r, B_f, B_r)
00015 {
00016         StepperMotor_X27168(A_f, A_r, B_f, B_r);
00017         step_position(init_step_position);
00018 }
00019 
00020 void StepperMotor_X27168::set_speed(float s) {
00021     speed = s;
00022 }
00023 
00024 int StepperMotor_X27168::get_speed() {
00025     return speed;
00026 }
00027 
00028 void StepperMotor_X27168::set_max_position(int p) {
00029     if(p<MAX_POS) {
00030         max_position = p;
00031     }
00032 }
00033 
00034 int StepperMotor_X27168::get_max_position() {
00035     return max_position;
00036 }
00037      
00038 int StepperMotor_X27168::step(int dir) {
00039     if(dir==2)
00040         cur_state = STOP_MOTOR;
00041     else if(dir == 0) {
00042         cur_state = (Polarity)((cur_state+1)%4);
00043         
00044         if(cur_position <= MAX_POS) {
00045             cur_position++;
00046         }
00047     }
00048     else if (dir == 1) {
00049         cur_state = (Polarity)((cur_state-1)%4);
00050         cur_state = (Polarity)(cur_state == 255 ? cur_state + 4 : cur_state);
00051         
00052         if(cur_position>= 0) {
00053             cur_position--;
00054         }
00055     }
00056     else
00057         return -1;
00058         
00059     switch (cur_state) { //improve 2 phase drive method. Original method commented out
00060         case 0:
00061             motor_control = 0x9;//0x1;
00062             break;
00063         case 1:
00064             motor_control = 0x5;//0x4;
00065             break;
00066         case 2:
00067             motor_control = 0x6;//0x2;
00068             break;
00069         case 3:
00070             motor_control = 0xA;//0x8;
00071             break;
00072         case 4:
00073             motor_control = 0x0;
00074             break;
00075     }
00076     wait(1.0/speed);
00077     return cur_position;
00078 }
00079 
00080 void StepperMotor_X27168::step_position(int pos) {
00081     if(pos > max_position)
00082         pos = max_position;
00083     else if(pos < 0)
00084         pos = 0;
00085     
00086     while(cur_position < pos) {
00087         step(0);
00088     }
00089     while(cur_position > pos) {
00090         step(1);
00091     }
00092     
00093     step(2);
00094 }
00095 
00096 void StepperMotor_X27168::angle_position(float angle) {
00097     step_position(int(angle*2));
00098 }
00099 
00100 void StepperMotor_X27168::init()
00101 {
00102     set_speed(400); //if too fast(>400) the motor will slip and go weird
00103 
00104     step_position(MAX_POS); //sweep to MAX position
00105     wait(0.5);
00106 
00107     step_position(0); //sweep to zero position
00108     wait(0.5);
00109 }