75 volatile uint16_t i = 0;
76 volatile int32_t stepCnt = 0, control = 0;
105 delayMicroseconds(1);
125 void TIMER2_COMPA_vect(
void)
127 asm volatile(
"push r16 \n\t");
128 asm volatile(
"in r16,0x3F \n\t");
129 asm volatile(
"push r16 \n\t");
130 asm volatile(
"push r30 \n\t");
131 asm volatile(
"push r31 \n\t");
132 asm volatile(
"lds r30,pointer \n\t");
133 asm volatile(
"lds r31,pointer+1 \n\t");
134 asm volatile(
"ldd r16,z+56 \n\t");
135 asm volatile(
"sbrs r16,0 \n\t");
136 asm volatile(
"jmp _AccelerationAlgorithm \n\t");
138 asm volatile(
"push r0 \n\t");
139 asm volatile(
"push r1 \n\t");
140 asm volatile(
"push r2 \n\t");
141 asm volatile(
"push r3 \n\t");
142 asm volatile(
"push r4 \n\t");
143 asm volatile(
"push r5 \n\t");
144 asm volatile(
"push r6 \n\t");
145 asm volatile(
"push r7 \n\t");
146 asm volatile(
"push r8 \n\t");
147 asm volatile(
"push r9 \n\t");
148 asm volatile(
"push r10 \n\t");
149 asm volatile(
"push r11 \n\t");
150 asm volatile(
"push r12 \n\t");
151 asm volatile(
"push r13 \n\t");
152 asm volatile(
"push r14 \n\t");
153 asm volatile(
"push r15 \n\t");
154 asm volatile(
"push r17 \n\t");
155 asm volatile(
"push r18 \n\t");
156 asm volatile(
"push r19 \n\t");
157 asm volatile(
"push r20 \n\t");
158 asm volatile(
"push r21 \n\t");
159 asm volatile(
"push r22 \n\t");
160 asm volatile(
"push r23 \n\t");
161 asm volatile(
"push r24 \n\t");
162 asm volatile(
"push r25 \n\t");
163 asm volatile(
"push r26 \n\t");
164 asm volatile(
"push r27 \n\t");
165 asm volatile(
"push r28 \n\t");
166 asm volatile(
"push r29 \n\t");
168 if(i < pointer->faultStepDelay)
176 delayMicroseconds(1);
190 asm volatile(
"pop r29 \n\t");
191 asm volatile(
"pop r28 \n\t");
192 asm volatile(
"pop r27 \n\t");
193 asm volatile(
"pop r26 \n\t");
194 asm volatile(
"pop r25 \n\t");
195 asm volatile(
"pop r24 \n\t");
196 asm volatile(
"pop r23 \n\t");
197 asm volatile(
"pop r22 \n\t");
198 asm volatile(
"pop r21 \n\t");
199 asm volatile(
"pop r20 \n\t");
200 asm volatile(
"pop r19 \n\t");
201 asm volatile(
"pop r18 \n\t");
202 asm volatile(
"pop r17 \n\t");
203 asm volatile(
"pop r15 \n\t");
204 asm volatile(
"pop r14 \n\t");
205 asm volatile(
"pop r13 \n\t");
206 asm volatile(
"pop r12 \n\t");
207 asm volatile(
"pop r11 \n\t");
208 asm volatile(
"pop r10 \n\t");
209 asm volatile(
"pop r9 \n\t");
210 asm volatile(
"pop r8 \n\t");
211 asm volatile(
"pop r7 \n\t");
212 asm volatile(
"pop r6 \n\t");
213 asm volatile(
"pop r5 \n\t");
214 asm volatile(
"pop r4 \n\t");
215 asm volatile(
"pop r3 \n\t");
216 asm volatile(
"pop r2 \n\t");
217 asm volatile(
"pop r1 \n\t");
218 asm volatile(
"pop r0 \n\t");
219 asm volatile(
"pop r31 \n\t");
220 asm volatile(
"pop r30 \n\t");
221 asm volatile(
"pop r16 \n\t");
222 asm volatile(
"out 0x3F,r16 \n\t");
223 asm volatile(
"pop r16 \n\t");
224 asm volatile(
"reti \n\t");
227 void TIMER1_COMPA_vect(
void)
230 float deltaAngle, newSpeed;
231 static float curAngle, oldAngle = 0.0, deltaSpeedAngle = 0.0, oldSpeed = 0.0;
232 static uint8_t loops = 0;
233 static float revolutions = 0.0;
243 curAngle = (float)((((uint16_t)data[0]) << 8 ) | (uint16_t)data[1])*0.087890625;
244 pointer->
encoder.angle = curAngle;
247 curAngle = fmod(curAngle + 360.0, 360.0);
249 deltaAngle = (oldAngle - curAngle);
251 if(deltaAngle < -180.0)
257 else if(deltaAngle > 180.0)
266 deltaSpeedAngle += deltaAngle;
271 newSpeed = oldSpeed*ALPHA + newSpeed*BETA;
275 deltaSpeedAngle = 0.0;
285 error = (float)stepCnt;
287 error *= pointer->stepResolution;
289 if(error > pointer->tolerance)
292 control = (int32_t)(error/pointer->stepResolution);
297 else if(error < -pointer->tolerance)
300 control = (int32_t)(error/pointer->stepResolution);
307 PORTB |= (PIND & 0x04) >> 2;
320 float float2::getFloatValue(
void)
328 a.i = (uint32_t)(this->value >> 25);
333 uint64_t float2::getRawValue(
void)
338 void float2::setValue(
float val)
348 this->value = ((uint64_t)a.i) << 25;
351 bool float2::operator<=(
const float &value)
353 if(this->getFloatValue() > value)
358 if(this->getFloatValue() == value)
360 if((this->value & 0x0000000000007FFF) > 0)
369 bool float2::operator<=(
const float2 &value)
371 if((this->value >> 56) > (value.value >> 56))
376 if((this->value >> 56) == (value.value >> 56))
378 if( (this->value >> 48) < (value.value >> 48) )
383 if( (this->value >> 48) == (value.value >> 48) )
385 if((this->value & 0x0000FFFFFFFFFFFF) <= (value.value & 0x0000FFFFFFFFFFFF))
395 float2 & float2::operator=(
const float &value)
397 this->setValue(value);
402 float2 & float2::operator+=(
const float &value)
410 uint64_t tempMant, tempExp;
413 if((this->value >> 56) == (temp.value >> 56))
417 cnt = (temp.value >> 48) - (this->value >> 48);
420 tempExp = (temp.value >> 48);
422 this->value &= 0x0000FFFFFFFFFFFF;
423 this->value |= 0x0001000000000000;
426 tempMant = (temp.value & 0x0000FFFFFFFFFFFF) | 0x0001000000000000;
427 tempMant += this->value;
429 while(tempMant > 0x2000000000000)
435 tempMant &= 0x0000FFFFFFFFFFFF;
436 this->value = (tempExp << 48) | tempMant;
440 this->value = temp.value;
446 cnt = (this->value >> 48) - (temp.value >> 48);
450 tempExp = (this->value >> 48);
452 temp.value &= 0x0000FFFFFFFFFFFF;
453 temp.value |= 0x0001000000000000;
456 tempMant = (this->value & 0x0000FFFFFFFFFFFF) | 0x0001000000000000;
457 tempMant += temp.value;
459 while(tempMant > 0x2000000000000)
465 tempMant &= 0x0000FFFFFFFFFFFF;
466 this->value = (tempExp << 48) | tempMant;
471 else if((this->value >> 56) == 1)
473 this->value &= 0x00FFFFFFFFFFFFFF;
477 cnt = (temp.value >> 48) - (this->value >> 48);
481 tempExp = (temp.value >> 48);
483 this->value &= 0x0000FFFFFFFFFFFF;
484 this->value |= 0x0001000000000000;
487 tempMant = (temp.value & 0x0000FFFFFFFFFFFF) | 0x0001000000000000;
489 tempMant -= this->value;
491 if(tempMant > 0x8000000000000000)
494 tempMant &= 0x0000FFFFFFFFFFFF;
498 while(tempMant < 0x1000000000000)
504 tempMant &= 0x0000FFFFFFFFFFFF;
506 this->value = (tempExp << 48) | tempMant;
511 this->value = temp.value;
517 cnt = (this->value >> 48) - (temp.value >> 48);
520 tempExp = (this->value >> 48);
522 temp.value &= 0x0000FFFFFFFFFFFF;
523 temp.value |= 0x0001000000000000;
526 tempMant = (this->value & 0x0000FFFFFFFFFFFF) | 0x0001000000000000;
528 tempMant -= temp.value;
530 if(tempMant > 0x8000000000000000)
532 tempMant &= 0x0000FFFFFFFFFFFF;
536 while(tempMant < 0x1000000000000)
542 tempMant &= 0x0000FFFFFFFFFFFF;
544 this->value = (tempExp << 48) | tempMant;
545 this->value |= 0x0100000000000000;
552 temp.value &= 0x00FFFFFFFFFFFFFF;
556 cnt = (this->value >> 48) - (temp.value >> 48);
559 tempExp = (this->value >> 48);
561 temp.value &= 0x0000FFFFFFFFFFFF;
562 temp.value |= 0x0001000000000000;
565 tempMant = (this->value & 0x0000FFFFFFFFFFFF) | 0x0001000000000000;
567 tempMant -= temp.value;
569 if(tempMant > 0x8000000000000000)
571 tempMant &= 0x0000FFFFFFFFFFFF;
575 while(tempMant < 0x1000000000000)
581 tempMant &= 0x0000FFFFFFFFFFFF;
583 this->value = (tempExp << 48) | tempMant;
589 cnt = (temp.value >> 48) - (this->value >> 48);
592 tempExp = (temp.value >> 48);
594 this->value &= 0x0000FFFFFFFFFFFF;
595 this->value |= 0x0001000000000000;
598 tempMant = (temp.value & 0x0000FFFFFFFFFFFF) | 0x0001000000000000;
600 tempMant -= this->value;
602 if(tempMant > 0x8000000000000000)
604 tempMant &= 0x0000FFFFFFFFFFFF;
608 while(tempMant < 0x1000000000000)
614 tempMant &= 0x0000FFFFFFFFFFFF;
616 this->value = (tempExp << 48) | tempMant;
617 this->value |= 0x0100000000000000;
622 this->value = temp.value;
623 this->value |= 0x0100000000000000;
643 Vout = analogRead(TEMP)*0.0048828125;
644 NTC = ((
R*5.0)/Vout)-
R;
646 T =
A +
B*NTC +
C*NTC*NTC*NTC;
707 TIMSK1 = (1 << OCIE1A);
708 TCCR1B = (1 << WGM12) | (1 << CS10);
710 this->
encoderOffset = (float)((((uint16_t)data[0]) << 8 ) | (uint16_t)data[1])*0.087890625;
724 this->
encoderOffset = (float)((((uint16_t)data[0]) << 8 ) | (uint16_t)data[1])*0.087890625;
743 return (((uint16_t)data[0]) << 8 )| (uint16_t)data[1];
768 else if(data == 0x10)
773 else if(data == 0x20)
867 else if(vel > 32000.0)
916 if(dir != digitalRead(DIR))
924 if(this->
exactDelay.getFloatValue() >= 65535.5)
926 this->
delay = 0xFFFF;
945 this->
accelSteps = (uint32_t)(((this->velocity*this->velocity) - (curVel*curVel))/(2.0*this->
acceleration));
970 if(this->
exactDelay.getFloatValue() > 65535.0)
972 this->
delay = 0xFFFF;
996 this->
hold = holdMode;
1004 if(dir != digitalRead(DIR))
1024 if(this->
exactDelay.getFloatValue() >= 65535.5)
1026 this->
delay = 0xFFFF;
1058 this->
accelSteps = (uint32_t)(((this->velocity*this->velocity) - (curVel*curVel))/(2.0*this->
acceleration));
1123 if(this->
exactDelay.getFloatValue() > 65535.0)
1125 this->
delay = 0xFFFF;
1145 this->
hold = holdMode;
1156 if(holdMode ==
SOFT)
1161 else if (holdMode ==
HARD)
1178 this->
hold = holdMode;
1190 if(this->
exactDelay.getFloatValue() > 65535.0)
1192 this->
delay = 0xFFFF;
1204 if(holdMode ==
SOFT)
1209 else if (holdMode ==
HARD)
1216 void uStepper::setup(
bool mode, uint8_t microStepping,
float faultSpeed, uint32_t faultTolerance)
1218 this->dropIn = mode;
1224 digitalWrite(2,HIGH);
1225 digitalWrite(3,HIGH);
1226 digitalWrite(4,HIGH);
1227 this->stepResolution = 360.0/((float)(200*microStepping));
1228 this->tolerance = ((float)faultTolerance)*this->stepResolution;
1229 this->faultStepDelay = (uint16_t)((
INTFREQ/faultSpeed) - 1);
1234 TCCR2B &= ~((1 << CS20) | (1 << CS21) | (1 << CS22) | (1 << WGM22));
1235 TCCR2A &= ~((1 << WGM20) | (1 << WGM21));
1236 TCCR2B |= (1 << CS21);
1237 TCCR2A |= (1 << WGM21);
1245 TIFR2 |= (1 << OCF2A);
1246 TIMSK2 |= (1 << OCIE2A);
1253 TIMSK2 &= ~(1 << OCF2A);
1299 while (!(TWCR & (1 << TWINT)));
1305 bool i2cMaster::read(uint8_t slaveAddr, uint8_t regAddr, uint8_t numOfBytes, uint8_t *data)
1307 uint8_t i, buff[numOfBytes];
1309 TIMSK1 &= ~(1 << OCIE1A);
1317 for(i = 0; i < (numOfBytes - 1); i++)
1326 TIMSK1 |= (1 << OCIE1A);
1335 TIMSK1 &= ~(1 << OCIE1A);
1340 for(i = 0; i < numOfBytes; i++)
1346 TIMSK1 |= (1 << OCIE1A);
1355 this->
cmd((1 << TWINT) | (1 << TWEN) | (1 << TWEA));
1360 this->
cmd((1 << TWINT) | (1 << TWEN));
1371 this->
cmd((1<<TWINT) | (1<<TWSTA) | (1<<TWEN));
1379 TWDR = (addr << 1) | RW;
1380 this->
cmd((1 << TWINT) | (1 << TWEN));
1395 return this->
start(addr, RW);
1402 this->
cmd((1 << TWINT) | (1 << TWEN));
1411 TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWSTO);
1414 while (TWCR & (1 << TWSTO));
uStepper(void)
Constructor of uStepper class.
uStepperEncoder(void)
Constructor.
void setup(bool mode=NORMAL, uint8_t microStepping=SIXTEEN, float faultSpeed=3000.0, uint32_t faultTolerance=20)
Initializes the different parts of the uStepper object.
uint16_t getStrength(void)
Measure the strength of the magnet.
float getSpeed(void)
Measure the current speed of the motor.
bool write(uint8_t slaveAddr, uint8_t regAddr, uint8_t numOfBytes, uint8_t *data)
sets up I2C connection to device, writes a number of data bytes and closes the connection ...
bool getMotorState(void)
Get the current state of the motor.
Prototype of class for accessing all features of the uStepper in a single object. ...
void enableMotor(void)
Enables the stepper driver output stage.
float getMaxAcceleration(void)
Get the value of the maximum motor acceleration.
float getTemp(void)
Request a reading of current temperature.
bool getCurrentDirection(void)
Returns the direction the motor is currently configured to rotate.
float getAngleMoved(void)
Measure the angle moved from reference position.
void hardStop(bool holdMode)
Stop the motor without deceleration.
void setMaxAcceleration(float accel)
Set the maximum acceleration of the stepper motor.
void startTimer(void)
Starts timer for stepper algorithm.
void setMaxVelocity(float vel)
Sets the maximum rotational velocity of the motor.
void moveSteps(uint32_t steps, bool dir, bool holdMode)
Make the motor perform a predefined number of steps.
float getMaxVelocity(void)
Returns the maximum rotational velocity of the motor.
bool writeByte(uint8_t data)
Writes a byte to a device on the I2C bus.
i2cMaster(void)
Constructor.
void softStop(bool holdMode)
Stop the motor with deceleration.
uint8_t detectMagnet(void)
Detect if magnet is present and within range.
uint8_t getStatus(void)
Get current I2C status.
void cmd(uint8_t cmd)
Sends commands over the I2C bus.
bool restart(uint8_t addr, bool RW)
Restarts connection between arduino and I2C device.
bool readByte(bool ack, uint8_t *data)
Reads a byte from the I2C bus.
void stopTimer(void)
Stops the timer for the stepper algorithm.
uint8_t getAgc(void)
Read the current AGC value of the encoder chip.
bool start(uint8_t addr, bool RW)
sets up connection between arduino and I2C device.
void runContinous(bool dir)
Make the motor rotate continuously.
#define ENCODERSPEEDCONSTANT
Prototype of class for accessing the TWI (I2C) interface of the AVR (master mode only).
void setHome(void)
Define new reference(home) position.
float getAngle(void)
Measure the current shaft angle.
bool stop(void)
Closes the I2C connection.
void begin(void)
Setup TWI (I2C) interface.
Function prototypes and definitions for the uStepper library.
volatile float angleMoved
uStepperTemp(void)
Constructor.
bool read(uint8_t slaveAddr, uint8_t regAddr, uint8_t numOfBytes, uint8_t *data)
sets up I2C connection to device, reads a number of data bytes and closes the connection ...
void disableMotor(void)
Disables the stepper driver output stage.
int64_t getStepsSinceReset(void)
Get the number of steps applied since reset.
void setup(void)
Setup the encoder.
uint32_t initialDecelSteps