/**
* Initialize all modules
*/
void initAll() {
// initialize ADC
initADC();
// initialize system timer
initTimer0();
// initialize serial com
uart_init();
// redirect printf output to serial port
stdout = &uart_stdout;
// motor.c: initialize motor PWM, pins, encoders and PID
initPWM1();
initMotorPins();
initEncoders();
initTachoVariables();
resetPID();
// clear motor speed and direction just in case
setDirection(LMOTOR, FORWARD);
setDirection(RMOTOR, FORWARD);
setPower100(LMOTOR, 0);
setPower100(RMOTOR, 0);
// odometer.c set initial position and position correction
setOdometerTo(0, 0);
updateRelativePos();
resetPosCorrection();
}
AF_DCMotor::AF_DCMotor(uint8_t num, uint8_t freq) {
motornum = num;
pwmfreq = freq;
MC.enable();
switch (num) {
case 1:
latch_state &= ~_BV(MOTOR1_A) & ~_BV(MOTOR1_B); // set both motor pins to 0
MC.latch_tx();
initPWM1(freq);
break;
case 2:
latch_state &= ~_BV(MOTOR2_A) & ~_BV(MOTOR2_B); // set both motor pins to 0
MC.latch_tx();
initPWM2(freq);
break;
case 3:
latch_state &= ~_BV(MOTOR3_A) & ~_BV(MOTOR3_B); // set both motor pins to 0
MC.latch_tx();
initPWM3(freq);
break;
case 4:
latch_state &= ~_BV(MOTOR4_A) & ~_BV(MOTOR4_B); // set both motor pins to 0
MC.latch_tx();
initPWM4(freq);
break;
}
}
AF_Stepper::AF_Stepper(uint16_t steps, uint8_t num) {
MC.enable();
revsteps = steps;
steppernum = num;
if (steppernum == 1) {
latch_state &= ~_BV(MOTOR1_A) & ~_BV(MOTOR1_B) &
~_BV(MOTOR2_A) & ~_BV(MOTOR2_B); // all motor pins to 0
MC.latch_tx();
// enable both H bridges
pinMode(11, OUTPUT);
pinMode(3, OUTPUT);
digitalWrite(11, HIGH);
digitalWrite(3, HIGH);
#ifdef MICROSTEPPING
// use PWM for microstepping support
initPWM1(MOTOR12_64KHZ);
initPWM2(MOTOR12_64KHZ);
setPWM1(255);
setPWM2(255);
#endif
} else if (steppernum == 2) {
latch_state &= ~_BV(MOTOR3_A) & ~_BV(MOTOR3_B) &
~_BV(MOTOR4_A) & ~_BV(MOTOR4_B); // all motor pins to 0
MC.latch_tx();
// enable both H bridges
pinMode(5, OUTPUT);
pinMode(6, OUTPUT);
digitalWrite(5, HIGH);
digitalWrite(6, HIGH);
#ifdef MICROSTEPPING
// use PWM for microstepping support
// use PWM for microstepping support
initPWM3(1);
initPWM4(1);
setPWM3(255);
setPWM4(255);
#endif
}
}
AF_DCMotor::AF_DCMotor(uint8_t num, uint8_t freq) {
motornum = num;
pwmfreq = freq;
switch (num) {
case 1:
pinMode(MOTOR1_DIR, OUTPUT);
digitalWrite(MOTOR1_DIR, LOW);
initPWM1(freq);
break;
case 2:
pinMode(MOTOR2_DIR, OUTPUT);
digitalWrite(MOTOR2_DIR, LOW);
initPWM2(freq);
break;
}
}
AF_Stepper::AF_Stepper(uint16_t steps, uint8_t num) {
MC.enable();
revsteps = steps;
steppernum = num;
currentstep = 0;
if (steppernum == 1) {
latch_state &= ~_BV(MOTOR1_A) & ~_BV(MOTOR1_B) &
~_BV(MOTOR2_A) & ~_BV(MOTOR2_B); // all motor pins to 0
MC.latch_tx();
// enable both H bridges
pinMode(11, OUTPUT);
pinMode(3, OUTPUT);
digitalWrite(11, HIGH);
digitalWrite(3, HIGH);
// use PWM for microstepping support
initPWM1(STEPPER1_PWM_RATE);
initPWM2(STEPPER1_PWM_RATE);
setPWM1(255);
setPWM2(255);
} else if (steppernum == 2) {
latch_state &= ~_BV(MOTOR3_A) & ~_BV(MOTOR3_B) &
~_BV(MOTOR4_A) & ~_BV(MOTOR4_B); // all motor pins to 0
MC.latch_tx();
// enable both H bridges
pinMode(5, OUTPUT);
pinMode(6, OUTPUT);
digitalWrite(5, HIGH);
digitalWrite(6, HIGH);
// use PWM for microstepping support
// use PWM for microstepping support
initPWM3(STEPPER2_PWM_RATE);
initPWM4(STEPPER2_PWM_RATE);
setPWM3(255);
setPWM4(255);
}
}
