Sho Yama / LV8548

LV8548 Motor Driver Stepper Motor Dc MOtor

Dependents:   LV8548_ON_MD_Modlle_kit_DCMtr_And_STEPMtr

LV8548.cpp@2:627825272d30, 2018-11-19 (annotated)

Committer:
yamasho
Date:
Mon Nov 19 01:52:22 2018 +0000
Revision:
2:627825272d30
Parent:
1:e60c7c42e6fc 
bug Fix;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yamasho 0:04db82da014d 1 /**
yamasho 0:04db82da014d 2 [LV8548.cpp]
yamasho 0:04db82da014d 3 Copyright (c) [2018] [YAMASHO]
yamasho 0:04db82da014d 4 This software is released under the MIT License.
yamasho 0:04db82da014d 5 http://opensource.org/licenses/mit-license.php
yamasho 0:04db82da014d 6 */
yamasho 0:04db82da014d 7
yamasho 0:04db82da014d 8 #include "LV8548.h"
yamasho 0:04db82da014d 9 #include "mbed.h"
yamasho 0:04db82da014d 10
yamasho 2:627825272d30 11 #define _DEBUG_ (0)
yamasho 0:04db82da014d 12
yamasho 0:04db82da014d 13 #define COIL_A (0X1)
yamasho 0:04db82da014d 14 #define COIL_B (0X2)
yamasho 0:04db82da014d 15 #define COIL_C (0X4)
yamasho 0:04db82da014d 16 #define COIL_D (0X8)
yamasho 0:04db82da014d 17
yamasho 0:04db82da014d 18
yamasho 0:04db82da014d 19
yamasho 0:04db82da014d 20 const uint8_t FullStepDrivePattern[STEPPHASEMAX] =
yamasho 0:04db82da014d 21 #if 0
yamasho 0:04db82da014d 22 {(COIL_A | COIL_D),
yamasho 0:04db82da014d 23 (COIL_A | COIL_D),
yamasho 0:04db82da014d 24 (COIL_A | COIL_B),
yamasho 0:04db82da014d 25 (COIL_A | COIL_B),
yamasho 0:04db82da014d 26 (COIL_B | COIL_C),
yamasho 0:04db82da014d 27 (COIL_B | COIL_C),
yamasho 0:04db82da014d 28 (COIL_C | COIL_D),
yamasho 0:04db82da014d 29 (COIL_C | COIL_D)};
yamasho 0:04db82da014d 30 #else
yamasho 0:04db82da014d 31 /// orgin
yamasho 0:04db82da014d 32 {(COIL_A | COIL_C),
yamasho 0:04db82da014d 33 (COIL_A | COIL_C),
yamasho 0:04db82da014d 34 (COIL_B | COIL_C),
yamasho 0:04db82da014d 35 (COIL_B | COIL_C),
yamasho 0:04db82da014d 36 (COIL_B | COIL_D),
yamasho 0:04db82da014d 37 (COIL_B | COIL_D),
yamasho 0:04db82da014d 38 (COIL_A | COIL_D),
yamasho 0:04db82da014d 39 (COIL_A | COIL_D)};
yamasho 0:04db82da014d 40
yamasho 0:04db82da014d 41 #endif
yamasho 0:04db82da014d 42 const uint8_t HaleStepDrivePattern[STEPPHASEMAX ] =
yamasho 0:04db82da014d 43 #if 0
yamasho 0:04db82da014d 44 {(COIL_A | COIL_D),
yamasho 0:04db82da014d 45 (COIL_A),
yamasho 0:04db82da014d 46 (COIL_A | COIL_B),
yamasho 0:04db82da014d 47 (COIL_B),
yamasho 0:04db82da014d 48 (COIL_B | COIL_C),
yamasho 0:04db82da014d 49 (COIL_C),
yamasho 0:04db82da014d 50 (COIL_C | COIL_D),
yamasho 0:04db82da014d 51 (COIL_D)};
yamasho 0:04db82da014d 52 #else
yamasho 0:04db82da014d 53 /// orgin
yamasho 0:04db82da014d 54 {(COIL_A),
yamasho 0:04db82da014d 55 (COIL_A | COIL_C),
yamasho 0:04db82da014d 56 (COIL_C),
yamasho 0:04db82da014d 57 (COIL_B | COIL_C),
yamasho 0:04db82da014d 58 (COIL_B),
yamasho 0:04db82da014d 59 (COIL_B | COIL_D),
yamasho 0:04db82da014d 60 (COIL_D),
yamasho 0:04db82da014d 61 (COIL_A | COIL_D)};
yamasho 0:04db82da014d 62
yamasho 0:04db82da014d 63 #endif
yamasho 2:627825272d30 64
yamasho 0:04db82da014d 65
yamasho 0:04db82da014d 66
yamasho 0:04db82da014d 67 LV8548::LV8548(PinName in1, PinName in2, PinName in3, PinName in4, DriverType drivertype,uint16_t baseus):
yamasho 0:04db82da014d 68 _in1(in1),_in2(in2),_in3(in3),_in4(in4),_Drivertype(drivertype),_baseus(baseus) {
yamasho 0:04db82da014d 69
yamasho 0:04db82da014d 70 _in1 = 0;
yamasho 0:04db82da014d 71 _in2 = 0;
yamasho 0:04db82da014d 72 _in3 = 0;
yamasho 0:04db82da014d 73 _in4 = 0;
yamasho 0:04db82da014d 74
yamasho 0:04db82da014d 75 _Pwmmode[MOTOR_A] = FWD_OPEN;
yamasho 0:04db82da014d 76 _Pwmmode[MOTOR_B] = FWD_OPEN;
yamasho 0:04db82da014d 77
yamasho 0:04db82da014d 78 _Pwmout[MOTOR_A] = OUTPUT_OFF;
yamasho 0:04db82da014d 79 _Pwmout[MOTOR_B] = OUTPUT_OFF;
yamasho 0:04db82da014d 80
yamasho 0:04db82da014d 81 _PwmPeriod[MOTOR_A] = 0.02f; // 20ms
yamasho 0:04db82da014d 82 _PwmPeriod[MOTOR_B] = 0.02f; // 20ms
yamasho 0:04db82da014d 83
yamasho 0:04db82da014d 84 _PwmDuty[MOTOR_A] = 1.0f; // Duyt 100%
yamasho 0:04db82da014d 85 _PwmDuty[MOTOR_B] = 1.0f; // Duyt 100%
yamasho 0:04db82da014d 86
yamasho 0:04db82da014d 87 if(_Drivertype == DCMOTOR ) return; // DC MotorにはTimerはない。
yamasho 0:04db82da014d 88 if(_baseus == 0) return; // 0us のタイマも無し.
yamasho 0:04db82da014d 89
yamasho 0:04db82da014d 90 float TimeVal = (float)_baseus / 1000000;
yamasho 0:04db82da014d 91 #if _DEBUG_
yamasho 0:04db82da014d 92 printf("TimerSetting %f",TimeVal);
yamasho 0:04db82da014d 93 #endif
yamasho 0:04db82da014d 94 _Lv8548BaseTimer.attach(this,&LV8548::SetStepTimerInt, TimeVal );
yamasho 0:04db82da014d 95 }
yamasho 0:04db82da014d 96
yamasho 0:04db82da014d 97 void LV8548::SetDcMotorStop(uint8_t ch)
yamasho 0:04db82da014d 98 {
yamasho 0:04db82da014d 99 if( _Drivertype != DCMOTOR ) return;
yamasho 0:04db82da014d 100 if(ch == MOTOR_A)
yamasho 0:04db82da014d 101 {
yamasho 0:04db82da014d 102 _in1.write(0.0f);
yamasho 0:04db82da014d 103 _in2.write(0.0f);
yamasho 0:04db82da014d 104 }
yamasho 0:04db82da014d 105 else
yamasho 0:04db82da014d 106 {
yamasho 0:04db82da014d 107 _in3.write(0.0f);
yamasho 0:04db82da014d 108 _in4.write(0.0f);
yamasho 0:04db82da014d 109 }
yamasho 0:04db82da014d 110 }
yamasho 0:04db82da014d 111
yamasho 0:04db82da014d 112 void LV8548::SetDcMotor(uint8_t ch)
yamasho 0:04db82da014d 113 {
yamasho 0:04db82da014d 114 if( _Drivertype != DCMOTOR ) return;
yamasho 0:04db82da014d 115 #if _DEBUG_
yamasho 0:04db82da014d 116 printf("Motor Set %2x %2x ",ch ,_Pwmout[ch]);
yamasho 0:04db82da014d 117 #endif
yamasho 0:04db82da014d 118 switch( _Pwmout[ch] )
yamasho 0:04db82da014d 119 {
yamasho 0:04db82da014d 120 default: /* OUTPUT_OFF*/
yamasho 0:04db82da014d 121 if(ch == MOTOR_A)
yamasho 0:04db82da014d 122 {
yamasho 0:04db82da014d 123 _in1.write(0.0f);
yamasho 0:04db82da014d 124 _in2.write(0.0f);
yamasho 0:04db82da014d 125 #if _DEBUG_
yamasho 0:04db82da014d 126 printf("1:0.0 2:0.0 ");
yamasho 0:04db82da014d 127 #endif
yamasho 0:04db82da014d 128 }
yamasho 0:04db82da014d 129 else
yamasho 0:04db82da014d 130 if(ch == MOTOR_B)
yamasho 0:04db82da014d 131 {
yamasho 0:04db82da014d 132 _in3.write(0.0f);
yamasho 0:04db82da014d 133 _in4.write(0.0f);
yamasho 0:04db82da014d 134 #if _DEBUG_
yamasho 0:04db82da014d 135 printf("3:0.0 4:0.0 ");
yamasho 0:04db82da014d 136 #endif
yamasho 0:04db82da014d 137 }
yamasho 0:04db82da014d 138 break;
yamasho 0:04db82da014d 139 case OUTPUT_BRAKE:
yamasho 0:04db82da014d 140 if(ch == MOTOR_A)
yamasho 0:04db82da014d 141 {
yamasho 0:04db82da014d 142 _in1.write(1.0f);
yamasho 0:04db82da014d 143 _in2.write(1.0f);
yamasho 0:04db82da014d 144 #if _DEBUG_
yamasho 0:04db82da014d 145 printf("1:1.0 2:1.0 ");
yamasho 0:04db82da014d 146 #endif
yamasho 0:04db82da014d 147 }
yamasho 0:04db82da014d 148 else
yamasho 0:04db82da014d 149 if(ch == MOTOR_B)
yamasho 0:04db82da014d 150 {
yamasho 0:04db82da014d 151 _in3.write(1.0f);
yamasho 0:04db82da014d 152 _in4.write(1.0f);
yamasho 0:04db82da014d 153 #if _DEBUG_
yamasho 0:04db82da014d 154 printf("3:1.0 4:1.0 ");
yamasho 0:04db82da014d 155 #endif
yamasho 0:04db82da014d 156 }
yamasho 0:04db82da014d 157 break;
yamasho 0:04db82da014d 158 case OUTPUT_START:
yamasho 0:04db82da014d 159 if(ch == MOTOR_A)
yamasho 0:04db82da014d 160 {
yamasho 0:04db82da014d 161 switch(_Pwmmode[MOTOR_A])
yamasho 0:04db82da014d 162 {
yamasho 0:04db82da014d 163 case FWD_BRAKE: /** FWD_BREAKE */
yamasho 0:04db82da014d 164 _in1.write(1.0f);
yamasho 0:04db82da014d 165 _in2.write(1.0f-_PwmDuty[MOTOR_A]);
yamasho 0:04db82da014d 166 #if _DEBUG_
yamasho 0:04db82da014d 167 printf("1:%f 2:1.0 ",_PwmDuty[MOTOR_A]);
yamasho 0:04db82da014d 168 #endif
yamasho 0:04db82da014d 169 break;
yamasho 0:04db82da014d 170 case RVS_OPEN: /** REVERS OPEN */
yamasho 0:04db82da014d 171 _in1.write(0.0f);
yamasho 0:04db82da014d 172 _in2.write(_PwmDuty[MOTOR_A]);
yamasho 0:04db82da014d 173 #if _DEBUG_
yamasho 0:04db82da014d 174 printf("1:0.0 2:%f ",_PwmDuty[MOTOR_A]);
yamasho 0:04db82da014d 175 #endif
yamasho 0:04db82da014d 176 break;
yamasho 0:04db82da014d 177 case RVS_BRAKE: /** REVSRS BRAKE */
yamasho 0:04db82da014d 178 _in1.write(1.0f-_PwmDuty[MOTOR_A]);
yamasho 0:04db82da014d 179 _in2.write(1.0f);
yamasho 0:04db82da014d 180 #if _DEBUG_
yamasho 0:04db82da014d 181 printf("1:1.0 2:%f ",_PwmDuty[MOTOR_A]);
yamasho 0:04db82da014d 182 #endif
yamasho 0:04db82da014d 183 break;
yamasho 0:04db82da014d 184 default: /* FWD_OPEN */
yamasho 0:04db82da014d 185 _in1.write(_PwmDuty[MOTOR_A]);
yamasho 0:04db82da014d 186 _in2.write(0.0f);
yamasho 0:04db82da014d 187 #if _DEBUG_
yamasho 0:04db82da014d 188 printf("1:%f 2:0.0 ",_PwmDuty[MOTOR_A]);
yamasho 0:04db82da014d 189 #endif
yamasho 0:04db82da014d 190 break;
yamasho 0:04db82da014d 191 }
yamasho 0:04db82da014d 192 }
yamasho 0:04db82da014d 193 else
yamasho 0:04db82da014d 194 if(ch == MOTOR_B)
yamasho 0:04db82da014d 195 {
yamasho 0:04db82da014d 196 switch(_Pwmmode[MOTOR_B])
yamasho 0:04db82da014d 197 {
yamasho 0:04db82da014d 198 case FWD_BRAKE: /** FWD_BREAKE */
yamasho 0:04db82da014d 199 _in3.write(1.0f);
yamasho 0:04db82da014d 200 _in4.write(1.0f-_PwmDuty[MOTOR_B]);
yamasho 0:04db82da014d 201 #if _DEBUG_
yamasho 0:04db82da014d 202 printf("3:%f 4:1.0 ",_PwmDuty[MOTOR_B]);
yamasho 0:04db82da014d 203 #endif
yamasho 0:04db82da014d 204 break;
yamasho 0:04db82da014d 205 case RVS_OPEN: /** REVERS OPEN */
yamasho 0:04db82da014d 206 _in3.write(0.0f);
yamasho 0:04db82da014d 207 _in4.write(_PwmDuty[MOTOR_B]);
yamasho 0:04db82da014d 208 #if _DEBUG_
yamasho 0:04db82da014d 209 printf("3:0.0 4:%f",_PwmDuty[MOTOR_B]);
yamasho 0:04db82da014d 210 #endif
yamasho 0:04db82da014d 211 break;
yamasho 0:04db82da014d 212 case RVS_BRAKE: /** REVSRS BRAKE */
yamasho 0:04db82da014d 213 _in3.write(1.0f-_PwmDuty[MOTOR_B]);
yamasho 0:04db82da014d 214 _in4.write(1.0f);
yamasho 0:04db82da014d 215 #if _DEBUG_
yamasho 0:04db82da014d 216 printf("3:1.0 4:%f ",_PwmDuty[MOTOR_B]);
yamasho 0:04db82da014d 217 #endif
yamasho 0:04db82da014d 218 break;
yamasho 0:04db82da014d 219 default: /*FWD_OPEN FWD_OPEN */
yamasho 0:04db82da014d 220 _in3.write(_PwmDuty[MOTOR_B]);
yamasho 0:04db82da014d 221 _in4.write(0.0f);
yamasho 0:04db82da014d 222 #if _DEBUG_
yamasho 0:04db82da014d 223 printf("3:%f 4:0.0 ",_PwmDuty[1]);
yamasho 0:04db82da014d 224 #endif
yamasho 0:04db82da014d 225 break;
yamasho 0:04db82da014d 226 }
yamasho 0:04db82da014d 227 }
yamasho 0:04db82da014d 228 break;
yamasho 0:04db82da014d 229 }
yamasho 0:04db82da014d 230 #if _DEBUG_
yamasho 0:04db82da014d 231 printf("\n");
yamasho 0:04db82da014d 232 #endif
yamasho 0:04db82da014d 233
yamasho 0:04db82da014d 234 }
yamasho 0:04db82da014d 235 /***************************************************************************************************/
yamasho 0:04db82da014d 236 /* FFFFFF DDDD MM MM */
yamasho 0:04db82da014d 237 /* FF DD DD MMM MMM tt */
yamasho 0:04db82da014d 238 /* FF oooo rrrrr DD DD cccc MMMMMMM oooo tttttt oooo rrrrr */
yamasho 0:04db82da014d 239 /* FFFF oo oo rr rr DD DD cc MM M MM oo oo tt oo oo rr rr */
yamasho 0:04db82da014d 240 /* FF oo oo rr DD DD cc MM MM oo oo tt oo oo rr */
yamasho 0:04db82da014d 241 /* FF oo oo rr DD DD cc MM MM oo oo tt oo oo rr */
yamasho 0:04db82da014d 242 /* FF oooo rr DDDD cccc MM MM oooo ttt oooo rr */
yamasho 0:04db82da014d 243 /* */
yamasho 0:04db82da014d 244 /***************************************************************************************************/
yamasho 0:04db82da014d 245 void LV8548::SetDcPwmFreqency( uint8_t ch ,float Freq )
yamasho 0:04db82da014d 246 {
yamasho 0:04db82da014d 247 if( _Drivertype != DCMOTOR ) return;
yamasho 0:04db82da014d 248
yamasho 0:04db82da014d 249 if(ch == MOTOR_A)
yamasho 0:04db82da014d 250 {
yamasho 0:04db82da014d 251 SetDcMotorStop(MOTOR_A);
yamasho 0:04db82da014d 252 _PwmPeriod[MOTOR_A] = 1/Freq;
yamasho 1:e60c7c42e6fc 253 #if USE_PWM_PORT
yamasho 0:04db82da014d 254 _in1.period( _PwmPeriod[MOTOR_A] );
yamasho 0:04db82da014d 255 _in2.period( _PwmPeriod[MOTOR_A] );
yamasho 1:e60c7c42e6fc 256 #endif
yamasho 0:04db82da014d 257 SetDcMotor(MOTOR_A);
yamasho 0:04db82da014d 258 #if _DEBUG_
yamasho 0:04db82da014d 259 printf("freq0 : %f ",_PwmPeriod[MOTOR_A]);
yamasho 0:04db82da014d 260 #endif
yamasho 0:04db82da014d 261 }else
yamasho 0:04db82da014d 262 if(ch == MOTOR_B)
yamasho 0:04db82da014d 263 {
yamasho 0:04db82da014d 264 SetDcMotorStop(MOTOR_B);
yamasho 0:04db82da014d 265 _PwmPeriod[MOTOR_B] = 1/Freq;
yamasho 1:e60c7c42e6fc 266 #if USE_PWM_PORT
yamasho 1:e60c7c42e6fc 267 _in3.period( _PwmPeriod[MOTOR_B] );
yamasho 1:e60c7c42e6fc 268 _in4.period( _PwmPeriod[MOTOR_B] );
yamasho 1:e60c7c42e6fc 269 #endif
yamasho 0:04db82da014d 270 #if _DEBUG_
yamasho 0:04db82da014d 271 printf("freq1 : %f ",_PwmPeriod[MOTOR_B]);
yamasho 0:04db82da014d 272 #endif
yamasho 0:04db82da014d 273 SetDcMotor(MOTOR_B);
yamasho 0:04db82da014d 274 }
yamasho 0:04db82da014d 275
yamasho 0:04db82da014d 276 }
yamasho 0:04db82da014d 277
yamasho 0:04db82da014d 278 void LV8548::SetDcPwmMode( uint8_t ch ,DriverPwmMode Mode )
yamasho 0:04db82da014d 279 {
yamasho 0:04db82da014d 280 if(ch < MOTOR_MAX)
yamasho 0:04db82da014d 281 {
yamasho 0:04db82da014d 282 #if _DEBUG_
yamasho 0:04db82da014d 283 printf("DC Pwm Set\n");
yamasho 0:04db82da014d 284 #endif
yamasho 0:04db82da014d 285
yamasho 0:04db82da014d 286 _Pwmmode[ch] = Mode;
yamasho 0:04db82da014d 287 SetDcMotor(ch);
yamasho 0:04db82da014d 288 }
yamasho 0:04db82da014d 289 }
yamasho 0:04db82da014d 290
yamasho 0:04db82da014d 291 void LV8548::SetDcPwmDuty( uint8_t ch ,float Duty )
yamasho 0:04db82da014d 292 {
yamasho 0:04db82da014d 293 if(ch < MOTOR_MAX)
yamasho 0:04db82da014d 294 {
yamasho 0:04db82da014d 295 #if _DEBUG_
yamasho 0:04db82da014d 296 printf("DC Duty Set\n");
yamasho 0:04db82da014d 297 #endif
yamasho 0:04db82da014d 298 _PwmDuty[ch] = Duty;
yamasho 0:04db82da014d 299 SetDcMotor(ch);
yamasho 0:04db82da014d 300 }
yamasho 0:04db82da014d 301 }
yamasho 0:04db82da014d 302
yamasho 0:04db82da014d 303
yamasho 0:04db82da014d 304 void LV8548::SetDcOutPut( uint8_t ch ,DriverPwmOut Mode )
yamasho 0:04db82da014d 305 {
yamasho 0:04db82da014d 306 if(ch < MOTOR_MAX)
yamasho 0:04db82da014d 307 {
yamasho 0:04db82da014d 308 #if _DEBUG_
yamasho 0:04db82da014d 309 printf("DC Out Set\n");
yamasho 0:04db82da014d 310 #endif
yamasho 0:04db82da014d 311 _Pwmout[ch] = Mode;
yamasho 0:04db82da014d 312 SetDcMotor(ch);
yamasho 0:04db82da014d 313 }
yamasho 0:04db82da014d 314 }
yamasho 0:04db82da014d 315
yamasho 0:04db82da014d 316 /*******************************************************************************************************************/
yamasho 0:04db82da014d 317 /* FFFFFF SSSS MM MM */
yamasho 0:04db82da014d 318 /* FF SS SS tt MMM MMM tt */
yamasho 0:04db82da014d 319 /* FF oooo rrrrr SS tttttt eeee ppppp MMMMMMM oooo tttttt oooo rrrrr */
yamasho 0:04db82da014d 320 /* FFFF oo oo rr rr SSSS tt ee ee pp pp MM M MM oo oo tt oo oo rr rr */
yamasho 0:04db82da014d 321 /* FF oo oo rr SS tt eeeeee pp pp MM MM oo oo tt oo oo rr */
yamasho 0:04db82da014d 322 /* FF oo oo rr SS SS tt ee ppppp MM MM oo oo tt oo oo rr */
yamasho 0:04db82da014d 323 /* FF oooo rr SSSS ttt eeee pp MM MM oooo ttt oooo rr */
yamasho 0:04db82da014d 324 /* pp */
yamasho 0:04db82da014d 325 /*******************************************************************************************************************/
yamasho 0:04db82da014d 326 void LV8548::SetStepAngle( uint16_t Angle)
yamasho 0:04db82da014d 327 {
yamasho 0:04db82da014d 328 _StepDeg = Angle;
yamasho 0:04db82da014d 329 #if _DEBUG_
yamasho 0:04db82da014d 330 printf("Set Angle Deg:%f \n",_StepDeg);
yamasho 0:04db82da014d 331 #endif
yamasho 0:04db82da014d 332 }
yamasho 0:04db82da014d 333 /// 周波数1～4800
yamasho 0:04db82da014d 334 /// DEG 0～65535
yamasho 0:04db82da014d 335 /// rotation 0:CW 1:CCW
yamasho 0:04db82da014d 336 /// EXP 0:FullStep 1:HalfStep
yamasho 0:04db82da014d 337 void LV8548::SetStepDeg ( uint16_t frequency , uint16_t deg , uint8_t Direction, uint8_t StepMode)
yamasho 0:04db82da014d 338 {
yamasho 0:04db82da014d 339 #if _DEBUG_
yamasho 0:04db82da014d 340 printf("Deg Commnad freq:%d deg:%d dir:%d Mode%d\n",frequency , deg , Direction, StepMode);
yamasho 0:04db82da014d 341 #endif
yamasho 0:04db82da014d 342
yamasho 0:04db82da014d 343 if ( frequency == 0 ) return;
yamasho 0:04db82da014d 344
yamasho 0:04db82da014d 345 if( frequency > STEPMAX_FREQ )
yamasho 0:04db82da014d 346 {
yamasho 0:04db82da014d 347 frequency = STEPMAX_FREQ;
yamasho 0:04db82da014d 348 }
yamasho 0:04db82da014d 349
yamasho 0:04db82da014d 350 SetStepStop(); /* Step Motor 初期化　*/
yamasho 0:04db82da014d 351
yamasho 0:04db82da014d 352 _TragetStepDirection = (DriverMotorDirction )Direction; // CW/CCW
yamasho 0:04db82da014d 353 _TargetStepMode = (DriverStepMode )StepMode; // Full/Half;
yamasho 0:04db82da014d 354 if(_TargetStepMode != _NowStepMode)
yamasho 0:04db82da014d 355 { /* 動作モード変更の為初期化 */
yamasho 0:04db82da014d 356 _StepPhase = 0;
yamasho 0:04db82da014d 357 #if _DEBUG_
yamasho 0:04db82da014d 358 printf("Stepper Mode Change Then StepPhase 0\n");
yamasho 0:04db82da014d 359 #endif
yamasho 0:04db82da014d 360 }
yamasho 0:04db82da014d 361 #if 0 // 意味の無い処理なのでコメント化
yamasho 0:04db82da014d 362 if( _TragetStepDirection != _NowStepDirection )
yamasho 0:04db82da014d 363 { // 方向の変更があった時 ///
yamasho 0:04db82da014d 364 if( _TragetStepDirection == DIR_CW )
yamasho 0:04db82da014d 365 {
yamasho 0:04db82da014d 366 _StepPhase++; //なぜ2パターン？？
yamasho 0:04db82da014d 367 _StepPhase++; // 元のソースは2パターン戻すが、必要なもないので削除
yamasho 0:04db82da014d 368 }
yamasho 0:04db82da014d 369 else
yamasho 0:04db82da014d 370 if( _TragetStepDirection == DIR_CCW )
yamasho 0:04db82da014d 371 {
yamasho 0:04db82da014d 372 _StepPhase--; //なぜ2パターン？？
yamasho 0:04db82da014d 373 _StepPhase--; // 元のソースは2パターン戻すが、必要なもないので削除
yamasho 0:04db82da014d 374 }
yamasho 0:04db82da014d 375 _StepPhase &= STEPPHASEMAX; // 0-7に！
yamasho 0:04db82da014d 376 }
yamasho 0:04db82da014d 377 #endif
yamasho 0:04db82da014d 378 _StepFreq = frequency ;
yamasho 0:04db82da014d 379 if(_StepDeg)
yamasho 0:04db82da014d 380 {
yamasho 0:04db82da014d 381 _TargetStep = ( deg / _StepDeg);
yamasho 0:04db82da014d 382 if(_TargetStepMode)
yamasho 0:04db82da014d 383 { /* HALF時2倍 */
yamasho 0:04db82da014d 384 _TargetStep = _TargetStep + _TargetStep;
yamasho 0:04db82da014d 385 }
yamasho 0:04db82da014d 386 }
yamasho 0:04db82da014d 387 else
yamasho 0:04db82da014d 388 {
yamasho 0:04db82da014d 389 _TargetStep = 0;
yamasho 0:04db82da014d 390 }
yamasho 0:04db82da014d 391
yamasho 0:04db82da014d 392 #if _DEBUG_
yamasho 0:04db82da014d 393 printf("Analyze StepFreq:%f TargetStep:%d StepPhase:%d \n",_StepFreq ,_TargetStep,_StepPhase);
yamasho 0:04db82da014d 394 #endif
yamasho 0:04db82da014d 395 SetStepMotor();
yamasho 0:04db82da014d 396 }
yamasho 0:04db82da014d 397
yamasho 0:04db82da014d 398 void LV8548::SetStepTime ( uint16_t frequency , uint16_t time , uint8_t Direction, uint8_t StepMode)
yamasho 0:04db82da014d 399 {
yamasho 0:04db82da014d 400 #if _DEBUG_
yamasho 0:04db82da014d 401 printf("Time Commnad freq:%d Time:%d dir:%d Mode%d\n",frequency ,time, Direction, StepMode);
yamasho 0:04db82da014d 402 #endif
yamasho 0:04db82da014d 403 if ( frequency == 0 ) return;
yamasho 0:04db82da014d 404
yamasho 0:04db82da014d 405 if( frequency > STEPMAX_FREQ )
yamasho 0:04db82da014d 406 {
yamasho 0:04db82da014d 407 frequency = STEPMAX_FREQ;
yamasho 0:04db82da014d 408 }
yamasho 0:04db82da014d 409 SetStepStop(); /* Step Motor 初期化　*/
yamasho 0:04db82da014d 410
yamasho 0:04db82da014d 411 _TragetStepDirection = (DriverMotorDirction )Direction;
yamasho 0:04db82da014d 412 _TargetStepMode = (DriverStepMode )StepMode; // Full/Half;
yamasho 0:04db82da014d 413
yamasho 0:04db82da014d 414 if(_TargetStepMode != _NowStepMode)
yamasho 0:04db82da014d 415 { /* 動作モード変更の為初期化 */
yamasho 0:04db82da014d 416 _StepPhase = 0;
yamasho 0:04db82da014d 417 #if _DEBUG_
yamasho 0:04db82da014d 418 printf("Stepper Mode Change Then StepPhase 0\n");
yamasho 0:04db82da014d 419 #endif
yamasho 0:04db82da014d 420 }
yamasho 0:04db82da014d 421
yamasho 0:04db82da014d 422 _StepFreq = frequency ;
yamasho 0:04db82da014d 423 _TargetStep = ( time * frequency );
yamasho 0:04db82da014d 424 #if _DEBUG_
yamasho 0:04db82da014d 425 printf("Analyze StepFreq:%f TargetStep:%d StepPhase:%d \n",_StepFreq ,_TargetStep,_StepPhase);
yamasho 0:04db82da014d 426 #endif
yamasho 0:04db82da014d 427 SetStepMotor();
yamasho 0:04db82da014d 428 }
yamasho 0:04db82da014d 429
yamasho 0:04db82da014d 430 void LV8548::SetStepStep ( uint16_t frequency , uint16_t step , uint8_t Direction, uint8_t StepMode)
yamasho 0:04db82da014d 431 {
yamasho 0:04db82da014d 432 #if _DEBUG_
yamasho 0:04db82da014d 433 printf("Step Commnad freq:%d step:%d dir:%d Mode%d\n",frequency , step , Direction, StepMode);
yamasho 0:04db82da014d 434 #endif
yamasho 0:04db82da014d 435 if ( frequency == 0 ) return;
yamasho 0:04db82da014d 436
yamasho 0:04db82da014d 437 if( frequency > STEPMAX_FREQ )
yamasho 0:04db82da014d 438 {
yamasho 0:04db82da014d 439 frequency = STEPMAX_FREQ;
yamasho 0:04db82da014d 440 }
yamasho 0:04db82da014d 441 SetStepStop(); /* Step Motor 初期化　*/
yamasho 0:04db82da014d 442
yamasho 0:04db82da014d 443 _TragetStepDirection = (DriverMotorDirction )Direction;
yamasho 0:04db82da014d 444 _TargetStepMode = (DriverStepMode )StepMode; // Full/Half;
yamasho 0:04db82da014d 445
yamasho 0:04db82da014d 446 if(_TargetStepMode != _NowStepMode)
yamasho 0:04db82da014d 447 { /* 動作モード変更の為初期化 */
yamasho 0:04db82da014d 448 _StepPhase = 0;
yamasho 0:04db82da014d 449 #if _DEBUG_
yamasho 0:04db82da014d 450 printf("Stepper Mode Change Then StepPhase 0\n");
yamasho 0:04db82da014d 451 #endif
yamasho 0:04db82da014d 452 }
yamasho 0:04db82da014d 453 #if 0
yamasho 0:04db82da014d 454 if( _TragetStepDirection != _NowStepDirection )
yamasho 0:04db82da014d 455 { // 方向の変更があった時 ///
yamasho 0:04db82da014d 456 if( _TragetStepDirection == DIR_CW )
yamasho 0:04db82da014d 457 {
yamasho 0:04db82da014d 458 _StepPhase++; //なぜ2パターン？？
yamasho 0:04db82da014d 459 _StepPhase++;
yamasho 0:04db82da014d 460 }
yamasho 0:04db82da014d 461 else
yamasho 0:04db82da014d 462 if( _TragetStepDirection == DIR_CCW )
yamasho 0:04db82da014d 463 {
yamasho 0:04db82da014d 464 _StepPhase--; //なぜ2パターン？？
yamasho 0:04db82da014d 465 _StepPhase--;
yamasho 0:04db82da014d 466 }
yamasho 0:04db82da014d 467 _StepPhase &= STEPPHASEMAX; // 0-7に！
yamasho 0:04db82da014d 468 }
yamasho 0:04db82da014d 469 #endif
yamasho 0:04db82da014d 470 _StepFreq = frequency;
yamasho 0:04db82da014d 471 _TargetStep = step;
yamasho 0:04db82da014d 472 #if _DEBUG_
yamasho 0:04db82da014d 473 printf("Analyze StepFreq:%f %f TargetStep:%d StepPhase:%d \n",_StepFreq ,_TargetStep,_StepPhase);
yamasho 0:04db82da014d 474 #endif
yamasho 0:04db82da014d 475 SetStepMotor();
yamasho 0:04db82da014d 476 }
yamasho 0:04db82da014d 477 void LV8548::SetStepStop ( void )
yamasho 0:04db82da014d 478 {
yamasho 0:04db82da014d 479 if( _Drivertype != STEPERMOTOR ) return;
yamasho 0:04db82da014d 480 /* 割込内の終了処理と同じ */
yamasho 0:04db82da014d 481 _StepOperation = false;
yamasho 0:04db82da014d 482 _TimerCounter = 0.0f;
yamasho 0:04db82da014d 483 _NowStep = 0;
yamasho 0:04db82da014d 484 #if _DEBUG_
yamasho 0:04db82da014d 485 printf("StepStop:%d \n",_StepPhase );
yamasho 0:04db82da014d 486 #endif
yamasho 0:04db82da014d 487
yamasho 0:04db82da014d 488 }
yamasho 0:04db82da014d 489 void LV8548::SetStepFree ( void )
yamasho 0:04db82da014d 490 {
yamasho 0:04db82da014d 491 #if _DEBUG_
yamasho 0:04db82da014d 492 printf("StepFree:%d \n",_StepPhase );
yamasho 0:04db82da014d 493 #endif
yamasho 0:04db82da014d 494 if( _Drivertype != STEPERMOTOR ) return;
yamasho 0:04db82da014d 495
yamasho 0:04db82da014d 496 SetStepStop();
yamasho 0:04db82da014d 497
yamasho 0:04db82da014d 498 _StepPhase = 0;
yamasho 0:04db82da014d 499 _in1.write(0.0f);
yamasho 0:04db82da014d 500 _in2.write(0.0f);
yamasho 0:04db82da014d 501 _in3.write(0.0f);
yamasho 0:04db82da014d 502 _in4.write(0.0f);
yamasho 0:04db82da014d 503
yamasho 0:04db82da014d 504 }
yamasho 0:04db82da014d 505
yamasho 0:04db82da014d 506 void LV8548::SetStepMotor(void)
yamasho 0:04db82da014d 507 {
yamasho 0:04db82da014d 508 _StepOperation = false; //
yamasho 0:04db82da014d 509
yamasho 0:04db82da014d 510 _NowStepDirection = _TragetStepDirection;
yamasho 0:04db82da014d 511 _NowStepMode = _TargetStepMode;
yamasho 0:04db82da014d 512 _IntTimerCount = _StepTimeCount; /* 即初回次のシーケンス起動 なんで初回の1回も普通にカウントしているの？*/
yamasho 0:04db82da014d 513 if( _NowStepMode == FULLSTEP )
yamasho 0:04db82da014d 514 {
yamasho 0:04db82da014d 515 _NowStep = 1; /* 波形比較により1/2シフト */
yamasho 0:04db82da014d 516 _StepTimeCount = 500000.0F / _StepFreq ;
yamasho 0:04db82da014d 517 _TargetStep = _TargetStep + _TargetStep; /* FULL時は2倍 */
yamasho 0:04db82da014d 518 }
yamasho 0:04db82da014d 519 else
yamasho 0:04db82da014d 520 {
yamasho 0:04db82da014d 521 _NowStep = 0;
yamasho 0:04db82da014d 522 _StepTimeCount = 1000000.0F / _StepFreq;
yamasho 0:04db82da014d 523 }
yamasho 0:04db82da014d 524 #if _DEBUG_
yamasho 0:04db82da014d 525 printf("StepTimeCount:%d \n",_StepTimeCount );
yamasho 0:04db82da014d 526 #endif
yamasho 0:04db82da014d 527 _StepOperation = true;
yamasho 0:04db82da014d 528 }
yamasho 0:04db82da014d 529
yamasho 0:04db82da014d 530 void LV8548::SetStepTimerInt(void)
yamasho 0:04db82da014d 531 {
yamasho 0:04db82da014d 532 uint8_t OutputImage;
yamasho 0:04db82da014d 533 if(!_StepOperation ) return;
yamasho 0:04db82da014d 534 _IntTimerCount += _baseus;
yamasho 0:04db82da014d 535
yamasho 0:04db82da014d 536 if( _IntTimerCount >= _StepTimeCount)
yamasho 0:04db82da014d 537 {
yamasho 0:04db82da014d 538 _IntTimerCount = 0;
yamasho 0:04db82da014d 539 if( _TargetStep )
yamasho 0:04db82da014d 540 {
yamasho 0:04db82da014d 541 if(_NowStep >= _TargetStep)
yamasho 0:04db82da014d 542 { // ステップ数よる停止条件の場合 */
yamasho 0:04db82da014d 543 /* Stop処理と同じ */
yamasho 0:04db82da014d 544 _StepOperation = false;
yamasho 0:04db82da014d 545 _TimerCounter = 0.0f;
yamasho 0:04db82da014d 546 _NowStep = 0;
yamasho 0:04db82da014d 547 return; /* 元ソースと同じ動作にあわせる為 */
yamasho 0:04db82da014d 548 }
yamasho 0:04db82da014d 549 /// Image 作成 /////////////////////
yamasho 0:04db82da014d 550 ++_NowStep;
yamasho 0:04db82da014d 551 switch(_NowStepMode)
yamasho 0:04db82da014d 552 {
yamasho 0:04db82da014d 553 case FULLSTEP:
yamasho 0:04db82da014d 554 OutputImage = FullStepDrivePattern[ _StepPhase ];
yamasho 0:04db82da014d 555 break;
yamasho 0:04db82da014d 556 case HALFSTEP:
yamasho 0:04db82da014d 557 OutputImage = HaleStepDrivePattern[ _StepPhase ];
yamasho 0:04db82da014d 558 break;
yamasho 0:04db82da014d 559 default:
yamasho 0:04db82da014d 560 OutputImage = 0;
yamasho 0:04db82da014d 561 break;
yamasho 0:04db82da014d 562 }
yamasho 0:04db82da014d 563 #if _DEBUG_
yamasho 0:04db82da014d 564 printf("Ot:%x %x %x\n",OutputImage,_StepPhase,_TargetStepMode);
yamasho 0:04db82da014d 565 #endif
yamasho 1:e60c7c42e6fc 566 #if USE_PWM_PORT
yamasho 0:04db82da014d 567 if( OutputImage & COIL_A ) _in1.write(1.0f);
yamasho 0:04db82da014d 568 else _in1.write(0.0f);
yamasho 0:04db82da014d 569 if( OutputImage & COIL_B ) _in2.write(1.0f);
yamasho 0:04db82da014d 570 else _in2.write(0.0f);
yamasho 0:04db82da014d 571 if( OutputImage & COIL_C ) _in3.write(1.0f);
yamasho 0:04db82da014d 572 else _in3.write(0.0f);
yamasho 0:04db82da014d 573 if( OutputImage & COIL_D ) _in4.write(1.0f);
yamasho 0:04db82da014d 574 else _in4.write(0.0f);
yamasho 1:e60c7c42e6fc 575 #else
yamasho 1:e60c7c42e6fc 576 _in1 = (( OutputImage & COIL_A ) != 0);
yamasho 1:e60c7c42e6fc 577 _in2 = (( OutputImage & COIL_B ) != 0);
yamasho 1:e60c7c42e6fc 578 _in3 = (( OutputImage & COIL_C ) != 0);
yamasho 1:e60c7c42e6fc 579 _in4 = (( OutputImage & COIL_D ) != 0);
yamasho 1:e60c7c42e6fc 580 #endif
yamasho 0:04db82da014d 581 if(_NowStepDirection) _StepPhase --;
yamasho 0:04db82da014d 582 else _StepPhase ++;
yamasho 0:04db82da014d 583 _StepPhase = _StepPhase & (STEPPHASEMAX -1);
yamasho 0:04db82da014d 584 }
yamasho 0:04db82da014d 585 }
yamasho 0:04db82da014d 586 }