In original sketch for motor control used two timers (Timer1, and Timer3) in CTC mode. These two timers, measure the period between two consecutive pulses of the STEP.
This approach is wasteful!
So, start saving - use only Timer1 and laid on him to work with sonar.
Analyzing the work Timer1, I chose a slightly different configuration of the base PinOut (reserved SPI pins for future use):
Code:
//Table PinOut Sparkfun Pro Micro:
// D0/RX -> PD2 (RXD1/INT2) : Serial1
// D1/TX -> PD3 (TXD1/INT3) : Serial1
// D2/SDA -> PD1 (SDA/INT1) : I2C
// D3/SCL -> PD0 (OC0B/SCL/INT0) : I2C
// D4 -> PD4 (ICP1/ADC8) : SONAR Echo
// D5 -> PC6 (OC3A/!OC4A) : DIR Motor1
// D6 -> PD7 (T0/OC4D/ADC10) : STEP Motor1
// D7 -> PE6 (INT.6/AIN0) : DIR Motor2
// D8 -> PB4 (PCINT4/ADC11) : STEP Motor2
// D9 -> PB5 (PCINT5/OC1A/!OC4B/ADC12) : SONAR Trigger
// D10 -> PB6 (PCINT6/OC1B/OC4B/ADC13) : SERVO
// D14/MISO -> PB3 (PDO/PCINT3/MISO) : SPI
// D15/SCLK -> PB1 (PCINT1/SCLK) : SPI
// D16/MOSI -> PB2 (PDI/PCINT2/MOSI) : SPI
// A0 -> PF7 (ADC7/TDI) : Battery monitor
// A1 -> PF6 (ADC6/TDO)
// A2 -> PF5 (ADC5/TMS)
// A3 -> PF4 (ADC4/TCK) : ENABLE Motors
#define SONAR_ECHO_PIN 4 // D4 -> PD4 (ICP1/ADC8)
#define MOTOR1_DIR_PIN 5 // D5 -> PC6 (OC3A/!OC4A)
#define MOTOR1_STEP_PIN 6 // D6 -> PD7 (T0/OC4D/ADC10)
#define MOTOR2_DIR_PIN 7 // D7 -> PE6 (INT.6/AIN0)
#define MOTOR2_STEP_PIN 8 // D8 -> PB4 (PCINT4/ADC11)
#define SONAR_TRIG_PIN 9 // D9 -> PB5 (PCINT5/OC1A/!OC4B/ADC12)
#define SERVO_PIN 10 // D10 -> PB6 (PCINT6/OC1B/OC4B/ADC13)
#define MOTORS_ENABLE_PIN A3 // A3 -> PF4 (ADC4/TCK)
Adding new variables:
Code:
#define TIMING_TRIG_PULSE 20 // 10us
#define CLR(x,y) (x&=(~(1<<y)))
#define SET(x,y) (x|=(1<<y))
int16_t speed_M1,speed_M2; // Actual speed of motors
int8_t dir_M1,dir_M2; // Actual direction of steppers motors
uint16_t period_M1, period_M2; // Actual period of steppers motors
uint16_t SonarStart = 0;
uint16_t SonarEnd = 0;
uint16_t SonarValue = 0;
float Distance = 0;
Actual code work stepper motors and sonar:
Code:
void delay_1us() // !!!calibrated on an oscilloscope
{
__asm__ __volatile__ (
"nop" "\n\t"
"nop" "\n\t"
"nop" "\n\t"
"nop" "\n\t"
"nop" "\n\t"
"nop" "\n\t"
"nop");
}
void eX_Motors_Sonar_Init(void)
{
dir_M1 = 0;
dir_M2 = 0;
TCCR1A = 0; // Timer1 in Normal Mode (WGM1x=0000) & disconnected OC1A, OC1B и OC1C (COMnx1:0=00)
TCCR1B = B00000010; // Prescaler=8, => 2Mhz (CS1x=010)
OCR1A = ZERO_SPEED; // Motor1 stopped
OCR1C = ZERO_SPEED; // Motor2 stopped
TCNT1 = 0;
OCR1B = TCNT1 + TIMING_TRIG_PULSE; // 10us
ICR1 = 0;
TIMSK1 = B00001111; // Enable Timer1 interrupts: OVF, COMPA, COMPB and COMPC
SET(PORTB,5); // front TRIG pulse or digitalWrite(SONAR_TRIG_PIN, HIGH);
CLR(PORTF,4); // motor enable or digitalWrite(MOTORS_ENABLE_PIN,LOW);
}
/// interrupts ///
ISR(TIMER1_OVF_vect) // прерывание наступает по переполнению timer1
{
if(!SonarEnd==0)
{
SonarValue = SonarEnd - SonarStart;
Distance = (Distance + SonarValue/116)/2;
SonarEnd = 0;
}
SonarStart = 0;
TIMSK1 = B00001111; // Enable interrupts: OVF, COMPA, COMPB and COMPC
OCR1B = TCNT1 + TIMING_TRIG_PULSE; // 10uS
SET(PORTB,5); // front TRIG pulse or digitalWrite(SONAR_TRIG_PIN, HIGH);
}
ISR( TIMER1_CAPT_vect ) // Input SONAR_ECHO_PIN level changed
{
if (TCCR1B & B01000000) // changed LOW to HIGH?
{
SonarStart = ICR1; // saved actual value TCNT1
ICR1 = 0;
TCCR1B = B10000010; // change mode CAPT
}
else
{
SonarEnd = ICR1;
ICR1 = 0;
TIMSK1 = B00001111; // Enable interrupts: OVF, COMPA, COMPB and COMPC
}
}
ISR( TIMER1_COMPB_vect ) // SONAR_TRIG end
{
CLR(PORTB,5); // or digitalWrite(SONAR_TRIG_PIN, LOW);
TCCR1B = B11000010; // Input SONAR_ECHO_PIN level changed
TIMSK1 = B00101111; // Enable interrupts CAPT
}
ISR(TIMER1_COMPA_vect) // interrupt STEP MOTOR1
{
if (dir_M1==0)
return;
if (dir_M1>0)
{
SET(PORTC,6); // DIR Motor 1 (Forward)
}
else
{
CLR(PORTC,6); // DIR Motor 2 (Revers)
}
OCR1A = OCR1A + period_M1;
SET(PORTD,7); // STEP MOTOR 1 (see table for PinOut D6)
delay_1us();
CLR(PORTD,7);
}
ISR( TIMER1_COMPC_vect ) // interrupt STEP MOTOR2
{
if (dir_M2==0)
return;
if (dir_M2>0)
{
CLR(PORTE,6); // DIR Motor 2 (Revers)
}
else
{
SET(PORTE,6); // DIR Motor 2 (Forward)
}
OCR1C = OCR1C + period_M2;
SET(PORTB,4); // STEP MOTOR 2 (see table for PinOut D8)
delay_1us();
CLR(PORTB,4);
}
The original procedures set speed engine be changed:
Replace the code in the procedure
setMotorSpeedM1(int16_t tspeed)
Code:
OCR1A = timer_period;
// Check if we need to reset the timer...
if (TCNT1 > OCR1A)
TCNT1 = 0;
in the code
Code:
period_M1 = timer_period;
Replace the code in the procedure
setMotorSpeedM2(int16_t tspeed)
Code:
OCR3A = timer_period;
// Check if we need to reset the timer...
if (TCNT3 > OCR3A)
TCNT3 = 0;
in the code
Code:
period_M2 = timer_period;
However, I replaced one of these two procedures:
Code:
void eX_Set_Motors_Speed(int16_t tspeed_M1, int16_t tspeed_M2)
{
long timer_period_1, timer_period_2;
int16_t speed_1, speed_2;
int8_t dir_1, dir_2;
// Лимитируем изменение скорости с учётом максимального ускорения для motor1
if ((speed_M1 - tspeed_M1)>MAX_ACCEL)
speed_M1 -= MAX_ACCEL;
else if ((speed_M1 - tspeed_M1)<-MAX_ACCEL)
speed_M1 += MAX_ACCEL;
else
speed_M1 = tspeed_M1;
// Лимитируем изменение скорости с учётом максимального ускорения для motor2
if ((speed_M2 - tspeed_M2)>MAX_ACCEL)
speed_M2 -= MAX_ACCEL;
else if ((speed_M2 - tspeed_M2)<-MAX_ACCEL)
speed_M2 += MAX_ACCEL;
else
speed_M2 = tspeed_M2;
#if MICROSTEPPING==16
speed_1 = speed_M1*46; // Adjust factor from control output speed to real motor speed in steps/second
speed_2 = speed_M2*46;
#else
speed_1 = speed_M1*23; // 1/8 Microstepping
speed_2 = speed_M2*23;
#endif
// Расчёт периода для motor1
if (speed_1==0)
{
timer_period_1 = ZERO_SPEED;
dir_1 = 0;
}
else if (speed_1>0)
{
timer_period_1 = 2000000/speed_1; // 2Mhz timer
dir_1 = 1;
}
else if (speed_1<0)
{
timer_period_1 = 2000000/-speed_1;
dir_1 = -1;
CLR(PORTC,6); // Dir Motor 1 (Revers)
}
if (timer_period_1 > 65535) // Check for minimun speed (maximun period without overflow)
timer_period_1 = ZERO_SPEED;
// Расчёт периода для motor2
if (speed_2==0)
{
timer_period_2 = ZERO_SPEED;
dir_2 = 0;
}
else if (speed_2>0)
{
timer_period_2 = 2000000/speed_2; // 2Mhz timer
dir_2 = 1;
}
else if (speed_2<0)
{
timer_period_2 = 2000000/-speed_2;
dir_2 = -1;
}
if (timer_period_2 > 65535) // Check for minimun speed (maximun period without overflow)
timer_period_2 = ZERO_SPEED;
dir_M1 = dir_1;
dir_M2 = dir_2;
period_M1 = timer_period_1;
period_M2 = timer_period_2;
}
In it, I almost simultaneously set the direction and rotation period of the motors.
Which option you choose is up to you ...
enjoy!
P.S. Claims on direct semantic nuances Google Russian-English translator.
Please post full sketch code.