Fabio Durigon / StepperMotor

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?

UserRevisionLine numberNew 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