StepperMotor.cpp

00001 #include "StepperMotor.h"
00002 #include "mbed.h"
00003 
00004 //Table for motor steps
00005 static uint8_t ptn_ClockWise_1x1[]     = {0x01, 0x02, 0x04, 0x08};
00006 static uint8_t ptn_AntiClockWise_1x1[] = {0x08, 0x04, 0x02, 0x01};
00007 
00008 static uint8_t ptn_ClockWise_2x2[]     = {0x03, 0x06, 0x0C, 0x09};
00009 static uint8_t ptn_AntiClockWise_2x2[] = {0x09, 0x0C, 0x06, 0x03};
00010 
00011 static uint8_t ptn_ClockWise_1x2[]     = {0x03, 0x02, 0x06, 0x04, 0x0C, 0x08, 0x09, 0x01};
00012 static uint8_t ptn_AntiClockWise_1x2[] = {0x01, 0x09, 0x08, 0x0C, 0x04, 0x06, 0x02, 0x03};
00013 
00014 static speed_data_t speed_data [] =
00015 {
00016     {     1,0,ptn_ClockWise_1x2,ptn_AntiClockWise_1x2,8},   // 0
00017     {1000*2,5,ptn_ClockWise_1x2,ptn_AntiClockWise_1x2,8},   // 1
00018     {500*2, 5,ptn_ClockWise_1x2,ptn_AntiClockWise_1x2,8},   // 2
00019     {333*2, 5,ptn_ClockWise_1x2,ptn_AntiClockWise_1x2,8},   // 3
00020     {250*2, 5,ptn_ClockWise_1x2,ptn_AntiClockWise_1x2,8},   // 4
00021     {200,   7,ptn_ClockWise_1x1,ptn_AntiClockWise_1x1,4},   // 5
00022     {167,   8,ptn_ClockWise_1x1,ptn_AntiClockWise_1x1,4},   // 6
00023     {143,   8,ptn_ClockWise_1x1,ptn_AntiClockWise_1x1,4},   // 7
00024     {125,   9,ptn_ClockWise_1x1,ptn_AntiClockWise_2x2,4},   // 8
00025     {111,  10,ptn_ClockWise_2x2,ptn_AntiClockWise_2x2,4},   // 9
00026     {100,  10,ptn_ClockWise_2x2,ptn_AntiClockWise_2x2,4}    // 10
00027 };
00028 
00029 
00030 
00031 // Constractor
00032 StepperMotor::StepperMotor(PinName x_pin_no, PinName y_pin_no, PinName nx_pin_no, PinName ny_pin_no)
00033         : _x(x_pin_no), _y(y_pin_no), _nx(nx_pin_no), _ny(ny_pin_no) {
00034 }
00035 
00036 // Destrutctor
00037 StepperMotor::~StepperMotor() {
00038 }
00039 
00040 //
00041 void StepperMotor::PulseEnable(void) {
00042     Pulse.attach_us(this,&StepperMotor::SetPulse14us,PULSE_INTERVAL);
00043 }
00044 
00045 //
00046 void StepperMotor::PulseDisable(void) {
00047     Pulse.detach();
00048 }
00049 
00050 void StepperMotor::SetSpeed(int speed, motor_dir direction) {
00051 
00052     if (speed > 10) speed = 10;
00053     if (speed < 0) speed =0;
00054 
00055 
00056     pwm_ratio = speed_data[speed].pwm_ratio;
00057     max_pulse_count = speed_data[speed].max_pulse_count;
00058     
00059     if (direction == CLOCK_WISE) {
00060         ptn = speed_data[speed].clockwise_ptn;
00061     }
00062     else {
00063         ptn = speed_data[speed].anticlockwise_ptn;        
00064     }
00065     ptn_count = speed_data[speed].ptn_count;
00066 
00067     pulse_count = 0;
00068     ptn_index = 0;
00069 
00070     pwm_on_count = pwm_ratio;
00071     pwm_off_count = 10 - pwm_ratio;
00072 
00073     state = PWM_ON;
00074 }
00075 
00076 
00077 /* private functions */
00078 
00079 void StepperMotor::SetPulse14us(void) {
00080 
00081     if (++pulse_count == max_pulse_count) {
00082         pulse_count = 0;
00083 
00084         if (++ptn_index == ptn_count) {
00085             ptn_index = 0;
00086         }
00087         ptn_data = ptn[ptn_index];
00088     }
00089 
00090     if (pwm_ratio == 10) {
00091         PulseOut();
00092     } else if (pwm_ratio == 0) {
00093         PulseStop();
00094     } else {
00095         switch (state) {
00096             case PWM_ON:
00097                 if (--pwm_on_count !=0) {
00098                     PulseOut();
00099                 } else {
00100                     pwm_on_count = pwm_ratio;
00101                     state = PWM_OFF;
00102                 }
00103                 break;
00104             case PWM_OFF:
00105                 if (--pwm_off_count != 0) {
00106                     PulseStop();
00107                 } else {
00108                     pwm_off_count = 10 - pwm_ratio;
00109                     state = PWM_ON;
00110                 }
00111                 break;
00112         }
00113     }
00114 }
00115 
00116 void StepperMotor::PulseOut(void) {
00117     //X
00118     if ((ptn_data & 0x01) == 0x01) {
00119         _x = 1;
00120     } else {
00121         _x = 0;
00122     }
00123     //Y
00124     if ((ptn_data & 0x02) == 0x02) {
00125         _y = 1;
00126     } else {
00127         _y = 0;
00128     }
00129     //Negative X
00130     if ((ptn_data & 0x04) == 0x04) {
00131         _nx = 1;
00132     } else {
00133         _nx = 0;
00134     }
00135     //Negative Y
00136     if ((ptn_data & 0x08) == 0x08) {
00137         _ny = 1;
00138     } else {
00139         _ny = 0;
00140     }
00141 }
00142 
00143 void StepperMotor::PulseStop(void) {
00144     _x = 0;
00145     _y = 0;
00146     _nx = 0;
00147     _ny = 0;
00148 }