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; }