Library for handle a Stepper Motor Driver Borad
StepperMotor.cpp@1:b96435078d4d, 2012-12-16 (annotated)
- Committer:
- dury
- Date:
- Sun Dec 16 16:39:40 2012 +0000
- Revision:
- 1:b96435078d4d
- Parent:
- 0:ce88091ae9fb
Library for move Stepper Motor using DIR and CLK pin of a control board.;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
dury | 1:b96435078d4d | 1 | #include "StepperMotor.h" |
dury | 1:b96435078d4d | 2 | |
dury | 1:b96435078d4d | 3 | /* TODO: |
dury | 1:b96435078d4d | 4 | _RunDegrees(float degrees) |
dury | 1:b96435078d4d | 5 | _SetDegreeJump(float degree) |
dury | 1:b96435078d4d | 6 | _DegreeUp() |
dury | 1:b96435078d4d | 7 | _DegreeDown() |
dury | 1:b96435078d4d | 8 | */ |
dury | 1:b96435078d4d | 9 | Serial debug(USBTX,USBRX); |
dury | 1:b96435078d4d | 10 | |
dury | 1:b96435078d4d | 11 | StepperMotorDrv::StepperMotorDrv(PinName PinDir, PinName PinClk):Dir(PinDir),Clock(PinClk) |
dury | 1:b96435078d4d | 12 | { |
dury | 1:b96435078d4d | 13 | MaxSpeed=100; // default 100 Hz |
dury | 1:b96435078d4d | 14 | Speed=50; // default 50 Hz |
dury | 1:b96435078d4d | 15 | SpeedJump=1; // default 1 Hz |
dury | 1:b96435078d4d | 16 | DutyCycle=50; // default 50 % |
dury | 1:b96435078d4d | 17 | SetDir(CW); // default ClockWise |
dury | 1:b96435078d4d | 18 | Clock=0; // Set to 0 for sure |
dury | 1:b96435078d4d | 19 | CurrPos=0; // Set 0 when power on |
dury | 1:b96435078d4d | 20 | CurrDegree=0; // default 0 degrees |
dury | 1:b96435078d4d | 21 | DegreesPerStep=0; // 0 -> NOT SET |
dury | 1:b96435078d4d | 22 | } |
dury | 1:b96435078d4d | 23 | |
dury | 1:b96435078d4d | 24 | StepperMotorDrv::~StepperMotorDrv() |
dury | 1:b96435078d4d | 25 | { |
dury | 1:b96435078d4d | 26 | } |
dury | 1:b96435078d4d | 27 | |
dury | 1:b96435078d4d | 28 | // Set duty cycle of pwm in percentual |
dury | 1:b96435078d4d | 29 | void StepperMotorDrv::SetDutyCyclePerc(float PercentualValue) |
dury | 1:b96435078d4d | 30 | { |
dury | 1:b96435078d4d | 31 | if (PercentualValue > 100) PercentualValue=100; |
dury | 1:b96435078d4d | 32 | DutyCycle=PercentualValue; |
dury | 1:b96435078d4d | 33 | tUp=(tPeriod/100)*DutyCycle; |
dury | 1:b96435078d4d | 34 | tDown=tPeriod-tUp; |
dury | 1:b96435078d4d | 35 | } |
dury | 1:b96435078d4d | 36 | |
dury | 1:b96435078d4d | 37 | // Set duty cycle of pwm as float value from 0.0 to 1.0 |
dury | 1:b96435078d4d | 38 | // Values grater than 1.0 were used as 0.n |
dury | 1:b96435078d4d | 39 | void StepperMotorDrv::SetDutyCycle(float DecimalValue) |
dury | 1:b96435078d4d | 40 | { |
dury | 1:b96435078d4d | 41 | if (DecimalValue > 1){ |
dury | 1:b96435078d4d | 42 | DecimalValue=DecimalValue - int(DecimalValue); |
dury | 1:b96435078d4d | 43 | } |
dury | 1:b96435078d4d | 44 | DutyCycle=DecimalValue*100; |
dury | 1:b96435078d4d | 45 | tUp=tPeriod*DecimalValue; |
dury | 1:b96435078d4d | 46 | tDown=tPeriod-tUp; |
dury | 1:b96435078d4d | 47 | } |
dury | 1:b96435078d4d | 48 | |
dury | 1:b96435078d4d | 49 | // Set Raw Speed in Hz |
dury | 1:b96435078d4d | 50 | void StepperMotorDrv::SetSpeed(float SpeedHz) |
dury | 1:b96435078d4d | 51 | { |
dury | 1:b96435078d4d | 52 | Speed=SpeedHz; |
dury | 1:b96435078d4d | 53 | tPeriod=1/Speed; |
dury | 1:b96435078d4d | 54 | // Recalculate also DutyCycle t for new period |
dury | 1:b96435078d4d | 55 | tUp=tPeriod*(DutyCycle/100); |
dury | 1:b96435078d4d | 56 | tDown=tPeriod-tUp; |
dury | 1:b96435078d4d | 57 | } |
dury | 1:b96435078d4d | 58 | |
dury | 1:b96435078d4d | 59 | void StepperMotorDrv::SetMaxSpeed(float MaxSpeedHz) |
dury | 1:b96435078d4d | 60 | { |
dury | 1:b96435078d4d | 61 | MaxSpeed=MaxSpeedHz; |
dury | 1:b96435078d4d | 62 | } |
dury | 1:b96435078d4d | 63 | |
dury | 1:b96435078d4d | 64 | void StepperMotorDrv::SetSpeedPerc(float PercValue) |
dury | 1:b96435078d4d | 65 | { |
dury | 1:b96435078d4d | 66 | Speed=(MaxSpeed/100)*PercValue; |
dury | 1:b96435078d4d | 67 | tPeriod=1/Speed; |
dury | 1:b96435078d4d | 68 | // Recalculate also DutyCycle t for new period |
dury | 1:b96435078d4d | 69 | tUp=tPeriod*(DutyCycle/100); |
dury | 1:b96435078d4d | 70 | tDown=tPeriod-tUp; |
dury | 1:b96435078d4d | 71 | } |
dury | 1:b96435078d4d | 72 | |
dury | 1:b96435078d4d | 73 | float StepperMotorDrv::SpeedUp(float ValueHz) |
dury | 1:b96435078d4d | 74 | { |
dury | 1:b96435078d4d | 75 | Speed+=ValueHz; |
dury | 1:b96435078d4d | 76 | // Recalculate also DutyCycle t for new period |
dury | 1:b96435078d4d | 77 | tUp=tPeriod*(DutyCycle/100); |
dury | 1:b96435078d4d | 78 | tDown=tPeriod-tUp; |
dury | 1:b96435078d4d | 79 | |
dury | 1:b96435078d4d | 80 | return(Speed); |
dury | 1:b96435078d4d | 81 | } |
dury | 1:b96435078d4d | 82 | |
dury | 1:b96435078d4d | 83 | float StepperMotorDrv::SpeedDown(float ValueHz) |
dury | 1:b96435078d4d | 84 | { |
dury | 1:b96435078d4d | 85 | Speed-=ValueHz; |
dury | 1:b96435078d4d | 86 | // Recalculate also DutyCycle t for new period |
dury | 1:b96435078d4d | 87 | tUp=tPeriod*(DutyCycle/100); |
dury | 1:b96435078d4d | 88 | tDown=tPeriod-tUp; |
dury | 1:b96435078d4d | 89 | |
dury | 1:b96435078d4d | 90 | return Speed; |
dury | 1:b96435078d4d | 91 | } |
dury | 1:b96435078d4d | 92 | |
dury | 1:b96435078d4d | 93 | void StepperMotorDrv::SetSpeedJump(float ValueHz) |
dury | 1:b96435078d4d | 94 | { |
dury | 1:b96435078d4d | 95 | SpeedJump=ValueHz; |
dury | 1:b96435078d4d | 96 | } |
dury | 1:b96435078d4d | 97 | |
dury | 1:b96435078d4d | 98 | float StepperMotorDrv::SpeedUp(void) |
dury | 1:b96435078d4d | 99 | { |
dury | 1:b96435078d4d | 100 | return(SpeedUp(SpeedJump)); |
dury | 1:b96435078d4d | 101 | } |
dury | 1:b96435078d4d | 102 | |
dury | 1:b96435078d4d | 103 | // Decrement Speed by <Step> Value |
dury | 1:b96435078d4d | 104 | // Return: new Speed in Hz |
dury | 1:b96435078d4d | 105 | float StepperMotorDrv::SpeedDown(void) |
dury | 1:b96435078d4d | 106 | { |
dury | 1:b96435078d4d | 107 | return(SpeedDown(SpeedJump)); |
dury | 1:b96435078d4d | 108 | } |
dury | 1:b96435078d4d | 109 | |
dury | 1:b96435078d4d | 110 | // Get the current speed in Hz |
dury | 1:b96435078d4d | 111 | float StepperMotorDrv::GetSpeed(void) |
dury | 1:b96435078d4d | 112 | { |
dury | 1:b96435078d4d | 113 | return Speed; |
dury | 1:b96435078d4d | 114 | } |
dury | 1:b96435078d4d | 115 | |
dury | 1:b96435078d4d | 116 | // Get the current speed in Perc Value |
dury | 1:b96435078d4d | 117 | float StepperMotorDrv::GetSpeedPerc(void) |
dury | 1:b96435078d4d | 118 | { |
dury | 1:b96435078d4d | 119 | return (Speed*100)/MaxSpeed; |
dury | 1:b96435078d4d | 120 | } |
dury | 1:b96435078d4d | 121 | |
dury | 1:b96435078d4d | 122 | // Get the current MAX Speed in Hz |
dury | 1:b96435078d4d | 123 | float StepperMotorDrv::GetMaxSpeed(void) |
dury | 1:b96435078d4d | 124 | { |
dury | 1:b96435078d4d | 125 | return (MaxSpeed); |
dury | 1:b96435078d4d | 126 | } |
dury | 1:b96435078d4d | 127 | |
dury | 1:b96435078d4d | 128 | // Get the current Duty Cycle in Perc Value |
dury | 1:b96435078d4d | 129 | float StepperMotorDrv::GetDutyCyclePerc(void) |
dury | 1:b96435078d4d | 130 | { |
dury | 1:b96435078d4d | 131 | return (DutyCycle); |
dury | 1:b96435078d4d | 132 | } |
dury | 1:b96435078d4d | 133 | |
dury | 1:b96435078d4d | 134 | // Get the current Duty Cycle decimal value |
dury | 1:b96435078d4d | 135 | float StepperMotorDrv::GetDutyCycle(void) |
dury | 1:b96435078d4d | 136 | { |
dury | 1:b96435078d4d | 137 | return (DutyCycle/100); |
dury | 1:b96435078d4d | 138 | } |
dury | 1:b96435078d4d | 139 | |
dury | 1:b96435078d4d | 140 | // Get the current Step Value |
dury | 1:b96435078d4d | 141 | float StepperMotorDrv::GetSpeedJump(void) |
dury | 1:b96435078d4d | 142 | { |
dury | 1:b96435078d4d | 143 | return (SpeedJump); |
dury | 1:b96435078d4d | 144 | } |
dury | 1:b96435078d4d | 145 | |
dury | 1:b96435078d4d | 146 | // Set Rotation Direction: CW=1 CC=0 |
dury | 1:b96435078d4d | 147 | void StepperMotorDrv::SetDir(bool RotationDir) |
dury | 1:b96435078d4d | 148 | { |
dury | 1:b96435078d4d | 149 | Dir=RotationDir; |
dury | 1:b96435078d4d | 150 | } |
dury | 1:b96435078d4d | 151 | |
dury | 1:b96435078d4d | 152 | // Set Rotation Direction CW |
dury | 1:b96435078d4d | 153 | void StepperMotorDrv::SetDirCW(void) |
dury | 1:b96435078d4d | 154 | { |
dury | 1:b96435078d4d | 155 | Dir=CW; |
dury | 1:b96435078d4d | 156 | } |
dury | 1:b96435078d4d | 157 | |
dury | 1:b96435078d4d | 158 | // Set Rotation Direction CC |
dury | 1:b96435078d4d | 159 | void StepperMotorDrv::SetDirCC(void) |
dury | 1:b96435078d4d | 160 | { |
dury | 1:b96435078d4d | 161 | Dir=CC; |
dury | 1:b96435078d4d | 162 | } |
dury | 1:b96435078d4d | 163 | |
dury | 1:b96435078d4d | 164 | // Set Rotation Direction CW |
dury | 1:b96435078d4d | 165 | void StepperMotorDrv::RevertDir(void) |
dury | 1:b96435078d4d | 166 | { |
dury | 1:b96435078d4d | 167 | Dir=!Dir; |
dury | 1:b96435078d4d | 168 | } |
dury | 1:b96435078d4d | 169 | |
dury | 1:b96435078d4d | 170 | // Get Rotation Direction Direction |
dury | 1:b96435078d4d | 171 | // Return: CW=1 or CC=0 |
dury | 1:b96435078d4d | 172 | bool StepperMotorDrv::GetDir(void) |
dury | 1:b96435078d4d | 173 | { |
dury | 1:b96435078d4d | 174 | return(Dir); |
dury | 1:b96435078d4d | 175 | } |
dury | 1:b96435078d4d | 176 | |
dury | 1:b96435078d4d | 177 | void StepperMotorDrv::Run(void) |
dury | 1:b96435078d4d | 178 | { |
dury | 1:b96435078d4d | 179 | Clock.period(tPeriod); |
dury | 1:b96435078d4d | 180 | Clock.write(DutyCycle/100); |
dury | 1:b96435078d4d | 181 | } |
dury | 1:b96435078d4d | 182 | |
dury | 1:b96435078d4d | 183 | // Stop the Motor |
dury | 1:b96435078d4d | 184 | void StepperMotorDrv::Stop(void) |
dury | 1:b96435078d4d | 185 | { |
dury | 1:b96435078d4d | 186 | Clock=0; |
dury | 1:b96435078d4d | 187 | } |
dury | 1:b96435078d4d | 188 | |
dury | 1:b96435078d4d | 189 | void StepperMotorDrv::RunStep(int nrSteps, bool RotationDir) |
dury | 1:b96435078d4d | 190 | { |
dury | 1:b96435078d4d | 191 | Dir=RotationDir; |
dury | 1:b96435078d4d | 192 | int StepAdd=(0-1)+(RotationDir*2); |
dury | 1:b96435078d4d | 193 | float DegreeAdd=DegreesPerStep*(float(StepAdd)); |
dury | 1:b96435078d4d | 194 | |
dury | 1:b96435078d4d | 195 | for (int step=0; step<nrSteps; step++){ |
dury | 1:b96435078d4d | 196 | Clock=1; |
dury | 1:b96435078d4d | 197 | wait(tUp); |
dury | 1:b96435078d4d | 198 | Clock=0; |
dury | 1:b96435078d4d | 199 | wait(tDown); |
dury | 1:b96435078d4d | 200 | CurrPos=CurrPos+StepAdd; // Update Current Step Position |
dury | 1:b96435078d4d | 201 | CurrDegree=CurrDegree+DegreeAdd; // Update Current Degree Position |
dury | 1:b96435078d4d | 202 | //debug.printf("Current Pos: %d Degree: %f\r\n",CurrPos,CurrDegree); |
dury | 1:b96435078d4d | 203 | } |
dury | 1:b96435078d4d | 204 | } |
dury | 1:b96435078d4d | 205 | |
dury | 1:b96435078d4d | 206 | void StepperMotorDrv::RunStep(int nrSteps) |
dury | 1:b96435078d4d | 207 | { |
dury | 1:b96435078d4d | 208 | RunStep(nrSteps,Dir); |
dury | 1:b96435078d4d | 209 | } |
dury | 1:b96435078d4d | 210 | |
dury | 1:b96435078d4d | 211 | void StepperMotorDrv::RunStep(void) |
dury | 1:b96435078d4d | 212 | { |
dury | 1:b96435078d4d | 213 | RunStep(1,Dir); |
dury | 1:b96435078d4d | 214 | } |
dury | 1:b96435078d4d | 215 | |
dury | 1:b96435078d4d | 216 | void StepperMotorDrv::RunStepCW(void) |
dury | 1:b96435078d4d | 217 | { |
dury | 1:b96435078d4d | 218 | RunStep(1,CW); |
dury | 1:b96435078d4d | 219 | } |
dury | 1:b96435078d4d | 220 | |
dury | 1:b96435078d4d | 221 | void StepperMotorDrv::RunStepCC(void) |
dury | 1:b96435078d4d | 222 | { |
dury | 1:b96435078d4d | 223 | RunStep(1,CC); |
dury | 1:b96435078d4d | 224 | } |
dury | 1:b96435078d4d | 225 | |
dury | 1:b96435078d4d | 226 | void StepperMotorDrv::SetZero(void) |
dury | 1:b96435078d4d | 227 | { |
dury | 1:b96435078d4d | 228 | CurrPos=0; |
dury | 1:b96435078d4d | 229 | CurrDegree=0; |
dury | 1:b96435078d4d | 230 | } |
dury | 1:b96435078d4d | 231 | |
dury | 1:b96435078d4d | 232 | void StepperMotorDrv::SetCurrPos(int Pos) |
dury | 1:b96435078d4d | 233 | { |
dury | 1:b96435078d4d | 234 | CurrPos=Pos; |
dury | 1:b96435078d4d | 235 | } |
dury | 1:b96435078d4d | 236 | |
dury | 1:b96435078d4d | 237 | int StepperMotorDrv::GetCurrPos(void) |
dury | 1:b96435078d4d | 238 | { |
dury | 1:b96435078d4d | 239 | return(CurrPos); |
dury | 1:b96435078d4d | 240 | } |
dury | 1:b96435078d4d | 241 | |
dury | 1:b96435078d4d | 242 | float StepperMotorDrv::GetPeriod(void) |
dury | 1:b96435078d4d | 243 | { |
dury | 1:b96435078d4d | 244 | return(tPeriod); |
dury | 1:b96435078d4d | 245 | } |
dury | 1:b96435078d4d | 246 | |
dury | 1:b96435078d4d | 247 | void StepperMotorDrv::SetDegreePerStep(float Degrees){ |
dury | 1:b96435078d4d | 248 | DegreesPerStep=Degrees; |
dury | 1:b96435078d4d | 249 | } |
dury | 1:b96435078d4d | 250 | |
dury | 1:b96435078d4d | 251 | float StepperMotorDrv::GetDegreePerStep(void){ |
dury | 1:b96435078d4d | 252 | return(DegreesPerStep); |
dury | 1:b96435078d4d | 253 | } |
dury | 1:b96435078d4d | 254 | |
dury | 1:b96435078d4d | 255 | float StepperMotorDrv::GetCurrDegree(void){ |
dury | 1:b96435078d4d | 256 | return(CurrDegree); |
dury | 1:b96435078d4d | 257 | } |
dury | 1:b96435078d4d | 258 | |
dury | 1:b96435078d4d | 259 |