Matteus Car / Mbed 2 deprecated Testep

Biblioteca com go_angle

Dependencies:   mbed

StepperMotor.cpp@0:765da3331ade, 2020-04-13 (annotated)

Committer:
MatteusCarr
Date:
Mon Apr 13 22:17:13 2020 +0000
Revision:
0:765da3331ade
Biblioteca com go_angle

Who changed what in which revision?

UserRevisionLine numberNew contents of line
MatteusCarr 0:765da3331ade 1 #include "mbed.h"
MatteusCarr 0:765da3331ade 2 #include "StepperMotor.h"
MatteusCarr 0:765da3331ade 3
MatteusCarr 0:765da3331ade 4 StepperMotor::StepperMotor(
MatteusCarr 0:765da3331ade 5 PinName out_A,
MatteusCarr 0:765da3331ade 6 PinName out_B,
MatteusCarr 0:765da3331ade 7 PinName out_PWR,
MatteusCarr 0:765da3331ade 8 PinName position_detect
MatteusCarr 0:765da3331ade 9 ) :
MatteusCarr 0:765da3331ade 10 motor_out( out_A, out_B ),
MatteusCarr 0:765da3331ade 11 pwr_out( out_PWR ),
MatteusCarr 0:765da3331ade 12 position_detect_pin( position_detect ),
MatteusCarr 0:765da3331ade 13 rot_mode( SHORTEST ),
MatteusCarr 0:765da3331ade 14 sync_mode( ASYNCHRONOUS ),
MatteusCarr 0:765da3331ade 15 max_pos( 480 ),
MatteusCarr 0:765da3331ade 16 current_pos( 0 ),
MatteusCarr 0:765da3331ade 17 pos_offset( 0 ),
MatteusCarr 0:765da3331ade 18 target_pos( 0 ),
MatteusCarr 0:765da3331ade 19 max_pps( MAX_PPS ),
MatteusCarr 0:765da3331ade 20 init_done( false ),
MatteusCarr 0:765da3331ade 21 pause( false ),
MatteusCarr 0:765da3331ade 22 power_ctrl( false ) {
MatteusCarr 0:765da3331ade 23
MatteusCarr 0:765da3331ade 24 pps = max_pps;
MatteusCarr 0:765da3331ade 25 t.attach( this, &StepperMotor::motor_maintain, 1.0 / (float)pps );
MatteusCarr 0:765da3331ade 26 }
MatteusCarr 0:765da3331ade 27
MatteusCarr 0:765da3331ade 28 int StepperMotor::set_pps( int v ) {
MatteusCarr 0:765da3331ade 29 int p;
MatteusCarr 0:765da3331ade 30
MatteusCarr 0:765da3331ade 31 p = pps;
MatteusCarr 0:765da3331ade 32 pps = v;
MatteusCarr 0:765da3331ade 33 t.detach();
MatteusCarr 0:765da3331ade 34 t.attach( this, &StepperMotor::motor_maintain, 1.0 / (float)pps );
MatteusCarr 0:765da3331ade 35 return ( p );
MatteusCarr 0:765da3331ade 36 }
MatteusCarr 0:765da3331ade 37
MatteusCarr 0:765da3331ade 38 void StepperMotor::set_max_pps( int v ) {
MatteusCarr 0:765da3331ade 39 max_pps = v;
MatteusCarr 0:765da3331ade 40 }
MatteusCarr 0:765da3331ade 41
MatteusCarr 0:765da3331ade 42 int StepperMotor::find_home_position( PositionDetectPorarity edge ) {
MatteusCarr 0:765da3331ade 43 RotMode prev_rot;
MatteusCarr 0:765da3331ade 44 SyncMode prev_sync;
MatteusCarr 0:765da3331ade 45 int prev_pps;
MatteusCarr 0:765da3331ade 46 int prev_det_pin;
MatteusCarr 0:765da3331ade 47 int direction;
MatteusCarr 0:765da3331ade 48
MatteusCarr 0:765da3331ade 49 prev_pps = set_pps( max_pps );
MatteusCarr 0:765da3331ade 50 prev_rot = rot_mode;
MatteusCarr 0:765da3331ade 51 prev_sync = sync_mode;
MatteusCarr 0:765da3331ade 52 set_sync_mode( SYNCHRONOUS );
MatteusCarr 0:765da3331ade 53 set_rot_mode( SHORTEST );
MatteusCarr 0:765da3331ade 54
MatteusCarr 0:765da3331ade 55 prev_det_pin = position_detect_pin;
MatteusCarr 0:765da3331ade 56
MatteusCarr 0:765da3331ade 57 if ( prev_rot == COUNTER_CLOCKWISE_ONLY )
MatteusCarr 0:765da3331ade 58 direction = -1;
MatteusCarr 0:765da3331ade 59 else
MatteusCarr 0:765da3331ade 60 direction = 1;
MatteusCarr 0:765da3331ade 61
MatteusCarr 0:765da3331ade 62 if ( prev_rot == NO_WRAPAROUND ) {
MatteusCarr 0:765da3331ade 63
MatteusCarr 0:765da3331ade 64 for ( int i = 0; i < (max_pos >> 1); i++ ) {
MatteusCarr 0:765da3331ade 65 move_steps( -1 );
MatteusCarr 0:765da3331ade 66 if ( ((edge == RISING_EDGE) && !prev_det_pin && position_detect_pin)
MatteusCarr 0:765da3331ade 67 || ((edge == FALLING_EDGE) && prev_det_pin && !position_detect_pin) ) {
MatteusCarr 0:765da3331ade 68 set_home_position();
MatteusCarr 0:765da3331ade 69 init_done = true;
MatteusCarr 0:765da3331ade 70 break;
MatteusCarr 0:765da3331ade 71 }
MatteusCarr 0:765da3331ade 72 prev_det_pin = position_detect_pin;
MatteusCarr 0:765da3331ade 73 }
MatteusCarr 0:765da3331ade 74 }
MatteusCarr 0:765da3331ade 75
MatteusCarr 0:765da3331ade 76 for ( int i = 0; i < ((prev_rot == NO_WRAPAROUND) ? (max_pos - 1) : (max_pos + 10)); i++ ) {
MatteusCarr 0:765da3331ade 77 move_steps( direction );
MatteusCarr 0:765da3331ade 78 if ( ((edge == RISING_EDGE) && !prev_det_pin && position_detect_pin)
MatteusCarr 0:765da3331ade 79 || ((edge == FALLING_EDGE) && prev_det_pin && !position_detect_pin) ) {
MatteusCarr 0:765da3331ade 80 set_home_position();
MatteusCarr 0:765da3331ade 81 init_done = true;
MatteusCarr 0:765da3331ade 82 break;
MatteusCarr 0:765da3331ade 83 }
MatteusCarr 0:765da3331ade 84 prev_det_pin = position_detect_pin;
MatteusCarr 0:765da3331ade 85 }
MatteusCarr 0:765da3331ade 86
MatteusCarr 0:765da3331ade 87 go_position( 0 );
MatteusCarr 0:765da3331ade 88 set_pps( prev_pps );
MatteusCarr 0:765da3331ade 89 set_rot_mode( prev_rot );
MatteusCarr 0:765da3331ade 90 set_sync_mode( prev_sync );
MatteusCarr 0:765da3331ade 91
MatteusCarr 0:765da3331ade 92 return ( init_done );
MatteusCarr 0:765da3331ade 93 }
MatteusCarr 0:765da3331ade 94
MatteusCarr 0:765da3331ade 95 void StepperMotor::set_home_position( void ) {
MatteusCarr 0:765da3331ade 96 set_pause( true );
MatteusCarr 0:765da3331ade 97 pos_offset = current_pos & 0x3;
MatteusCarr 0:765da3331ade 98 current_pos = 0;
MatteusCarr 0:765da3331ade 99 set_target_pos( 0 );
MatteusCarr 0:765da3331ade 100 set_pause( false );
MatteusCarr 0:765da3331ade 101 }
MatteusCarr 0:765da3331ade 102
MatteusCarr 0:765da3331ade 103 int StepperMotor::go_position( int v ) {
MatteusCarr 0:765da3331ade 104 set_target_pos( v );
MatteusCarr 0:765da3331ade 105 return ( current_pos );
MatteusCarr 0:765da3331ade 106 }
MatteusCarr 0:765da3331ade 107
MatteusCarr 0:765da3331ade 108 void StepperMotor::go_angle( float angle ) {
MatteusCarr 0:765da3331ade 109 go_position( (int)((angle / 360.0) * (float)max_pos) );
MatteusCarr 0:765da3331ade 110 }
MatteusCarr 0:765da3331ade 111
MatteusCarr 0:765da3331ade 112 int StepperMotor::move_steps( int s ) {
MatteusCarr 0:765da3331ade 113 set_target_pos( current_pos + s );
MatteusCarr 0:765da3331ade 114 return ( current_pos );
MatteusCarr 0:765da3331ade 115 }
MatteusCarr 0:765da3331ade 116
MatteusCarr 0:765da3331ade 117 void StepperMotor::set_rot_mode( RotMode m ) {
MatteusCarr 0:765da3331ade 118 rot_mode = m;
MatteusCarr 0:765da3331ade 119 }
MatteusCarr 0:765da3331ade 120
MatteusCarr 0:765da3331ade 121 void StepperMotor::set_sync_mode( SyncMode m ) {
MatteusCarr 0:765da3331ade 122 sync_mode = m;
MatteusCarr 0:765da3331ade 123 }
MatteusCarr 0:765da3331ade 124
MatteusCarr 0:765da3331ade 125 int StepperMotor::distance( void ) {
MatteusCarr 0:765da3331ade 126 return( target_pos - current_pos );
MatteusCarr 0:765da3331ade 127 }
MatteusCarr 0:765da3331ade 128
MatteusCarr 0:765da3331ade 129 void StepperMotor::set_pause( int sw ) {
MatteusCarr 0:765da3331ade 130 pause = sw;
MatteusCarr 0:765da3331ade 131 }
MatteusCarr 0:765da3331ade 132
MatteusCarr 0:765da3331ade 133 void StepperMotor::set_target_pos( int p ) {
MatteusCarr 0:765da3331ade 134 target_pos = (p + max_pos) % max_pos;
MatteusCarr 0:765da3331ade 135
MatteusCarr 0:765da3331ade 136 if (sync_mode == SYNCHRONOUS)
MatteusCarr 0:765da3331ade 137 while ( distance() )
MatteusCarr 0:765da3331ade 138 wait( 0 );
MatteusCarr 0:765da3331ade 139 }
MatteusCarr 0:765da3331ade 140
MatteusCarr 0:765da3331ade 141 void StepperMotor::set_power_ctrl( int sw ) {
MatteusCarr 0:765da3331ade 142 power_ctrl = sw;
MatteusCarr 0:765da3331ade 143 }
MatteusCarr 0:765da3331ade 144
MatteusCarr 0:765da3331ade 145 void StepperMotor::set_steps_per_rotate( int steps ) {
MatteusCarr 0:765da3331ade 146 max_pos = steps;
MatteusCarr 0:765da3331ade 147 }
MatteusCarr 0:765da3331ade 148
MatteusCarr 0:765da3331ade 149 void StepperMotor::motor_maintain( void ) {
MatteusCarr 0:765da3331ade 150
MatteusCarr 0:765da3331ade 151 int diff;
MatteusCarr 0:765da3331ade 152 int direction;
MatteusCarr 0:765da3331ade 153
MatteusCarr 0:765da3331ade 154 if ( pause )
MatteusCarr 0:765da3331ade 155 return;
MatteusCarr 0:765da3331ade 156
MatteusCarr 0:765da3331ade 157 diff = target_pos - current_pos;
MatteusCarr 0:765da3331ade 158
MatteusCarr 0:765da3331ade 159 if ( !diff ) {
MatteusCarr 0:765da3331ade 160 pwr_out = power_ctrl ? 0 : 1;
MatteusCarr 0:765da3331ade 161 return;
MatteusCarr 0:765da3331ade 162 }
MatteusCarr 0:765da3331ade 163
MatteusCarr 0:765da3331ade 164 pwr_out = 1;
MatteusCarr 0:765da3331ade 165
MatteusCarr 0:765da3331ade 166 diff = (diff + max_pos) % max_pos;
MatteusCarr 0:765da3331ade 167
MatteusCarr 0:765da3331ade 168 if ( diff > (max_pos >> 1) )
MatteusCarr 0:765da3331ade 169 diff -= max_pos;
MatteusCarr 0:765da3331ade 170
MatteusCarr 0:765da3331ade 171 switch ( rot_mode ) {
MatteusCarr 0:765da3331ade 172 case NO_WRAPAROUND :
MatteusCarr 0:765da3331ade 173 direction = ( (target_pos - current_pos) < 0 ) ? -1 : 1;
MatteusCarr 0:765da3331ade 174 break;
MatteusCarr 0:765da3331ade 175 case CLOCKWISE_ONLY :
MatteusCarr 0:765da3331ade 176 direction = 1;
MatteusCarr 0:765da3331ade 177 break;
MatteusCarr 0:765da3331ade 178 case COUNTER_CLOCKWISE_ONLY :
MatteusCarr 0:765da3331ade 179 direction = -1;
MatteusCarr 0:765da3331ade 180 break;
MatteusCarr 0:765da3331ade 181 default : // SHORTEST :
MatteusCarr 0:765da3331ade 182 direction = ( diff < 0 ) ? -1 : 1;
MatteusCarr 0:765da3331ade 183 break;
MatteusCarr 0:765da3331ade 184 }
MatteusCarr 0:765da3331ade 185
MatteusCarr 0:765da3331ade 186
MatteusCarr 0:765da3331ade 187 current_pos = ((current_pos + max_pos) + direction) % max_pos;
MatteusCarr 0:765da3331ade 188
MatteusCarr 0:765da3331ade 189 // printf( "pos=%d ofst=%d puls=%d\r\n", current_pos, pos_offset, (current_pos + pos_offset) );
MatteusCarr 0:765da3331ade 190 motor_out = pat[ (current_pos + pos_offset) & 0x3 ];
MatteusCarr 0:765da3331ade 191 };
MatteusCarr 0:765da3331ade 192
MatteusCarr 0:765da3331ade 193 const unsigned char StepperMotor::pat[ 4 ] = { 0, 1, 3, 2 };