void stepMotor(Encoder &enc, Motor &mot, short step){
short startpos = enc.pos;
short dest = startpos + step;
uint8_t i=0;
uint16_t error = abs(enc.pos - dest);
while(abs(enc.pos - startpos) < abs(step)){
error = abs(enc.pos - dest);
int16_t force = max(error*6, 70) - 2*abs(enc.speed);
uint8_t power = constrain(force, 0, 112);
if (force < 30){
mot.stop();
}else{
if (i < power){
if (step > 0)
mot.right();
else
mot.left();
}else{
mot.disable();
}
}
i++;
}
mot.stop();
_delay_ms(100);
mot.disable();
}
// This is an old function for controlling the wheels
void drive() {
for(int i=0; i<256; i++) {
left.rate(i);
right.rate(i);
left.drive(true);
right.drive(true);
delay(10);
}
for(int i=255; i>0; i--) {
left.rate(i);
right.rate(i);
left.drive(true);
right.drive(true);
delay(10);
}
left.stop();
right.stop();
delay(500);
for(int i=0; i<256; i++) {
left.rate(i);
right.rate(i);
left.drive(false);
right.drive(false);
delay(10);
}
for(int i=255; i>0; i--) {
left.rate(i);
right.rate(i);
left.drive(false);
right.drive(false);
delay(10);
}
left.stop();
right.stop();
delay(500);
}
