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?
User | Revision | Line number | New 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 | } |