Revision 0:4beb37ae37ce, committed 2010-11-25

Comitter:

okano

Date:

Thu Nov 25 11:11:51 2010 +0000

Child:

1:dc6cf8f8bcb7

Commit message:

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/StepperMotor.cpp	Thu Nov 25 11:11:51 2010 +0000
@@ -0,0 +1,193 @@
+#include "mbed.h"
+#include "StepperMotor.h"
+
+StepperMotor::StepperMotor(
+    PinName out_A,
+    PinName out_B,
+    PinName out_PWR,
+    PinName position_detect
+) :
+        motor_out( out_A, out_B ),
+        pwr_out( out_PWR ),
+        position_detect_pin( position_detect ),
+        rot_mode( SHORTEST ),
+        sync_mode( ASYNCHRONOUS ),
+        max_pos( 480 ),
+        current_pos( 0 ),
+        pos_offset( 0 ),
+        target_pos( 0 ),
+        max_pps( MAX_PPS ),
+        init_done( false ),
+        pause( false ),
+        power_ctrl( false ) {
+
+    pps     = max_pps;
+    t.attach( this, &StepperMotor::motor_maintain, 1.0 / (float)pps );
+}
+
+int StepperMotor::set_pps( int v ) {
+    int     p;
+
+    p   = pps;
+    pps = v;
+    t.detach();
+    t.attach( this, &StepperMotor::motor_maintain, 1.0 / (float)pps );
+    return ( p );
+}
+
+void StepperMotor::set_max_pps( int v ) {
+    max_pps     = v;
+}
+
+int StepperMotor::find_home_position( PositionDetectPorarity edge ) {
+    RotMode     prev_rot;
+    SyncMode    prev_sync;
+    int         prev_pps;
+    int         prev_det_pin;
+    int         direction;
+
+    prev_pps    = set_pps( max_pps );
+    prev_rot    = rot_mode;
+    prev_sync   = sync_mode;
+    set_sync_mode( SYNCHRONOUS );
+    set_rot_mode( SHORTEST );
+
+    prev_det_pin    = position_detect_pin;
+
+    if ( prev_rot == COUNTER_CLOCKWISE_ONLY )
+        direction   = -1;
+    else
+        direction   = 1;
+
+    if ( prev_rot == NO_WRAPAROUND ) {
+
+        for ( int i = 0; i < (max_pos >> 1); i++ ) {
+            move_steps( -1 );
+            if ( ((edge == RISING_EDGE) && !prev_det_pin && position_detect_pin)
+                    || ((edge == FALLING_EDGE) && prev_det_pin && !position_detect_pin) ) {
+                set_home_position();
+                init_done   = true;
+                break;
+            }
+            prev_det_pin    = position_detect_pin;
+        }
+    }
+
+    for ( int i = 0; i < ((prev_rot == NO_WRAPAROUND) ? (max_pos - 1) : (max_pos + 10)); i++ ) {
+        move_steps( direction );
+        if ( ((edge == RISING_EDGE) && !prev_det_pin && position_detect_pin)
+                || ((edge == FALLING_EDGE) && prev_det_pin && !position_detect_pin) ) {
+            set_home_position();
+            init_done   = true;
+            break;
+        }
+        prev_det_pin    = position_detect_pin;
+    }
+
+    go_position( 0 );
+    set_pps( prev_pps );
+    set_rot_mode( prev_rot );
+    set_sync_mode( prev_sync );
+
+    return ( init_done );
+}
+
+void StepperMotor::set_home_position( void ) {
+    set_pause( true );
+    pos_offset  = current_pos & 0x3;
+    current_pos = 0;
+    set_target_pos( 0 );
+    set_pause( false );
+}
+
+int StepperMotor::go_position( int v ) {
+    set_target_pos( v );
+    return ( current_pos );
+}
+
+void StepperMotor::go_angle( float angle ) {
+    go_position( (int)((angle / 360.0) * (float)max_pos) );
+}
+
+int StepperMotor::move_steps( int s ) {
+    set_target_pos( current_pos + s );
+    return ( current_pos );
+}
+
+void StepperMotor::set_rot_mode( RotMode m ) {
+    rot_mode    = m;
+}
+
+void StepperMotor::set_sync_mode( SyncMode m ) {
+    sync_mode    = m;
+}
+
+int StepperMotor::distance( void ) {
+    return( target_pos - current_pos );
+}
+
+void StepperMotor::set_pause( int sw ) {
+    pause   = sw;
+}
+
+void StepperMotor::set_target_pos( int p ) {
+    target_pos  = (p + max_pos) % max_pos;
+
+    if (sync_mode == SYNCHRONOUS)
+        while ( distance() )
+            wait( 0 );
+}
+
+void StepperMotor::set_power_ctrl( int sw ) {
+    power_ctrl  = sw;
+}
+
+void StepperMotor::set_steps_per_rotate( int steps ) {
+    max_pos = steps;
+}
+
+void StepperMotor::motor_maintain( void ) {
+
+    int     diff;
+    int     direction;
+
+    if ( pause )
+        return;
+
+    diff    = target_pos - current_pos;
+
+    if ( !diff ) {
+        pwr_out = power_ctrl ? 0 : 1;
+        return;
+    }
+
+    pwr_out = 1;
+
+    diff = (diff + max_pos) % max_pos;
+
+    if ( diff > (max_pos >> 1) )
+        diff -= max_pos;
+
+    switch ( rot_mode ) {
+        case NO_WRAPAROUND :
+            direction   = ( (target_pos - current_pos) < 0 ) ? -1 : 1;
+            break;
+        case CLOCKWISE_ONLY :
+            direction   = 1;
+            break;
+        case COUNTER_CLOCKWISE_ONLY :
+            direction   = -1;
+            break;
+        default : // SHORTEST :
+            direction   = ( diff < 0 ) ? -1 : 1;
+            break;
+    }
+
+
+    current_pos = ((current_pos + max_pos) + direction) % max_pos;
+
+//    printf( "pos=%d  ofst=%d  puls=%d\r\n", current_pos, pos_offset, (current_pos + pos_offset) );
+    motor_out   = pat[ (current_pos + pos_offset) & 0x3 ];
+};
+
+const unsigned char StepperMotor::pat[ 4 ]  = { 0, 1, 3, 2 };

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/StepperMotor.h	Thu Nov 25 11:11:51 2010 +0000
@@ -0,0 +1,228 @@
+/** Stepper Motor control library
+ *  
+ *  Copyright: 2010 Tedd OKANO, Tsukimidai Communications Syndicate - Crawl Design
+ *  The library that controls stepper motor via motor driver chip: TA7774
+ *  The TA7774 is a driver for a bipolar stepper motor. 
+ *  With this library, mbed will generate 2 phase pulses to operate the motor. 
+ */
+ 
+#ifndef    MBED_STEPPERMOTOR
+#define    MBED_STEPPERMOTOR
+
+#include "mbed.h"
+
+#define MAX_PPS   50   //  pulse per second
+
+/** Stepper Motor control class
+ *
+ *  Example:
+ *  @code
+ *  #include "mbed.h"
+ *  #include "StepperMotor.h"
+ *
+ *  StepperMotor m( p21, p22, p23, p24 );
+ *
+ *  int main() {
+ *      m.set_sync_mode( StepperMotor::SYNCHRONOUS );
+ *      m.set_power_ctrl( true );
+ *
+ *      while( 1 ) {
+ *          m.go_angle( 120 );
+ *          wait( 0.5 );
+ *     
+ *          m.go_angle( 240 );
+ *          wait( 0.5 );
+ *   
+ *          m.go_angle( 0 );
+ *          wait( 0.5 ); 
+ *
+ *          m.go_angle( 240 );
+ *          wait( 0.5 );
+ *     
+ *          m.go_angle( 120 );
+ *          wait( 0.5 );
+ *   
+ *          m.go_angle( 0 );
+ *          wait( 0.5 ); 
+ *      }
+ *  }
+ *  @endcode
+ */
+
+class StepperMotor {
+public:
+
+    /** Constants for motor rotate mode */     
+    typedef enum  {
+        SHORTEST,                   /**< turn by shortest direction      */
+        NO_WRAPAROUND,              /**< do not accross home position    */
+        CLOCKWISE_ONLY,             /**< one-way: clockwise turn         */
+        COUNTER_CLOCKWISE_ONLY      /**< one-way: counter clockwise turn */
+    } RotMode;
+
+    /** Constants for syncronization mode */     
+    typedef enum  {
+        ASYNCHRONOUS,               /**< program does wait motor turn completion     */
+        SYNCHRONOUS                 /**< program doesn't wait motor turn completion  */
+    } SyncMode;
+
+    /** Constants for position detection edge polarity */     
+    typedef enum  {
+        RISING_EDGE,                /**< position detection done by rising  edge */
+        FALLING_EDGE                /**< position detection done by falling edge */
+    } PositionDetectPorarity;
+
+    /** Create a stepper motor object connected to specified DigitalOut pins and a DigitalIn pin
+     *
+     *  @param out_A DigitalOut pin for motor pulse signal-A
+     *  @param out_B DigitalOut pin for motor pulse signal-B
+     *  @param out_PWR DigitalOut pin for TA7774's power control (option)
+     *  @param position_detect DigitalIn pin for home position detection (option)
+     */
+    StepperMotor(
+        PinName out_A   = p21,
+        PinName out_B   = p22,
+        PinName out_PWR = p23,
+        PinName position_detect = p24
+    ) ;
+
+    /** Set the pulse width (i.e. motor turning speed)
+     *
+     *  @param v pulse per second : default is 100. lower number makes the turn slower
+     */
+    int set_pps( int v );
+
+    /** Set maximum PPS (= minimum pulse width) which will be used in finding home position
+     *
+     *  @param v maximum pulse per second : default is 100. lower number makes the turn slower
+     */
+    void set_max_pps( int v );
+
+    /** Find home position: rotate the motor until the detection edge comes.
+     *
+     *  Turns the motor until the home position detected. 
+     *  The "home position" is a reference point for the step and angle. It will be step=0 and angle=0. 
+     *  The detection signal edge can be defined by an argument. 
+     *  It follows the rotate mode. 
+     *  When the edge is detected, the motor will be stopped and it will be the new home position. 
+     *  If no detection signal detected, no home position update done. 
+     * 
+     *  @param edge defines detection edge rise or fall
+     */
+    int find_home_position( PositionDetectPorarity edge );
+    
+    /** Update home position
+     *
+     *  Set the home position as current motor position.
+     */     
+    void set_home_position( void );
+    
+    /** Turn the motor to defined position (by steps from home position)
+     *
+     *  Make motor move to absolute position
+     *
+     *  @param v the position defined by steps from home position
+     */     
+    int go_position( int v );
+
+    /** Turn the motor to defined position (by angle (0.0..360 degree) from home position)
+     *
+     *  Make motor move to absolute position
+     *
+     *  @param v the position defined by steps from home position
+     */     
+    void go_angle( float angle );
+
+    /** Turn the motor to defined position (by steps from current position)
+     *
+     *  Make motor move to defined position
+     *
+     *  @param v the position defined by steps from home position
+     */     
+    int move_steps( int s );
+    
+    /** Interface for motor rotate mode setting
+     *
+     *  Example:
+     *  @code
+     *  StepperMotor    m( p21, p22, p23, p24 );
+     *  int main() {
+     *      m.set_rot_mode( StepperMotor::NO_WRAPAROUND );
+     *      ...
+     *  @endcode
+     *
+     *  @param m motor rotate mode : SHORTEST, NO_WRAPAROUND, CLOCKWISE_ONLY or COUNTER_CLOCKWISE_ONLY
+     */
+    void set_rot_mode( RotMode m );
+
+    /** Interface for syncronization mode setting
+     *
+     *  Example:
+     *  @code
+     *  StepperMotor    m( p21, p22, p23, p24 );
+     *  int main() {
+     *      m.set_sync_mode( StepperMotor::NO_WRAPAROUND );
+     *      ...
+     *  @endcode
+     *
+     *  @param m motor rotate mode : ASYNCHRONOUS or SYNCHRONOUS
+     */
+    void set_sync_mode( SyncMode m );
+
+    /** Check remaining distance that motor need to move
+     *
+     *  software can check if the motor action completed in asynchronous mode
+     *  
+     *  @return remaining steps that motor need to go
+     */
+    int distance( void );
+
+    /** Pause/Resume the motor action
+     *
+     *  @param sw use "true" for pause, "false" for resume
+     */
+    void set_pause( int sw );
+
+    /** Auto power control enable
+     *
+     *  If the auto power control is enabled, the motor power will be turned-off when it stays same place
+     *  
+     *  @param sw use "true" for pause, "false" for resume
+     */
+    void set_power_ctrl( int sw );
+
+    /** Setting for steps/rotate
+     *
+     *  This parameter is required if program want to use the "go_angle()" interface. 
+     *  The angle will be calculated from this parameter.
+     *  
+     *  @param steps per rotate
+     */
+    void set_steps_per_rotate( int steps );
+    
+private:
+
+    Ticker      t;
+    BusOut      motor_out;
+    DigitalOut  pwr_out;
+    DigitalIn   position_detect_pin;
+
+    static const unsigned char  pat[ 4 ];  //  2 phase pulse pattern for motor control
+    RotMode     rot_mode;
+    SyncMode    sync_mode;
+    int         max_pos;
+    int         current_pos;
+    int         pos_offset;
+    int         target_pos;
+    int         pps;
+    int         max_pps;
+    int         init_done;
+    int         pause;
+    int         power_ctrl;
+
+    void set_target_pos( int p );  //  target position setting interface
+    void motor_maintain( void );   //  this function is called periodically by Ticker
+};
+
+
+#endif
\ No newline at end of file