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;
242 curAngle = (float)((((uint16_t)data[0]) << 8 )| (uint16_t)data[1])*0.087890625;
243 pointer->
encoder.angle = curAngle;
246 curAngle = fmod(curAngle + 360.0, 360.0);
248 deltaAngle = (oldAngle - curAngle);
250 if(deltaAngle < -180.0)
256 else if(deltaAngle > 180.0)
265 deltaSpeedAngle += deltaAngle;
270 newSpeed = oldSpeed*ALPHA + newSpeed*BETA;
274 deltaSpeedAngle = 0.0;
284 error = (float)stepCnt;
286 error *= pointer->stepResolution;
288 if(error > pointer->tolerance)
291 control = (int32_t)(error/pointer->stepResolution);
296 else if(error < -pointer->tolerance)
299 control = (int32_t)(error/pointer->stepResolution);
306 PORTB |= (PIND & 0x04) >> 2;
319 float float2::getFloatValue(
void)
327 a.i = (uint32_t)(this->value >> 25);
332 uint64_t float2::getRawValue(
void)
337 void float2::setValue(
float val)
347 this->value = ((uint64_t)a.i) << 25;
350 bool float2::operator<=(
const float &value)
352 if(this->getFloatValue() > value)
357 if(this->getFloatValue() == value)
359 if((this->value & 0x0000000000007FFF) > 0)
368 bool float2::operator<=(
const float2 &value)
370 if((this->value >> 56) > (value.value >> 56))
375 if((this->value >> 56) == (value.value >> 56))
377 if( (this->value >> 48) < (value.value >> 48) )
382 if( (this->value >> 48) == (value.value >> 48) )
384 if((this->value & 0x0000FFFFFFFFFFFF) <= (value.value & 0x0000FFFFFFFFFFFF))
394 float2 & float2::operator=(
const float &value)
396 this->setValue(value);
401 float2 & float2::operator+=(
const float &value)
409 uint64_t tempMant, tempExp;
412 if((this->value >> 56) == (temp.value >> 56))
416 cnt = (temp.value >> 48) - (this->value >> 48);
419 tempExp = (temp.value >> 48);
421 this->value &= 0x0000FFFFFFFFFFFF;
422 this->value |= 0x0001000000000000;
425 tempMant = (temp.value & 0x0000FFFFFFFFFFFF) | 0x0001000000000000;
426 tempMant += this->value;
428 while(tempMant > 0x2000000000000)
434 tempMant &= 0x0000FFFFFFFFFFFF;
435 this->value = (tempExp << 48) | tempMant;
439 this->value = temp.value;
445 cnt = (this->value >> 48) - (temp.value >> 48);
449 tempExp = (this->value >> 48);
451 temp.value &= 0x0000FFFFFFFFFFFF;
452 temp.value |= 0x0001000000000000;
455 tempMant = (this->value & 0x0000FFFFFFFFFFFF) | 0x0001000000000000;
456 tempMant += temp.value;
458 while(tempMant > 0x2000000000000)
464 tempMant &= 0x0000FFFFFFFFFFFF;
465 this->value = (tempExp << 48) | tempMant;
470 else if((this->value >> 56) == 1)
472 this->value &= 0x00FFFFFFFFFFFFFF;
476 cnt = (temp.value >> 48) - (this->value >> 48);
480 tempExp = (temp.value >> 48);
482 this->value &= 0x0000FFFFFFFFFFFF;
483 this->value |= 0x0001000000000000;
486 tempMant = (temp.value & 0x0000FFFFFFFFFFFF) | 0x0001000000000000;
488 tempMant -= this->value;
490 if(tempMant > 0x8000000000000000)
493 tempMant &= 0x0000FFFFFFFFFFFF;
497 while(tempMant < 0x1000000000000)
503 tempMant &= 0x0000FFFFFFFFFFFF;
505 this->value = (tempExp << 48) | tempMant;
510 this->value = temp.value;
516 cnt = (this->value >> 48) - (temp.value >> 48);
519 tempExp = (this->value >> 48);
521 temp.value &= 0x0000FFFFFFFFFFFF;
522 temp.value |= 0x0001000000000000;
525 tempMant = (this->value & 0x0000FFFFFFFFFFFF) | 0x0001000000000000;
527 tempMant -= temp.value;
529 if(tempMant > 0x8000000000000000)
531 tempMant &= 0x0000FFFFFFFFFFFF;
535 while(tempMant < 0x1000000000000)
541 tempMant &= 0x0000FFFFFFFFFFFF;
543 this->value = (tempExp << 48) | tempMant;
544 this->value |= 0x0100000000000000;
551 temp.value &= 0x00FFFFFFFFFFFFFF;
555 cnt = (this->value >> 48) - (temp.value >> 48);
558 tempExp = (this->value >> 48);
560 temp.value &= 0x0000FFFFFFFFFFFF;
561 temp.value |= 0x0001000000000000;
564 tempMant = (this->value & 0x0000FFFFFFFFFFFF) | 0x0001000000000000;
566 tempMant -= temp.value;
568 if(tempMant > 0x8000000000000000)
570 tempMant &= 0x0000FFFFFFFFFFFF;
574 while(tempMant < 0x1000000000000)
580 tempMant &= 0x0000FFFFFFFFFFFF;
582 this->value = (tempExp << 48) | tempMant;
588 cnt = (temp.value >> 48) - (this->value >> 48);
591 tempExp = (temp.value >> 48);
593 this->value &= 0x0000FFFFFFFFFFFF;
594 this->value |= 0x0001000000000000;
597 tempMant = (temp.value & 0x0000FFFFFFFFFFFF) | 0x0001000000000000;
599 tempMant -= this->value;
601 if(tempMant > 0x8000000000000000)
603 tempMant &= 0x0000FFFFFFFFFFFF;
607 while(tempMant < 0x1000000000000)
613 tempMant &= 0x0000FFFFFFFFFFFF;
615 this->value = (tempExp << 48) | tempMant;
616 this->value |= 0x0100000000000000;
621 this->value = temp.value;
622 this->value |= 0x0100000000000000;
642 Vout = analogRead(TEMP)*0.0048828125;
643 NTC = ((
R*5.0)/Vout)-
R;
645 T =
A +
B*NTC +
C*NTC*NTC*NTC;
705 TIMSK1 = (1 << OCIE1A);
706 TCCR1B = (1 << WGM12) | (1 << CS10);
736 return (((uint16_t)data[0]) << 8 )| (uint16_t)data[1];
761 else if(data == 0x10)
766 else if(data == 0x20)
860 else if(vel > 32000.0)
908 if(dir != digitalRead(DIR))
916 if(this->
exactDelay.getFloatValue() >= 65535.5)
918 this->
delay = 0xFFFF;
937 this->
accelSteps = (uint32_t)(((this->velocity*this->velocity) - (curVel*curVel))/(2.0*this->
acceleration));
962 if(this->
exactDelay.getFloatValue() > 65535.0)
964 this->
delay = 0xFFFF;
988 this->
hold = holdMode;
996 if(dir != digitalRead(DIR))
1016 if(this->
exactDelay.getFloatValue() >= 65535.5)
1018 this->
delay = 0xFFFF;
1050 this->
accelSteps = (uint32_t)(((this->velocity*this->velocity) - (curVel*curVel))/(2.0*this->
acceleration));
1115 if(this->
exactDelay.getFloatValue() > 65535.0)
1117 this->
delay = 0xFFFF;
1137 this->
hold = holdMode;
1148 if(holdMode ==
SOFT)
1153 else if (holdMode ==
HARD)
1170 this->
hold = holdMode;
1182 if(this->
exactDelay.getFloatValue() > 65535.0)
1184 this->
delay = 0xFFFF;
1196 if(holdMode ==
SOFT)
1201 else if (holdMode ==
HARD)
1208 void uStepper::setup(
bool mode, uint8_t microStepping,
float faultSpeed, uint32_t faultTolerance)
1210 this->dropIn = mode;
1216 digitalWrite(2,HIGH);
1217 digitalWrite(3,HIGH);
1218 digitalWrite(4,HIGH);
1219 this->stepResolution = 360.0/((float)(200*microStepping));
1220 this->tolerance = ((float)faultTolerance)*this->stepResolution;
1221 this->faultStepDelay = (uint16_t)((
INTFREQ/faultSpeed) - 1);
1226 TCCR2B &= ~((1 << CS20) | (1 << CS21) | (1 << CS22) | (1 << WGM22));
1227 TCCR2A &= ~((1 << WGM20) | (1 << WGM21));
1228 TCCR2B |= (1 << CS21);
1229 TCCR2A |= (1 << WGM21);
1237 TIFR2 |= (1 << OCF2A);
1238 TIMSK2 |= (1 << OCIE2A);
1245 TIMSK2 &= ~(1 << OCF2A);
1291 while (!(TWCR & (1 << TWINT)));
1297 bool i2cMaster::read(uint8_t slaveAddr, uint8_t regAddr, uint8_t numOfBytes, uint8_t *data)
1299 uint8_t i, buff[numOfBytes];
1301 TIMSK1 &= ~(1 << OCIE1A);
1309 for(i = 0; i < (numOfBytes - 1); i++)
1318 TIMSK1 |= (1 << OCIE1A);
1327 TIMSK1 &= ~(1 << OCIE1A);
1332 for(i = 0; i < numOfBytes; i++)
1338 TIMSK1 |= (1 << OCIE1A);
1347 this->
cmd((1 << TWINT) | (1 << TWEN) | (1 << TWEA));
1352 this->
cmd((1 << TWINT) | (1 << TWEN));
1363 this->
cmd((1<<TWINT) | (1<<TWSTA) | (1<<TWEN));
1371 TWDR = (addr << 1) | RW;
1372 this->
cmd((1 << TWINT) | (1 << TWEN));
1387 return this->
start(addr, RW);
1394 this->
cmd((1 << TWINT) | (1 << TWEN));
1403 TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWSTO);
1406 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