Sho Yama / LV8548

LV8548 Motor Driver Stepper Motor Dc MOtor

Dependents:   LV8548_ON_MD_Modlle_kit_DCMtr_And_STEPMtr

LV8548.cpp@0:04db82da014d, 2018-11-16 (annotated)

Committer:
yamasho
Date:
Fri Nov 16 16:45:46 2018 +0000
Revision:
0:04db82da014d
Child:
1:e60c7c42e6fc
DC Motor And StepMotor Support Fist Commit

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 0:04db82da014d 11
yamasho 0:04db82da014d 12 #define COIL_A (0X1)
yamasho 0:04db82da014d 13 #define COIL_B (0X2)
yamasho 0:04db82da014d 14 #define COIL_C (0X4)
yamasho 0:04db82da014d 15 #define COIL_D (0X8)
yamasho 0:04db82da014d 16
yamasho 0:04db82da014d 17
yamasho 0:04db82da014d 18
yamasho 0:04db82da014d 19 const uint8_t FullStepDrivePattern[STEPPHASEMAX] =
yamasho 0:04db82da014d 20 #if 0
yamasho 0:04db82da014d 21 {(COIL_A | COIL_D),
yamasho 0:04db82da014d 22 (COIL_A | COIL_D),
yamasho 0:04db82da014d 23 (COIL_A | COIL_B),
yamasho 0:04db82da014d 24 (COIL_A | COIL_B),
yamasho 0:04db82da014d 25 (COIL_B | COIL_C),
yamasho 0:04db82da014d 26 (COIL_B | COIL_C),
yamasho 0:04db82da014d 27 (COIL_C | COIL_D),
yamasho 0:04db82da014d 28 (COIL_C | COIL_D)};
yamasho 0:04db82da014d 29 #else
yamasho 0:04db82da014d 30 /// orgin
yamasho 0:04db82da014d 31 {(COIL_A | COIL_C),
yamasho 0:04db82da014d 32 (COIL_A | COIL_C),
yamasho 0:04db82da014d 33 (COIL_B | COIL_C),
yamasho 0:04db82da014d 34 (COIL_B | COIL_C),
yamasho 0:04db82da014d 35 (COIL_B | COIL_D),
yamasho 0:04db82da014d 36 (COIL_B | COIL_D),
yamasho 0:04db82da014d 37 (COIL_A | COIL_D),
yamasho 0:04db82da014d 38 (COIL_A | COIL_D)};
yamasho 0:04db82da014d 39
yamasho 0:04db82da014d 40 #endif
yamasho 0:04db82da014d 41 const uint8_t HaleStepDrivePattern[STEPPHASEMAX ] =
yamasho 0:04db82da014d 42 #if 0
yamasho 0:04db82da014d 43 {(COIL_A | COIL_D),
yamasho 0:04db82da014d 44 (COIL_A),
yamasho 0:04db82da014d 45 (COIL_A | COIL_B),
yamasho 0:04db82da014d 46 (COIL_B),
yamasho 0:04db82da014d 47 (COIL_B | COIL_C),
yamasho 0:04db82da014d 48 (COIL_C),
yamasho 0:04db82da014d 49 (COIL_C | COIL_D),
yamasho 0:04db82da014d 50 (COIL_D)};
yamasho 0:04db82da014d 51 #else
yamasho 0:04db82da014d 52 /// orgin
yamasho 0:04db82da014d 53 {(COIL_A),
yamasho 0:04db82da014d 54 (COIL_A | COIL_C),
yamasho 0:04db82da014d 55 (COIL_C),
yamasho 0:04db82da014d 56 (COIL_B | COIL_C),
yamasho 0:04db82da014d 57 (COIL_B),
yamasho 0:04db82da014d 58 (COIL_B | COIL_D),
yamasho 0:04db82da014d 59 (COIL_D),
yamasho 0:04db82da014d 60 (COIL_A | COIL_D)};
yamasho 0:04db82da014d 61
yamasho 0:04db82da014d 62 #endif
yamasho 0:04db82da014d 63 #define _DEBUG_ (0)
yamasho 0:04db82da014d 64
yamasho 0:04db82da014d 65
yamasho 0:04db82da014d 66 LV8548::LV8548(PinName in1, PinName in2, PinName in3, PinName in4, DriverType drivertype,uint16_t baseus):
yamasho 0:04db82da014d 67 _in1(in1),_in2(in2),_in3(in3),_in4(in4),_Drivertype(drivertype),_baseus(baseus) {
yamasho 0:04db82da014d 68
yamasho 0:04db82da014d 69 _in1 = 0;
yamasho 0:04db82da014d 70 _in2 = 0;
yamasho 0:04db82da014d 71 _in3 = 0;
yamasho 0:04db82da014d 72 _in4 = 0;
yamasho 0:04db82da014d 73
yamasho 0:04db82da014d 74 _Pwmmode[MOTOR_A] = FWD_OPEN;
yamasho 0:04db82da014d 75 _Pwmmode[MOTOR_B] = FWD_OPEN;
yamasho 0:04db82da014d 76
yamasho 0:04db82da014d 77 _Pwmout[MOTOR_A] = OUTPUT_OFF;
yamasho 0:04db82da014d 78 _Pwmout[MOTOR_B] = OUTPUT_OFF;
yamasho 0:04db82da014d 79
yamasho 0:04db82da014d 80 _PwmPeriod[MOTOR_A] = 0.02f; // 20ms
yamasho 0:04db82da014d 81 _PwmPeriod[MOTOR_B] = 0.02f; // 20ms
yamasho 0:04db82da014d 82
yamasho 0:04db82da014d 83 _PwmDuty[MOTOR_A] = 1.0f; // Duyt 100%
yamasho 0:04db82da014d 84 _PwmDuty[MOTOR_B] = 1.0f; // Duyt 100%
yamasho 0:04db82da014d 85
yamasho 0:04db82da014d 86 if(_Drivertype == DCMOTOR ) return; // DC MotorにはTimerはない。
yamasho 0:04db82da014d 87 if(_baseus == 0) return; // 0us のタイマも無し.
yamasho 0:04db82da014d 88
yamasho 0:04db82da014d 89 float TimeVal = (float)_baseus / 1000000;
yamasho 0:04db82da014d 90 #if _DEBUG_
yamasho 0:04db82da014d 91 printf("TimerSetting %f",TimeVal);
yamasho 0:04db82da014d 92 #endif
yamasho 0:04db82da014d 93 _Lv8548BaseTimer.attach(this,&LV8548::SetStepTimerInt, TimeVal );
yamasho 0:04db82da014d 94 }
yamasho 0:04db82da014d 95
yamasho 0:04db82da014d 96 void LV8548::SetDcMotorStop(uint8_t ch)
yamasho 0:04db82da014d 97 {
yamasho 0:04db82da014d 98 if( _Drivertype != DCMOTOR ) return;
yamasho 0:04db82da014d 99
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 0:04db82da014d 253 _in1.period( _PwmPeriod[MOTOR_A] );
yamasho 0:04db82da014d 254 _in2.period( _PwmPeriod[MOTOR_A] );
yamasho 0:04db82da014d 255 SetDcMotor(MOTOR_A);
yamasho 0:04db82da014d 256 #if _DEBUG_
yamasho 0:04db82da014d 257 printf("freq0 : %f ",_PwmPeriod[MOTOR_A]);
yamasho 0:04db82da014d 258 #endif
yamasho 0:04db82da014d 259 }else
yamasho 0:04db82da014d 260 if(ch == MOTOR_B)
yamasho 0:04db82da014d 261 {
yamasho 0:04db82da014d 262 SetDcMotorStop(MOTOR_B);
yamasho 0:04db82da014d 263 _PwmPeriod[MOTOR_B] = 1/Freq;
yamasho 0:04db82da014d 264 _in2.period( _PwmPeriod[MOTOR_B] );
yamasho 0:04db82da014d 265 _in3.period( _PwmPeriod[MOTOR_B] );
yamasho 0:04db82da014d 266 #if _DEBUG_
yamasho 0:04db82da014d 267 printf("freq1 : %f ",_PwmPeriod[MOTOR_B]);
yamasho 0:04db82da014d 268 #endif
yamasho 0:04db82da014d 269 SetDcMotor(MOTOR_B);
yamasho 0:04db82da014d 270 }
yamasho 0:04db82da014d 271
yamasho 0:04db82da014d 272 }
yamasho 0:04db82da014d 273
yamasho 0:04db82da014d 274 void LV8548::SetDcPwmMode( uint8_t ch ,DriverPwmMode Mode )
yamasho 0:04db82da014d 275 {
yamasho 0:04db82da014d 276 if(ch < MOTOR_MAX)
yamasho 0:04db82da014d 277 {
yamasho 0:04db82da014d 278 #if _DEBUG_
yamasho 0:04db82da014d 279 printf("DC Pwm Set\n");
yamasho 0:04db82da014d 280 #endif
yamasho 0:04db82da014d 281
yamasho 0:04db82da014d 282 _Pwmmode[ch] = Mode;
yamasho 0:04db82da014d 283 SetDcMotor(ch);
yamasho 0:04db82da014d 284 }
yamasho 0:04db82da014d 285 }
yamasho 0:04db82da014d 286
yamasho 0:04db82da014d 287 void LV8548::SetDcPwmDuty( uint8_t ch ,float Duty )
yamasho 0:04db82da014d 288 {
yamasho 0:04db82da014d 289 if(ch < MOTOR_MAX)
yamasho 0:04db82da014d 290 {
yamasho 0:04db82da014d 291 #if _DEBUG_
yamasho 0:04db82da014d 292 printf("DC Duty Set\n");
yamasho 0:04db82da014d 293 #endif
yamasho 0:04db82da014d 294 _PwmDuty[ch] = Duty;
yamasho 0:04db82da014d 295 SetDcMotor(ch);
yamasho 0:04db82da014d 296 }
yamasho 0:04db82da014d 297 }
yamasho 0:04db82da014d 298
yamasho 0:04db82da014d 299
yamasho 0:04db82da014d 300 void LV8548::SetDcOutPut( uint8_t ch ,DriverPwmOut Mode )
yamasho 0:04db82da014d 301 {
yamasho 0:04db82da014d 302 if(ch < MOTOR_MAX)
yamasho 0:04db82da014d 303 {
yamasho 0:04db82da014d 304 #if _DEBUG_
yamasho 0:04db82da014d 305 printf("DC Out Set\n");
yamasho 0:04db82da014d 306 #endif
yamasho 0:04db82da014d 307 _Pwmout[ch] = Mode;
yamasho 0:04db82da014d 308 SetDcMotor(ch);
yamasho 0:04db82da014d 309 }
yamasho 0:04db82da014d 310 }
yamasho 0:04db82da014d 311
yamasho 0:04db82da014d 312 /*******************************************************************************************************************/
yamasho 0:04db82da014d 313 /* FFFFFF SSSS MM MM */
yamasho 0:04db82da014d 314 /* FF SS SS tt MMM MMM tt */
yamasho 0:04db82da014d 315 /* FF oooo rrrrr SS tttttt eeee ppppp MMMMMMM oooo tttttt oooo rrrrr */
yamasho 0:04db82da014d 316 /* FFFF oo oo rr rr SSSS tt ee ee pp pp MM M MM oo oo tt oo oo rr rr */
yamasho 0:04db82da014d 317 /* FF oo oo rr SS tt eeeeee pp pp MM MM oo oo tt oo oo rr */
yamasho 0:04db82da014d 318 /* FF oo oo rr SS SS tt ee ppppp MM MM oo oo tt oo oo rr */
yamasho 0:04db82da014d 319 /* FF oooo rr SSSS ttt eeee pp MM MM oooo ttt oooo rr */
yamasho 0:04db82da014d 320 /* pp */
yamasho 0:04db82da014d 321 /*******************************************************************************************************************/
yamasho 0:04db82da014d 322 void LV8548::SetStepAngle( uint16_t Angle)
yamasho 0:04db82da014d 323 {
yamasho 0:04db82da014d 324 _StepDeg = Angle;
yamasho 0:04db82da014d 325 #if _DEBUG_
yamasho 0:04db82da014d 326 printf("Set Angle Deg:%f \n",_StepDeg);
yamasho 0:04db82da014d 327 #endif
yamasho 0:04db82da014d 328 }
yamasho 0:04db82da014d 329 /// 周波数1～4800
yamasho 0:04db82da014d 330 /// DEG 0～65535
yamasho 0:04db82da014d 331 /// rotation 0:CW 1:CCW
yamasho 0:04db82da014d 332 /// EXP 0:FullStep 1:HalfStep
yamasho 0:04db82da014d 333 void LV8548::SetStepDeg ( uint16_t frequency , uint16_t deg , uint8_t Direction, uint8_t StepMode)
yamasho 0:04db82da014d 334 {
yamasho 0:04db82da014d 335 #if _DEBUG_
yamasho 0:04db82da014d 336 printf("Deg Commnad freq:%d deg:%d dir:%d Mode%d\n",frequency , deg , Direction, StepMode);
yamasho 0:04db82da014d 337 #endif
yamasho 0:04db82da014d 338
yamasho 0:04db82da014d 339 if ( frequency == 0 ) return;
yamasho 0:04db82da014d 340
yamasho 0:04db82da014d 341 if( frequency > STEPMAX_FREQ )
yamasho 0:04db82da014d 342 {
yamasho 0:04db82da014d 343 frequency = STEPMAX_FREQ;
yamasho 0:04db82da014d 344 }
yamasho 0:04db82da014d 345
yamasho 0:04db82da014d 346 SetStepStop(); /* Step Motor 初期化　*/
yamasho 0:04db82da014d 347
yamasho 0:04db82da014d 348 _TragetStepDirection = (DriverMotorDirction )Direction; // CW/CCW
yamasho 0:04db82da014d 349 _TargetStepMode = (DriverStepMode )StepMode; // Full/Half;
yamasho 0:04db82da014d 350 if(_TargetStepMode != _NowStepMode)
yamasho 0:04db82da014d 351 { /* 動作モード変更の為初期化 */
yamasho 0:04db82da014d 352 _StepPhase = 0;
yamasho 0:04db82da014d 353 #if _DEBUG_
yamasho 0:04db82da014d 354 printf("Stepper Mode Change Then StepPhase 0\n");
yamasho 0:04db82da014d 355 #endif
yamasho 0:04db82da014d 356 }
yamasho 0:04db82da014d 357 #if 0 // 意味の無い処理なのでコメント化
yamasho 0:04db82da014d 358 if( _TragetStepDirection != _NowStepDirection )
yamasho 0:04db82da014d 359 { // 方向の変更があった時 ///
yamasho 0:04db82da014d 360 if( _TragetStepDirection == DIR_CW )
yamasho 0:04db82da014d 361 {
yamasho 0:04db82da014d 362 _StepPhase++; //なぜ2パターン？？
yamasho 0:04db82da014d 363 _StepPhase++; // 元のソースは2パターン戻すが、必要なもないので削除
yamasho 0:04db82da014d 364 }
yamasho 0:04db82da014d 365 else
yamasho 0:04db82da014d 366 if( _TragetStepDirection == DIR_CCW )
yamasho 0:04db82da014d 367 {
yamasho 0:04db82da014d 368 _StepPhase--; //なぜ2パターン？？
yamasho 0:04db82da014d 369 _StepPhase--; // 元のソースは2パターン戻すが、必要なもないので削除
yamasho 0:04db82da014d 370 }
yamasho 0:04db82da014d 371 _StepPhase &= STEPPHASEMAX; // 0-7に！
yamasho 0:04db82da014d 372 }
yamasho 0:04db82da014d 373 #endif
yamasho 0:04db82da014d 374 _StepFreq = frequency ;
yamasho 0:04db82da014d 375 if(_StepDeg)
yamasho 0:04db82da014d 376 {
yamasho 0:04db82da014d 377 _TargetStep = ( deg / _StepDeg);
yamasho 0:04db82da014d 378 if(_TargetStepMode)
yamasho 0:04db82da014d 379 { /* HALF時2倍 */
yamasho 0:04db82da014d 380 _TargetStep = _TargetStep + _TargetStep;
yamasho 0:04db82da014d 381 }
yamasho 0:04db82da014d 382 }
yamasho 0:04db82da014d 383 else
yamasho 0:04db82da014d 384 {
yamasho 0:04db82da014d 385 _TargetStep = 0;
yamasho 0:04db82da014d 386 }
yamasho 0:04db82da014d 387
yamasho 0:04db82da014d 388 #if _DEBUG_
yamasho 0:04db82da014d 389 printf("Analyze StepFreq:%f TargetStep:%d StepPhase:%d \n",_StepFreq ,_TargetStep,_StepPhase);
yamasho 0:04db82da014d 390 #endif
yamasho 0:04db82da014d 391 SetStepMotor();
yamasho 0:04db82da014d 392 }
yamasho 0:04db82da014d 393
yamasho 0:04db82da014d 394 void LV8548::SetStepTime ( uint16_t frequency , uint16_t time , uint8_t Direction, uint8_t StepMode)
yamasho 0:04db82da014d 395 {
yamasho 0:04db82da014d 396 #if _DEBUG_
yamasho 0:04db82da014d 397 printf("Time Commnad freq:%d Time:%d dir:%d Mode%d\n",frequency ,time, Direction, StepMode);
yamasho 0:04db82da014d 398 #endif
yamasho 0:04db82da014d 399 if ( frequency == 0 ) return;
yamasho 0:04db82da014d 400
yamasho 0:04db82da014d 401 if( frequency > STEPMAX_FREQ )
yamasho 0:04db82da014d 402 {
yamasho 0:04db82da014d 403 frequency = STEPMAX_FREQ;
yamasho 0:04db82da014d 404 }
yamasho 0:04db82da014d 405 SetStepStop(); /* Step Motor 初期化　*/
yamasho 0:04db82da014d 406
yamasho 0:04db82da014d 407 _TragetStepDirection = (DriverMotorDirction )Direction;
yamasho 0:04db82da014d 408 _TargetStepMode = (DriverStepMode )StepMode; // Full/Half;
yamasho 0:04db82da014d 409
yamasho 0:04db82da014d 410 if(_TargetStepMode != _NowStepMode)
yamasho 0:04db82da014d 411 { /* 動作モード変更の為初期化 */
yamasho 0:04db82da014d 412 _StepPhase = 0;
yamasho 0:04db82da014d 413 #if _DEBUG_
yamasho 0:04db82da014d 414 printf("Stepper Mode Change Then StepPhase 0\n");
yamasho 0:04db82da014d 415 #endif
yamasho 0:04db82da014d 416 }
yamasho 0:04db82da014d 417
yamasho 0:04db82da014d 418 _StepFreq = frequency ;
yamasho 0:04db82da014d 419 _TargetStep = ( time * frequency );
yamasho 0:04db82da014d 420 #if _DEBUG_
yamasho 0:04db82da014d 421 printf("Analyze StepFreq:%f TargetStep:%d StepPhase:%d \n",_StepFreq ,_TargetStep,_StepPhase);
yamasho 0:04db82da014d 422 #endif
yamasho 0:04db82da014d 423 SetStepMotor();
yamasho 0:04db82da014d 424 }
yamasho 0:04db82da014d 425
yamasho 0:04db82da014d 426 void LV8548::SetStepStep ( uint16_t frequency , uint16_t step , uint8_t Direction, uint8_t StepMode)
yamasho 0:04db82da014d 427 {
yamasho 0:04db82da014d 428 #if _DEBUG_
yamasho 0:04db82da014d 429 printf("Step Commnad freq:%d step:%d dir:%d Mode%d\n",frequency , step , Direction, StepMode);
yamasho 0:04db82da014d 430 #endif
yamasho 0:04db82da014d 431 if ( frequency == 0 ) return;
yamasho 0:04db82da014d 432
yamasho 0:04db82da014d 433 if( frequency > STEPMAX_FREQ )
yamasho 0:04db82da014d 434 {
yamasho 0:04db82da014d 435 frequency = STEPMAX_FREQ;
yamasho 0:04db82da014d 436 }
yamasho 0:04db82da014d 437 SetStepStop(); /* Step Motor 初期化　*/
yamasho 0:04db82da014d 438
yamasho 0:04db82da014d 439 _TragetStepDirection = (DriverMotorDirction )Direction;
yamasho 0:04db82da014d 440 _TargetStepMode = (DriverStepMode )StepMode; // Full/Half;
yamasho 0:04db82da014d 441
yamasho 0:04db82da014d 442 if(_TargetStepMode != _NowStepMode)
yamasho 0:04db82da014d 443 { /* 動作モード変更の為初期化 */
yamasho 0:04db82da014d 444 _StepPhase = 0;
yamasho 0:04db82da014d 445 #if _DEBUG_
yamasho 0:04db82da014d 446 printf("Stepper Mode Change Then StepPhase 0\n");
yamasho 0:04db82da014d 447 #endif
yamasho 0:04db82da014d 448 }
yamasho 0:04db82da014d 449 #if 0
yamasho 0:04db82da014d 450 if( _TragetStepDirection != _NowStepDirection )
yamasho 0:04db82da014d 451 { // 方向の変更があった時 ///
yamasho 0:04db82da014d 452 if( _TragetStepDirection == DIR_CW )
yamasho 0:04db82da014d 453 {
yamasho 0:04db82da014d 454 _StepPhase++; //なぜ2パターン？？
yamasho 0:04db82da014d 455 _StepPhase++;
yamasho 0:04db82da014d 456 }
yamasho 0:04db82da014d 457 else
yamasho 0:04db82da014d 458 if( _TragetStepDirection == DIR_CCW )
yamasho 0:04db82da014d 459 {
yamasho 0:04db82da014d 460 _StepPhase--; //なぜ2パターン？？
yamasho 0:04db82da014d 461 _StepPhase--;
yamasho 0:04db82da014d 462 }
yamasho 0:04db82da014d 463 _StepPhase &= STEPPHASEMAX; // 0-7に！
yamasho 0:04db82da014d 464 }
yamasho 0:04db82da014d 465 #endif
yamasho 0:04db82da014d 466 _StepFreq = frequency;
yamasho 0:04db82da014d 467 _TargetStep = step;
yamasho 0:04db82da014d 468 #if _DEBUG_
yamasho 0:04db82da014d 469 printf("Analyze StepFreq:%f %f TargetStep:%d StepPhase:%d \n",_StepFreq ,_TargetStep,_StepPhase);
yamasho 0:04db82da014d 470 #endif
yamasho 0:04db82da014d 471 SetStepMotor();
yamasho 0:04db82da014d 472 }
yamasho 0:04db82da014d 473 void LV8548::SetStepStop ( void )
yamasho 0:04db82da014d 474 {
yamasho 0:04db82da014d 475 if( _Drivertype != STEPERMOTOR ) return;
yamasho 0:04db82da014d 476 /* 割込内の終了処理と同じ */
yamasho 0:04db82da014d 477 _StepOperation = false;
yamasho 0:04db82da014d 478 _TimerCounter = 0.0f;
yamasho 0:04db82da014d 479 _NowStep = 0;
yamasho 0:04db82da014d 480 #if _DEBUG_
yamasho 0:04db82da014d 481 printf("StepStop:%d \n",_StepPhase );
yamasho 0:04db82da014d 482 #endif
yamasho 0:04db82da014d 483
yamasho 0:04db82da014d 484 }
yamasho 0:04db82da014d 485 void LV8548::SetStepFree ( void )
yamasho 0:04db82da014d 486 {
yamasho 0:04db82da014d 487 #if _DEBUG_
yamasho 0:04db82da014d 488 printf("StepFree:%d \n",_StepPhase );
yamasho 0:04db82da014d 489 #endif
yamasho 0:04db82da014d 490 if( _Drivertype != STEPERMOTOR ) return;
yamasho 0:04db82da014d 491
yamasho 0:04db82da014d 492 SetStepStop();
yamasho 0:04db82da014d 493
yamasho 0:04db82da014d 494 _StepPhase = 0;
yamasho 0:04db82da014d 495 _in1.write(0.0f);
yamasho 0:04db82da014d 496 _in2.write(0.0f);
yamasho 0:04db82da014d 497 _in3.write(0.0f);
yamasho 0:04db82da014d 498 _in4.write(0.0f);
yamasho 0:04db82da014d 499
yamasho 0:04db82da014d 500 }
yamasho 0:04db82da014d 501
yamasho 0:04db82da014d 502 void LV8548::SetStepMotor(void)
yamasho 0:04db82da014d 503 {
yamasho 0:04db82da014d 504 _StepOperation = false; //
yamasho 0:04db82da014d 505
yamasho 0:04db82da014d 506 _NowStepDirection = _TragetStepDirection;
yamasho 0:04db82da014d 507 _NowStepMode = _TargetStepMode;
yamasho 0:04db82da014d 508 _IntTimerCount = _StepTimeCount; /* 即初回次のシーケンス起動 なんで初回の1回も普通にカウントしているの？*/
yamasho 0:04db82da014d 509 if( _NowStepMode == FULLSTEP )
yamasho 0:04db82da014d 510 {
yamasho 0:04db82da014d 511 _NowStep = 1; /* 波形比較により1/2シフト */
yamasho 0:04db82da014d 512 _StepTimeCount = 500000.0F / _StepFreq ;
yamasho 0:04db82da014d 513 _TargetStep = _TargetStep + _TargetStep; /* FULL時は2倍 */
yamasho 0:04db82da014d 514 }
yamasho 0:04db82da014d 515 else
yamasho 0:04db82da014d 516 {
yamasho 0:04db82da014d 517 _NowStep = 0;
yamasho 0:04db82da014d 518 _StepTimeCount = 1000000.0F / _StepFreq;
yamasho 0:04db82da014d 519 }
yamasho 0:04db82da014d 520 #if _DEBUG_
yamasho 0:04db82da014d 521 printf("StepTimeCount:%d \n",_StepTimeCount );
yamasho 0:04db82da014d 522 #endif
yamasho 0:04db82da014d 523 _StepOperation = true;
yamasho 0:04db82da014d 524 }
yamasho 0:04db82da014d 525
yamasho 0:04db82da014d 526 void LV8548::SetStepTimerInt(void)
yamasho 0:04db82da014d 527 {
yamasho 0:04db82da014d 528 uint8_t OutputImage;
yamasho 0:04db82da014d 529 if(!_StepOperation ) return;
yamasho 0:04db82da014d 530 _IntTimerCount += _baseus;
yamasho 0:04db82da014d 531
yamasho 0:04db82da014d 532 if( _IntTimerCount >= _StepTimeCount)
yamasho 0:04db82da014d 533 {
yamasho 0:04db82da014d 534 _IntTimerCount = 0;
yamasho 0:04db82da014d 535 if( _TargetStep )
yamasho 0:04db82da014d 536 {
yamasho 0:04db82da014d 537 if(_NowStep >= _TargetStep)
yamasho 0:04db82da014d 538 { // ステップ数よる停止条件の場合 */
yamasho 0:04db82da014d 539 /* Stop処理と同じ */
yamasho 0:04db82da014d 540 _StepOperation = false;
yamasho 0:04db82da014d 541 _TimerCounter = 0.0f;
yamasho 0:04db82da014d 542 _NowStep = 0;
yamasho 0:04db82da014d 543 return; /* 元ソースと同じ動作にあわせる為 */
yamasho 0:04db82da014d 544 }
yamasho 0:04db82da014d 545 /// Image 作成 /////////////////////
yamasho 0:04db82da014d 546 ++_NowStep;
yamasho 0:04db82da014d 547 switch(_NowStepMode)
yamasho 0:04db82da014d 548 {
yamasho 0:04db82da014d 549 case FULLSTEP:
yamasho 0:04db82da014d 550 OutputImage = FullStepDrivePattern[ _StepPhase ];
yamasho 0:04db82da014d 551 break;
yamasho 0:04db82da014d 552 case HALFSTEP:
yamasho 0:04db82da014d 553 OutputImage = HaleStepDrivePattern[ _StepPhase ];
yamasho 0:04db82da014d 554 break;
yamasho 0:04db82da014d 555 default:
yamasho 0:04db82da014d 556 OutputImage = 0;
yamasho 0:04db82da014d 557 break;
yamasho 0:04db82da014d 558 }
yamasho 0:04db82da014d 559 #if _DEBUG_
yamasho 0:04db82da014d 560 printf("Ot:%x %x %x\n",OutputImage,_StepPhase,_TargetStepMode);
yamasho 0:04db82da014d 561 #endif
yamasho 0:04db82da014d 562
yamasho 0:04db82da014d 563 if( OutputImage & COIL_A ) _in1.write(1.0f);
yamasho 0:04db82da014d 564 else _in1.write(0.0f);
yamasho 0:04db82da014d 565 if( OutputImage & COIL_B ) _in2.write(1.0f);
yamasho 0:04db82da014d 566 else _in2.write(0.0f);
yamasho 0:04db82da014d 567 if( OutputImage & COIL_C ) _in3.write(1.0f);
yamasho 0:04db82da014d 568 else _in3.write(0.0f);
yamasho 0:04db82da014d 569 if( OutputImage & COIL_D ) _in4.write(1.0f);
yamasho 0:04db82da014d 570 else _in4.write(0.0f);
yamasho 0:04db82da014d 571
yamasho 0:04db82da014d 572
yamasho 0:04db82da014d 573 if(_NowStepDirection) _StepPhase --;
yamasho 0:04db82da014d 574 else _StepPhase ++;
yamasho 0:04db82da014d 575 _StepPhase = _StepPhase & (STEPPHASEMAX -1);
yamasho 0:04db82da014d 576 }
yamasho 0:04db82da014d 577 }
yamasho 0:04db82da014d 578 }