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