Jose, the particle robot

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);
        }


    }
}