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();
}
int main() {
UCSR0B = 0;
Serial.begin(115200);
PCMSK0 = (1 << 1) | (1 << 6) | (1 << 2) | (1 << 7);
PCICR |= (1 << PCIE0);
PAPER_SENSOR_PORT |= (1 << PAPER_SENSOR_PIN);
OCR1A = 100;
TCCR1A = 0;
TCCR1B = ((1 << CS11)); // Enable timer, 1mhz
TIMSK1 = (1 << TOIE1) | (1 << OCIE1A);
TCNT1 = 0;
SERVO_DDR |= (1 << SERVO_PIN);
sei();
uint16_t val = 0;
for(;;){
if (carriageEnc.changeFlag || feedEnc.changeFlag){
carriageEnc.changeFlag = feedEnc.changeFlag = 0;
//Serial.println(carriageEnc.speed);
}
if (Serial.available() > 0) {
int incomingByte = Serial.read();
switch (incomingByte){
case 'p':
feedPaper();
break;
case 't':
Serial.println(val);
break;
case 'f':
stepMotor(feedEnc, feed, val);
break;
case 'c':
stepMotor(carriageEnc, carriage, val-carriageEnc.pos);
break;
case 's':
servoPos = val;
_delay_ms(150);
break;
case 'x':
feed.right();
_delay_ms(1500);
feed.disable();
case 'n':
servoPos += 10;
break;
case 'm':
servoPos -= 10;
break;
default:
feed.disable();
carriage.disable();
}
if (incomingByte >='1' && incomingByte <='9'){
val = val*10 + (1 + incomingByte - '1');
}else if (incomingByte == '0'){
val = val*10;
}else{
val = 0;
printStatus();
}
}
}
return 0;
}
