Created October 31, 2017 © GPL3+

Jose, the particle robot

Did you ever wanted your cleaning robot to stop colliding around, or maybe that collaborated with another robots?

IntermediateWork in progress3 hours48

Things used in this project

Hardware components

Wandboard PICO-PI-IMX6UL

Texas Instruments Dual H-Bridge motor drivers L293D

Ultrasonic Sensor - HC-SR04 (Generic)

DC motor (generic)

SparkFun Photo Interrupter

Breadboard (generic)

Software apps and online services

Google Android Things

Microsoft Visual Studio 2015

Story

Automation can held much bennefits in household, and one of those is cleaning, generic cleaning robots can adress this issue in a suboptimal way, in which their plannig algorithms lead to suboptimal path planing, also, mapping is done making the robot collide with the enviroment, the third suboptimal factor is that the robot is not programed to cooperate in a swarm, this means that the group of robots can clean an already cleaned portion, those issues grow when you have pets, which often make their needs inside of the house...

Jose, the particle robot is here to help with those issues, although is still in development, one of the complexities that arrise is non-linear feedback control, also the need of higher order controls in order to make the robot able to plan a path and also know the enviroment, in this proyect the non-linear feedback control and odometry algorithm will be presented for control of diferential robot, all programed in an PICO-PI-IMX6UL (thanks Hackster :D).

The main steps in mobile robot control can be described as:

Planning

Location

Mapping

Control

For this proyect we will use volumetric mapping, in which a matrix represents free or occupied spaces, and each cell represents a portion of space ( like 10x10 centimeters), a probabilistic filter may be implemented for this, but we will assume that our sensors are not noisy, basically, the set of ecuations for knowing where an obstacle are:

X_obs=Measured_distance*sin(robot_angle + sensor_angle);

Y_obs=Measured_distance*cos(robot_angle + sensor_angle);

From that we will know where our obstacle are, Volumetric mapping is as simple as dropping a 1 when there's an obstacle in the map-like matrix where X_obs = matrix_column and Y_obs = matrix_row, and don't do anything where there is not, resolution of each cell may be considered, a simple way to tackle that is dividing our measurment between the resolution and rounding it:

matrix_column=round(X_obs/res);

matrix_row=round(Y_obs/res);

And after describing that, is where the real fun begins, because of that we now need a discrete path finding algorithm, such as dijkstra, dfs or A-star (which I implemented in matlab, didn't had time to make it in c# and upload it in google cloud, I want to implement it with some other heuristics and optimal controllers in order to make web based MPC or system identification), the only addition that I did to A-star is that a integral of cost is added to heuristic function to allow the algorithm to scape local minima, the rest of the code is explained by itself.

Another algorithm such as PSO can be combined to make coverage path planning in the presence of multiple robot, and that is the other heuristic algorithm i want to implement in google cloud to make the coverage.

I dont want to make the story to long, so for having location of the robot, the kinematic model is used to make it aware of its position in space (that is what we call odometry), velocity control can be done solving the equation

ref_vel=sqrt((x_odometry-x_objective)+(y_odometry-y_objective))

and direction can be computed as:

dir_ref= atan2((y_odometry-y_objective),(x_odometry-x_objective));

and

x_odometry=x_odometry+(robot_velocity*(sin(robot_angle))

y_odometry=y_odometry+(robot_velocity*(cos(robot_angle))

robot_angle=robot_angle+(diference_in_wheels)

also:

robot_velocity=(rigth_wheel+left_wheel)/2

diference_in_wheels= (rigth_wheel-left_wheel)/distance_between_wheels

and I think thats all for this mobile robot control proyect, I want to say that distance readings and speed readings where to noisy in pico-pi, for that I implemented and alpha-beta filter, with which i won't bother you with details and just provide a wikipedia link to anyone interesed:

https://en.wikipedia.org/wiki/Alpha_beta_filter

Well, I think thats all, I want to thank hackster for shipping a PICO-PI-IMX6UL, the board is awesome and i fell in love with Android things platform, I will use it for higher order controls (vision, system identification, plannig and all sort of stuff) and industrial control of machines, It will be obvious in java code that I'm a contol engineer and not a mobile developer (It have benefits as I'm not library dependant), anyway, so much thanks Hackster :D

Btw, it's called particle robot because in swarm intelligence the members of a swarm are called particles.

The need of sensor fusion and kalman filtering is pretty obvious in this one

Code

function A_star(~)
clc, close all, clear all
mapa=[0 0 0 0 0 0 0;
            1 1 1 1 1 1 0;
            0 1 0 0 0 1 0;
            0 1 0 0 0 1 0;
            0 1 0 0 0 1 0;
            0 1 0 0 0 1 0;
            0 1 1 1 1 1 0;];
% mapa=[0 0 0 0 0 0 0;
%             1 1 1 1 1 1 0;
%             0 1 0 0 0 0 0;
%             0 1 1 1 1 1 0;
%             0 0 0 0 0 0 0;
%             0 0 0 0 0 0 0;
%             0 1 1 1 1 0 0;];
traverse=mapa; 
matriz_cost=mapa;


pos_inicial=[1,1];
pos_act_automata=pos_inicial;
pos_obj=[7,7];
cola_visitados=pos_inicial;
cola_no_visitados=[0,0];
cola_costo=0;
cont_cola=1;
cont_asignacion=1;
traverse(pos_inicial(1), pos_inicial(2))=1;
 while sum((pos_act_automata-pos_obj) ~= [0,0])
     cont_cola=1;
     for i=pos_act_automata(1)-1:pos_act_automata(1)+1
         for j=pos_act_automata(2)-1:pos_act_automata(2)+1
             if size(cola_visitados, 1) > 1
             [dis, boo]=Heuristica(pos_obj, mapa, [i,j], cola_visitados,  cola_visitados(size(cola_visitados, 1)-1,:),matriz_cost);
             else
                 [dis, boo]=Heuristica(pos_obj, mapa, [i,j], cola_visitados,pos_inicial,matriz_cost  );
             end
             if boo
                 cola_costo(cont_cola)=dis+sum(sqrt(pos_act_automata-[i,j]).^2);
                 %cola_costo(cont_cola)=dis+sum(abs(pos_act_automata-[i,j]));
               %  cola_no_visitados(cont_cola,:)=dis;
             else 
                 cola_costo(cont_cola)=inf;
                 %cola_no_visitados(cont_cola,:)=inf;
             end
             cola_no_visitados(cont_cola, :)=[i,j];
             cont_cola=cont_cola+1;
             
         end
     end
     [val_min, index]=min(cola_costo);
     cola_visitados(cont_asignacion+1,:)=cola_no_visitados(index,:);
     pos_act_automata= cola_visitados(size(cola_visitados,1),:);
     traverse(pos_act_automata(1), pos_act_automata(2))=cont_asignacion+1;
     %% this is the integral of cost matrix I talked about
     matriz_cost(pos_act_automata(1), pos_act_automata(2))=matriz_cost(pos_act_automata(1), pos_act_automata(2))+cola_costo(index);
     
     cont_asignacion=cont_asignacion+1;
     end
 traverse
 pos_act_automata
 cola_visitados
 matriz_cost
 bonito(cola_visitados, mapa, pos_obj);
 
 end
 
    function [dis, boo]=Heuristica(meta, mapa, pos,cola_visitados, pos_ant ,matriz_cost)
    boo=1;
    temp_cost=0;
    if pos(1)>0 && pos(2) >0 && pos(2)<8 && pos(1)<8
        
    if mapa(pos(1), pos(2)) ==1
        boo=0;           
    end
 
    end
     if pos(1)==0 || pos(2) ==0 || pos(2)==8 || pos(1)==8
         boo=0;           
     end
    for i=1:size(cola_visitados,1)
        if sum(cola_visitados(i,:)==pos(:)') == 2;
            %boo=0;   
            temp_cost=10;
            for j=1:size(cola_visitados,1)
                if sum(cola_visitados(i,:)==pos_ant(:)') == 2;
                    temp_cost=30;
                end
            end
     
            %break;
        end
    end
    temp_mat=0;
     if pos(1)>0 && pos(2) >0 && pos(2)<8 && pos(1)<8
    temp_mat=matriz_cost(pos(1), pos(2));
     end
    %dis=sum(abs(meta-pos))+ temp_cost+temp_mat;
    dis=sum(abs(meta-pos))+temp_mat;
    
        
        
    end
    %
    function bonito(cola_visitados, mapa, pos_obj)
    matriz_costo=zeros(size(cola_visitados, 1),size(cola_visitados, 1));
    matriz_costo(:,:)=inf;
    matriz_pos_x=zeros(size(cola_visitados, 1),size(cola_visitados, 1));
    matriz_pos_y=zeros(size(cola_visitados, 1),size(cola_visitados, 1));
    
    for i=1:size(cola_visitados, 1);
        pos_temp=(cola_visitados(i,:));
        for j=size(cola_visitados, 1):-1:i
            if (sqrt(sum((pos_temp)-cola_visitados(j,:)).^2))==0
                matriz_costo(i, j)=inf;
            %matriz_costo(i, j)=(sqrt(sum((pos_temp)-cola_visitados(j,:)).^2));
            else
               matriz_costo(i, j)=(sqrt(sum((pos_temp)-cola_visitados(j,:)).^2));
            end
            matriz_pos_x(i,j)=cola_visitados(j,1);
            matriz_pos_y(i,j)=cola_visitados(j,2);
        end
        [min_val,index]=min(matriz_costo(i, :));
        vec(i,:)=[matriz_pos_x(i,index),matriz_pos_y(i,index)];
    end
    vec
    size(vec, 1)
    
    end

package com.example.rauls.joseph_the_robot;

import android.app.Activity;

import android.os.Bundle;
import android.os.Handler;
import android.util.Log;

import com.google.android.things.pio.Gpio;
import com.google.android.things.pio.PeripheralManagerService;
import com.google.android.things.pio.Pwm;
import com.google.android.things.pio.UartDevice;

import java.io.IOException;
import vaf.vishal.hcsr04.Hcsr04;

public class MainActivity extends Activity {
    private static final String GPIO_TRIG = "GPIO4_IO19";
    private static final String GPIO_ECHO = "GPIO1_IO18";
    private static final String GPIO_gen = "GPIO2_IO01";//"GPIO4_IO23";//"GPIO2_IO01";
    private static final String GPIO_gen1 = "GPIO4_IO23";
    private static final String rigth_wheel = "GPIO4_IO22";
    private static final String left_wheel = "GPIO4_IO21";
    private static final String pwm_1= "PWM7";
    private static final String pwm_2= "PWM8";

    //https://www.facebook.com/electronicadelfuturo2020/videos/1626165270740394/
    //private static final String GPIO_ECHO = "GPIO4_IO19";
    //private static final String GPIO_TRIG = "GPIO1_IO18";
    float dist = -1;
    private Handler mHandler = new Handler();
    private static final String TAG = MainActivity.class.getSimpleName();
    private Gpio echo;
    private Gpio trig;
    private Gpio led;
    private Gpio led1;
    private Gpio r_w;
    private Gpio l_w;
    private final static float SOUND_SPEED = 340.29f; // speed of sound in m/s
    private final static int TRIG_DURATION_IN_MICROS = 10; // trigger duration
    private final static int TIMEOUT = 21000;
    private static final int INTERVAL_BETWEEN_BLINKS_MS = 100;
    private boolean State = false;
    private double x_pred = 0.0;

    private double x_est = 0.0;
    private double x_est1 = 0.0;
    private double x_est2 = 0.0;
    private double dx = 0.0;
    private double residual = 0.0;
    private double h = 0.00001;
    private double g = 0.00212;
    private double[] state_vector = {0.0, 0.0, 0.0};
    Hcsr04 hcsr04;
    private double ul_process_variance=0.0;
    private double ul_noise_variance=0.0;
    private double x_est_ant;
    private double l_time_ant;
    private double r_time_ant;
    private double l_speed;
    private double r_speed;
    double r_now;
    double l_now;
    private int l_rpm_count=-1;
    private int r_rpm_count=-1;
    private int l_rpm_count_ant=-1;
    private int r_rpm_count_ant=-1;
    private boolean r_state = false;
    private boolean l_state=false;
    private static final String UART_DEVICE_NAME = "UART3";

    private UartDevice mDevice;
    private int k;
    private Pwm Pw1;
    private Pwm Pw2;


    long time1, time2;
    int keepBusy;

    @Override
    protected void onCreate(Bundle savedInstanceState) {
        super.onCreate(savedInstanceState);
        // setContentView(R.layout.activity_main);
        PeripheralManagerService service = new PeripheralManagerService();
        try {
            mDevice = service.openUartDevice(UART_DEVICE_NAME);
        } catch (IOException e) {
            e.printStackTrace();
        }
        try {
            configureUartFrame(mDevice);
            Log.w(TAG, "Ya se configuro");
        }
        catch (IOException e){
            Log.w(TAG, "No se pudo configurar", e);
        }

        try {
            // Set everything we need
            Pw1 = service.openPwm(pwm_1);
            Pw1.setPwmFrequencyHz(120);
            Pw2 = service.openPwm(pwm_2);
            Pw2.setPwmFrequencyHz(120);
            echo = service.openGpio(GPIO_ECHO);
            led = service.openGpio(GPIO_gen);

            led.setDirection(Gpio.DIRECTION_OUT_INITIALLY_LOW);
            led1 = service.openGpio(GPIO_gen1);
            trig = service.openGpio(GPIO_TRIG);
            r_w=service.openGpio(rigth_wheel);
            l_w=service.openGpio(left_wheel);

            r_w.setDirection(Gpio.DIRECTION_IN);
            l_w.setDirection(Gpio.DIRECTION_IN);

            led1.setDirection(Gpio.DIRECTION_OUT_INITIALLY_LOW);
            trig.setDirection(Gpio.DIRECTION_OUT_INITIALLY_LOW);

            echo.setDirection(Gpio.DIRECTION_IN);
            echo.setEdgeTriggerType(Gpio.EDGE_BOTH);
            echo.setActiveType(Gpio.ACTIVE_HIGH);

            Log.i(TAG, "Corriendo el runnable");

            mHandler.post(mMainRunnable);
            // Runs our function

        } catch (IOException e) {
            e.printStackTrace();
        }


        //.start();


    }

    //@Override
    @Override
    protected void onDestroy() {
        super.onDestroy();
        // Remove pending blink Runnable from the handler.
        mHandler.removeCallbacks(mMainRunnable);
        // Close the Gpio pin.
        Log.i(TAG, "Closing LED GPIO pin");
        try {
            Pw1.close();
            Pw2.close();
            Pw2 = null;
            Pw1 = null;
        } catch (IOException e) {
            e.printStackTrace();
        }


        if (mDevice != null) {
            try {
                mDevice.close();
                mDevice = null;

            } catch (IOException e) {
                Log.w(TAG, "Unable to close UART device", e);
            }
        }

        try {
            echo.close();
            trig.close();
            led.close();
        } catch (IOException e) {
            Log.e(TAG, "Error on PeripheralIO API", e);
        } finally {
            echo = null;
            trig = null;
            led = null;
        }

    }


    private Runnable mMainRunnable = new Runnable() {
        @Override
        public void run() {
            // Exit Runnable if the GPIO is already closed
            double aca = 0;


            if (echo == null || trig == null) {
                return;
            }


            try {
                trig.setValue(false);
                Thread.sleep(0, 2000);
                trig.setValue(true);
                Thread.sleep(0, 10000); //10 microsec
                trig.setValue(false);

                time1 = System.nanoTime();

                while (echo.getValue() == true) {
                    Thread.sleep(0, 1);

                }
                time2 = System.nanoTime();

            } catch (IOException e) {
                e.printStackTrace();
            } catch (InterruptedException e) {
                e.printStackTrace();
            }


            // Attempt to read arduino with distance
            /*
            try {
                k=  readUartBuffer(mDevice);
                //Log.d(TAG, String.valueOf(k));
            } catch (IOException e) {
                e.printStackTrace();
            }
            */

            // State Change detection, Usefull for Speed measurment
            try {
                if (r_w.getValue() != r_state) {
                    if (r_w.getValue() == true) {
                        r_rpm_count += 1;
                        r_now = System.nanoTime();
                    }
                }
            } catch (IOException e) {
                e.printStackTrace();
            }
            try {
                r_state = r_w.getValue();
            } catch (IOException e) {
                e.printStackTrace();
            }

            try {
                if (l_w.getValue() != l_state) {
                    if (l_w.getValue() == true) {
                        l_rpm_count += 1;
                        l_now = System.nanoTime();
                    }
                }
            } catch (IOException e) {
                e.printStackTrace();
            }
            try {
                l_state = l_w.getValue();
            } catch (IOException e) {
                e.printStackTrace();
            }


            // derivates

            l_speed = (l_rpm_count - l_rpm_count_ant);

            r_speed = r_rpm_count - r_rpm_count_ant;


            //Log.i(TAG, "Echo ENDED!");

            // Measure how long the echo pin was held high (pulse width)
            long pulseWidth = time2 - time1;

            // Calculate distance in centimeters. The constants
            // are coming from the datasheet, and calculated from the assumed speed
            // of sound in air at sea level (~340 m/s).
            //double distance = (pulseWidth / 1000.0 ) / 58.23 ; //cm
            double distance = ((pulseWidth / 1000.0) / 29.0 / 2.0) * 0.26; //cm
            /* more arduino attemp
                if (k<=0){
                    k=250;
                }
                if(x_est >250.00){
                    x_est=0.0;
                }*/

            // an alpha-beta filter
            ul_process_variance = ul_process_variance + Math.pow(distance, 2);
            ul_noise_variance = ul_noise_variance + Math.pow((distance - x_pred), 2);
            double gamma = ((g / h));
            double weird_r = ((4 - gamma - (Math.sqrt((8.0 * gamma) + Math.pow(gamma, 2)))) / 4.0);
            h = 1 - Math.pow(weird_r, 2);
            g = 2 * (2 - h) - 4 * Math.sqrt(1 - h);


            x_pred = x_est + (dx);
            residual = distance - x_pred;
            dx = dx + (h * residual);
            x_est = x_pred + (g * residual);
            if (x_est < 0) {
                x_est = 0.0;
            }

            x_est_ant = x_est;
            // end of alpha-beta filter

  /*        this is still the attempt
            try {
                if (k > 25 ) {
                    try {
                        led.setValue(true);
                    } catch (IOException e) {
                        e.printStackTrace();
                    }
                } else if (k<= 25  )  {
                    State = !State;
                    led.setValue(false);
                    led1.setValue(!State);
                }
                */

            // objective position
            double objective[] = {100, 0};
            // blanding factor for filtering
            double balance = 0.3;

            // another filter (exponetial smothing)
            x_est2 = ((3.14 * 6.5) * (l_speed / 20.0) * balance) + (x_est2 * (1 - balance));
            x_est1 = ((3.14 * 6.5) * (r_speed / 20.0) * balance) + (x_est1 * (1 - balance));


            // speed reference
            double vel = Math.sqrt(Math.pow(objective[0] - state_vector[0], 2) + Math.pow(objective[1] - state_vector[1], 2));
            // direction reference
            double dir = Math.atan2((objective[1] - state_vector[1]), (objective[0] - state_vector[0]));
            // angle error
            double ang_dif = Math.atan2(Math.cos(dir - state_vector[2]), Math.sin(dir - state_vector[2]));
            // control signals
            double ck = 10 * vel + (15.0 * ang_dif);
            double ck2 = 10 * vel - (15.0 * ang_dif);
            // call of control outputs
            try {
                setDuty(ck);
                setDuty1(ck2);
            } catch (IOException e1) {
                e1.printStackTrace();
            }



            // more mathematics, or odometry equations, or kinematic model
            state_vector[0] = state_vector[0] + 1 * ((((x_est + x_est1) / 2)) * Math.sin(state_vector[2]));
            state_vector[1] = state_vector[1] + 1 * ((((x_est + x_est1) / 2)) * Math.cos(state_vector[2]));
            state_vector[2] = state_vector[2] + 1 * (((x_est - x_est1) / 15));
            // helped me debbugging, must comment
            Log.d(TAG, String.valueOf(l_speed) + " " + state_vector[0] + " " + state_vector[1] + " " + state_vector[2] + " " + r_rpm_count + " " + k);

            // this is a continous process
            mHandler.postDelayed(mMainRunnable, 5);

            // part of diferential equations
            l_rpm_count_ant = l_rpm_count;
            r_rpm_count_ant = r_rpm_count;

        }
    };




    public void configureUartFrame(UartDevice uart) throws IOException {
        // Configure the UART port
        uart.setBaudrate(9600);
        uart.setDataSize(8);
        uart.setParity(UartDevice.PARITY_NONE);
        uart.setStopBits(1);
    }
    public int readUartBuffer(UartDevice uart) throws IOException {
        // Maximum amount of data to read at one time
        final int maxCount = 8;
        byte[] buffer = new byte[maxCount];


        int count;

        uart.read(buffer, buffer.length);
        int pl=0;
        for (count = 0; count < buffer.length; count++) {
            if((int)(buffer[count])> pl){
                pl=(int)(buffer[count]);
            }
            //Log.w(TAG,  String.valueOf(buffer[count]));
        }

        return (pl);

    }
    public void setDuty(double duty) throws IOException {
        // for any reason, the compiler didn't let me set a continous value to setPwmDutyCycle(continous)
        Pw1.setEnabled(true);
        if (duty>=0 && duty <5 || duty <0) {
            Pw1.setPwmDutyCycle(0);
        }
        else if(duty>=5 && duty <15) {
            Pw1.setPwmDutyCycle(10);
        }else if(duty>=15 && duty <25) {
            Pw1.setPwmDutyCycle(20);
        }
        else if(duty>=25 && duty <35) {
            Pw1.setPwmDutyCycle(30);
        }
        else if(duty>=35 && duty <45) {
            Pw1.setPwmDutyCycle(40);
        }
        else if(duty>=45 && duty <55) {
            Pw1.setPwmDutyCycle(40);
        }
        else if(duty>=55 && duty <65) {
            Pw1.setPwmDutyCycle(60);
        }
        else if(duty>=65 && duty <75) {
            Pw1.setPwmDutyCycle(70);
        }
        else if(duty>=75 && duty <85) {
            Pw1.setPwmDutyCycle(80);
        }
        else if(duty>=85 && duty <95) {
            Pw1.setPwmDutyCycle(90);
        }
        else if(duty>=95 ) {
            Pw1.setPwmDutyCycle(100);
        }



    }
    public void setDuty1(double duty) throws IOException {
        Pw2.setEnabled(true);
        if (duty>=0 && duty <5 || duty <0) {
            Pw2.setPwmDutyCycle(0);
        }
        else if(duty>=5 && duty <15) {
            Pw2.setPwmDutyCycle(10);
        }else if(duty>=15 && duty <25) {
            Pw2.setPwmDutyCycle(20);
        }
        else if(duty>=25 && duty <35) {
            Pw2.setPwmDutyCycle(30);
        }
        else if(duty>=35 && duty <45) {
            Pw2.setPwmDutyCycle(40);
        }
        else if(duty>=45 && duty <55) {
            Pw2.setPwmDutyCycle(40);
        }
        else if(duty>=55 && duty <65) {
            Pw2.setPwmDutyCycle(60);
        }
        else if(duty>=65 && duty <75) {
            Pw2.setPwmDutyCycle(70);
        }
        else if(duty>=75 && duty <85) {
            Pw2.setPwmDutyCycle(80);
        }
        else if(duty>=85 && duty <95) {
            Pw2.setPwmDutyCycle(90);
        }
        else if(duty>=95 ) {
            Pw2.setPwmDutyCycle(100);
        }


    }
}

Credits

Raul Sosa Cortés

1 project • 1 follower

I got a master's degree in control and automation, and i have nothing left to say

Jose, the particle robot

Things used in this project

Hardware components

Software apps and online services

Story

Schematics

Schematic

Code

A-Star

Android things control

Credits

Raul Sosa Cortés

Comments

Embed the widget on your own site

Jose, the particle robot

Jose, the particle robot

Things used in this project

Hardware components

Software apps and online services

Story

Schematics

Schematic

Code

A-Star

Android things control

Credits

Raul Sosa Cortés

Comments

Related channels and tags