411a57627d
complete rework of Move-Algorithm, no position errors any more, complete in integer math //it was the hell to debug it... ;-) optimized timing of Ack-Answer added command QP "Query Pen", untested, answer might be wrong 0 <--> 1? optimized Init-String for EBBv13 optimized some variable types to enhance memory performance changed enablePenMotor to 6 by default to match it to Spherebot Electronics changed default maxSpeeds from 1000 to 2000;
21 lines
486 B
C++
21 lines
486 B
C++
void initHardware(){
|
|
pinMode(enableRotMotor, OUTPUT);
|
|
pinMode(enablePenMotor, OUTPUT);
|
|
rotMotor.setMaxSpeed(2000.0);
|
|
rotMotor.setAcceleration(10000.0);
|
|
penMotor.setMaxSpeed(2000.0);
|
|
penMotor.setAcceleration(10000.0);
|
|
digitalWrite(enableRotMotor, HIGH) ;
|
|
digitalWrite(enablePenMotor, HIGH) ;
|
|
penServo.attach(servoPin);
|
|
penServo.write(penUpPos);
|
|
}
|
|
|
|
void inline sendAck(){
|
|
Serial.print("OK\r\n");
|
|
}
|
|
|
|
void inline sendError(){
|
|
Serial.print("unknown CMD\r\n");
|
|
}
|