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) {
00060         case 0:
00061             motor_control = 0x1;
00062             break;
00063         case 1:
00064             motor_control = 0x4;
00065             break;
00066         case 2:
00067             motor_control = 0x2;
00068             break;
00069         case 3:
00070             motor_control = 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 int StepperMotor_X27168::get_position() {
00101     return cur_position;   
00102 }
00103 
00104 void StepperMotor_X27168::set_position_dynamic(float pos){
00105      if(pos > max_position)
00106         pos = max_position;
00107     else if(pos < 0)
00108         pos = 0;
00109 
00110 
00111     int delta = abs(pos - cur_position);
00112     speed = 100;
00113     int max_speed = 1000;
00114     //set_speed(speed);
00115 
00116 
00117     while(cur_position < pos) {
00118         if(abs(pos - cur_position) > delta/2 && speed < max_speed)
00119             speed +=50;
00120         if(abs(pos - cur_position) <= delta/2&& abs(pos - cur_position) <= 25 && speed >100)
00121             speed -= 50;
00122 
00123         step(0);
00124     }
00125     while(cur_position > pos) {
00126         if(abs(pos - cur_position) > delta/2 && speed < max_speed)
00127             speed +=50;
00128         if(abs(pos - cur_position) <= delta/2 && abs(pos - cur_position) <= 25 && speed >100)
00129             speed -= 50;
00130 
00131         step(1);
00132     }
00133 
00134     step(2);
00135 
00136 
00137 
00138 }