eX-Robot. New Ideas and Solutions for Self Balancing Robot

[KomX]

When opening a new topic, I want to tell here about their findings, the changes made and plans to expand the functionality of the B-Robot.

I'll start with the concept laid the basis for a rethinking of the basic version:
- The robot must be relatively cheap;
- Should be able to be upgraded in terms of both hardware and software.

Following this concept, have been replaced with stepper motors (17HS2408), ultrasonic ranger (HC-SR04), Wi-Fi module (ESP8266) and battery (Li-Ion 5000mAh). Added logic levels converter bi-directional module, step-up voltage converters (DC-DC 3.7V to 12V) to power the stepper motors, two step-down voltage converter (DC-DC 12V to 5V and 12V to 3.3V) and charging module (TP4056x4 4.2V 3A) for Li-Ion battery.

Today, the preamble is completed.
In the next post I will discuss how to ensure the sonar and control two motors with a single timer.
See you.

[KomX]

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.

[KomX]

Hi!
Under our future ideas need free memory and time for their execution.
Now we will unleash 16 bytes of memory and a 100 microsecond time.
So, we will upgrade the original procedure for measuring the battery voltage. The modernization will improve measurement accuracy and free up 16 bytes of memory and a 100 microsecond time.
The original code (not the library version of the code):

Code:

#define BATT_VOLT_FACTOR 8

boolean first_time = true;

int readBattery()
{
  if (first_time)
    battery = analogRead(5)/BATT_VOLT_FACTOR;
  else
    battery = (battery*9 + (analogRead(5)/BATT_VOLT_FACTOR))/10;
  return battery;
}

Replace a new code:

Code:

uint16_t   tbattery = 0;

int readBattery() // c
{
  battery = (battery*9 + (tbattery/BATT_VOLT_FACTOR))/10; //
  ADMUX = (1<<REFS0) | (1<<REFS1);                       // Vref = 2.56В, for A5 (ADC0) all MUX5..0=0
  ADCSRA |= (1<<ADSC) | (1<<ADIE);                       // start ADC & enable interrupt
  return battery;
}

ISR(ADC_vect)                    // ready data
{
  tbattery = ADCL;
  tbattery |= ADCH<<8;
}

NOTE: In the original code BATT_VOLT_FACTOR = 8 selected for maximum battery voltage (Vin) equal to 12.5 volts and a voltage reference source Vref = 5 volt. Using a resistive divider R3(33K) end R5(22K).
The new code uses a built-in stabilized power supply with Vref = 2.56V. This limits the voltage at pin A5 and require different resistors R3, R5 and constant BATT_VOLT_FACTOR.

Question developers:
How to solve the problem of monitoring the battery on the board Pro Micro? In it there is no pin A5!

[sasa999]

(06-11-2015 07:34 PM)KomX Wrote:  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;

Do you mean change these 4 lines to this 1 line or something different?? I tried make these changes, but unsuccefull Please post full sketch code.

[KomX]

Replacement of which you asked, it is only required if you change the pinout connections of motors and use the program works only with Timer1. Then you can do the replacement or use a program at the end of the post you are interested in.

[KomX]

We continue to analyze the code in order to optimize resources.
Analysis of motor control program demonstrates that the pulses on STEP motors Nicodemus different drivers do not come at the same time. The analysis of the work of the drivers said that the level of the signal DIR is important only at the time of arrival of a signal STEP.
It can be concluded that for normal operation quite the same control signal STEP to service two drivers.
Set free for future use pin D5!

[Wampo]

Hy KomX,

are your Posted Codes are the Complete Code or it is just a Update, for better running?

Lets Show us some Videos from your Robot or Pictures.
Did you are running with a Pro Mini?

Can you send me the Complete Code?

Greetings from Germany

Wampo

[KomX]

(10-14-2015 10:54 AM)Wampo Wrote:  are your Posted Codes are the Complete Code or it is just a Update, for better running?

Placed in this topic codes only small routines and changes to the core code sketch version 1.2.
You know, I tried to adapt the code for Pro Mini, making maximum use of hardware features and functionality inherent in the conclusions of the chip.
So I still remains free SPI interface, for example.
Additional analysis of the drivers of stepper motors allowed to carry out the direction of rotation control (DIR) one signal.

Quote:Lets Show us some Videos from your Robot or Pictures.

Unfortunately, the robot dismantled in anticipation of fundamental changes.

Quote:Can you send me the Complete Code?

Always!
If it will help you ...

PS. Added subroutine light status indication.

PPS. Code of crude. Comments do not always correspond to the logic of the moment.

PPPS. ".txt" Extension to remove

[vlelectronic]

Hello KomX,
The "dir" link them together?
How does a wheel to move forward and backward?
Thank you
greetings VL

[KomX]

(10-14-2015 07:19 PM)vlelectronic Wrote:  Hello KomX,
The "dir" link them together?
How does a wheel to move forward and backward?
Thank you
greetings VL

If you look at the documentation on the drivers of stepper motors, you will see that the signals "DIR" and "STEP" comes into the input register. And the logic of the scheme is run signal "STEP" and requires a steady level on the line "DIR" (0 or 1). After removal of the signal "STEP", you can arbitrarily change the signal "DIR" and this will not affect the driver of the motor. It is this behavior and was included in the program. Since the motor control signals generated in the interrupt routine, and by default Arduino nested interrupts are disabled, the interrupt routine for the left and right motors are processed sequentially, without affecting each other. Thus, if we require that the motors rotate in opposite directions, we expose one motor one signal "DIR" (the same signal is supplied and a second motor) and confirm its signal "STEP" for the engine (since the second motor signal "STEP" does not arrive, the driver of the motor does not "see" signal value "DIR" which it is not intended.
Something like that