Emanuele86
Published © GPL3+

RC Android Rover with GPS Tracker Using DaguRobot and ARTe

An RC rover with GPS tracker, controlled by an Android application, made leveraging the ARTe Arduino real-time extension

IntermediateProtip446
RC Android Rover with GPS Tracker Using DaguRobot and ARTe

Things used in this project

Story

Read more

Code

MainActivity.java

Java
package com.example.myapplication;


import android.Manifest;
import android.content.BroadcastReceiver;
import android.content.Context;
import android.content.Intent;
import android.content.IntentFilter;
import android.content.pm.PackageManager;
import android.graphics.Color;
import android.net.wifi.ScanResult;
import android.net.wifi.WifiConfiguration;
import android.net.wifi.WifiManager;
import android.os.AsyncTask;
import android.support.v4.app.ActivityCompat;
import android.support.v4.app.FragmentActivity;
import android.support.v4.content.ContextCompat;
import android.support.v7.app.AppCompatActivity;
import android.os.Bundle;
import android.util.Log;
import android.view.MotionEvent;
import android.view.View;
import android.widget.ArrayAdapter;
import android.widget.Button;
import android.widget.ListView;
import android.widget.TextView;
import android.widget.Toast;

import com.google.android.gms.maps.CameraUpdateFactory;
import com.google.android.gms.maps.GoogleMap;
import com.google.android.gms.maps.MapFragment;
import com.google.android.gms.maps.OnMapReadyCallback;
import com.google.android.gms.maps.model.LatLng;
import com.google.android.gms.maps.model.MarkerOptions;
import com.google.android.gms.maps.model.Polyline;
import com.google.android.gms.maps.model.PolylineOptions;

import java.io.BufferedWriter;
import java.io.IOException;
import java.io.OutputStreamWriter;
import java.io.PrintWriter;
import java.net.InetAddress;
import java.net.InetSocketAddress;
import java.net.Socket;
import java.net.UnknownHostException;
import java.util.ArrayList;
import java.util.List;

//public class MainActivity extends AppCompatActivity {
public class MainActivity extends FragmentActivity implements OnMapReadyCallback {
    final String AccessWifi = Manifest.permission.ACCESS_WIFI_STATE;
    final String ChangeWifi = Manifest.permission.CHANGE_WIFI_STATE;
    final String CoarseLoc = Manifest.permission.ACCESS_COARSE_LOCATION;
    final String FineLoc = Manifest.permission.ACCESS_FINE_LOCATION;
    final String ChangeNet = Manifest.permission.CHANGE_NETWORK_STATE;
    final String AccessNet = Manifest.permission.ACCESS_NETWORK_STATE;
    final String Internet = Manifest.permission.INTERNET;
    public static final int ID_PERM=1;

    WifiManager wifi;
    ListView lv;
    int size = 0;
    List<ScanResult> results;
    ArrayList<String> arraylist = new ArrayList<>();
    ArrayAdapter adapter;
    public boolean findWifi=true;
    public boolean connected=false;
    //private static final int SERVERPORT = 333;
    //private static final String SERVER_IP = "192.168.4.1";
    public TCPClient mTcpClient;

    private GoogleMap mMap;
    private Polyline polyline;
    private PolylineOptions polyline_opt;
    private List<LatLng> points;
    TextView textState;
    TextView textCmd;
    Button butLstop;
    Button butRstop;

    /*
    @Override
    public void onRequestPermissionsResult(int requestCode, String permissions[], int[] grantResults) {
        switch (requestCode) {
            case ID_PERM: {
                if (grantResults.length > 0 && grantResults[0] == PackageManager.PERMISSION_GRANTED) {
                    // permission concessa: eseguiamo il codice

                } else {
                    // permission negata: provvediamo in qualche maniera
                }
                return;
            }
        }
    }
    */

    @Override
    protected void onCreate(Bundle savedInstanceState) {
        super.onCreate(savedInstanceState);
        setContentView(R.layout.activity_main);
        // Permessi

        int permiStateAWifi = ContextCompat.checkSelfPermission(this, AccessWifi);
        int permiStateCWifi = ContextCompat.checkSelfPermission(this, ChangeWifi);
        int permiStateCLoc = ContextCompat.checkSelfPermission(this, CoarseLoc);
        int permiStateFLoc = ContextCompat.checkSelfPermission(this, FineLoc);
        int permiStateCNet = ContextCompat.checkSelfPermission(this, ChangeNet);
        int permiStateANet = ContextCompat.checkSelfPermission(this, AccessNet);
        int permiStateInt = ContextCompat.checkSelfPermission(this, Internet);

        if (permiStateAWifi != PackageManager.PERMISSION_GRANTED && permiStateCWifi != PackageManager.PERMISSION_GRANTED &&
                permiStateCLoc != PackageManager.PERMISSION_GRANTED && permiStateFLoc != PackageManager.PERMISSION_GRANTED
                && permiStateCNet != PackageManager.PERMISSION_GRANTED && permiStateInt != PackageManager.PERMISSION_GRANTED
                && permiStateANet != PackageManager.PERMISSION_GRANTED) {
            ActivityCompat.requestPermissions(this, new String[]{AccessWifi, ChangeWifi, CoarseLoc, FineLoc, ChangeNet,AccessNet, Internet},ID_PERM);
        }

        /**/
        wifi = (WifiManager) getApplicationContext().getSystemService(Context.WIFI_SERVICE);
        if (wifi.isWifiEnabled() == false)
        {
            Toast.makeText(getApplicationContext(), "wifi is disabled..making it enabled", Toast.LENGTH_LONG).show();
            wifi.setWifiEnabled(true);
        }
        IntentFilter intentFilter = new IntentFilter();
        intentFilter.addAction(wifi.SCAN_RESULTS_AVAILABLE_ACTION);
        getApplicationContext().registerReceiver(wifiScanReceiver, intentFilter);

        //scanWifiNetworks();
        lv = (ListView)findViewById(R.id.list);
        adapter =  new ArrayAdapter<String>(this,android.R.layout.simple_list_item_1,arraylist);
        lv.setAdapter(adapter);

        MapFragment mapFragment = (MapFragment) getFragmentManager().findFragmentById(R.id.map);
        mapFragment.getMapAsync(this);
        points= new ArrayList<>();
        textState= findViewById(R.id.textView2);
        textCmd= findViewById(R.id.textView3);
        butLstop=findViewById(R.id.button10);
        butRstop=findViewById(R.id.button11);


        butLstop.setOnTouchListener(new View.OnTouchListener() {
            @Override
            public boolean onTouch(View v, MotionEvent event) {
                if(event.getAction() == MotionEvent.ACTION_DOWN) {
                    textCmd.setText("L STOP");
                    if (mTcpClient != null) {
                        try {
                            mTcpClient.sendMessage("a");
                        } catch (IOException e) {
                            e.printStackTrace();
                        }
                    }
                } else if (event.getAction() == MotionEvent.ACTION_UP) {
                    if (textCmd.getText()=="L STOP" && mTcpClient != null) {
                        try {
                            mTcpClient.sendMessage("w");
                            textCmd.setText("Forward");
                        } catch (IOException e) {
                            e.printStackTrace();
                        }
                    }
                }

                return true;
            }
        });

        butRstop.setOnTouchListener(new View.OnTouchListener() {
            @Override
            public boolean onTouch(View v, MotionEvent event) {
                if(event.getAction() == MotionEvent.ACTION_DOWN) {
                    textCmd.setText("R STOP");
                    if (mTcpClient != null) {
                        try {
                            mTcpClient.sendMessage("d");
                        } catch (IOException e) {
                            e.printStackTrace();
                        }
                    }
                } else if (event.getAction() == MotionEvent.ACTION_UP) {
                    if (textCmd.getText()=="R STOP" && mTcpClient != null) {
                        try {
                            mTcpClient.sendMessage("w");
                            textCmd.setText("Forward");
                        } catch (IOException e) {
                            e.printStackTrace();
                        }
                    }
                }

                return true;
            }
        });
    }

    @Override
    public void onMapReady(GoogleMap googleMap) {
        mMap = googleMap;
        //polyline_opt = new PolylineOptions().width(5).color(Color.BLUE);
        //polyline = mMap.addPolyline(polyline_opt);
    }
    /*
    public void addToMap(View view){
        Polyline line = mMap.addPolyline(new PolylineOptions()
                .add(new LatLng(41, 9), new LatLng(42, 9))
                .width(5)
                .color(Color.RED));

    }
    */
    BroadcastReceiver wifiScanReceiver = new BroadcastReceiver() {
        @Override
        public void onReceive(Context c, Intent intent) {
            boolean success = intent.getBooleanExtra(wifi.EXTRA_RESULTS_UPDATED, false);
            if (success && !findWifi) {
                scanSuccess();
            }
        }
    };


    public void WifiCon (View view){
        findWifi=false;
        arraylist.clear();
        boolean success = wifi.startScan();
        if (!success) {
            // scan failure handling
            Toast.makeText(this, "Scan Failure", Toast.LENGTH_SHORT).show();
        }
    }


    private void scanSuccess() {
        results=wifi.getScanResults();
        size=results.size();
        //Toast.makeText(this, "Scanning.... " + size, Toast.LENGTH_SHORT).show();
        for (ScanResult result : results) {
            //arraylist.add(result.SSID);
            //adapter.notifyDataSetChanged();
            if (result.SSID.equals("INO_ESP8266")){
                Toast.makeText(this, "Trovato ", Toast.LENGTH_SHORT).show();
                setSsidAndPassword(getApplicationContext(),"INO_ESP8266","12345678",wifi);
                break;
            }
        }
        //getApplicationContext().unregisterReceiver(wifiScanReceiver);
        //lv.setAdapter(adapter);
    }


    public boolean setSsidAndPassword(Context context, String ssid, String ssidPassword, WifiManager wifi) {
        try {

            WifiConfiguration conf = new WifiConfiguration();
            conf.SSID = "\"" + ssid+ "\"";
            //conf.preSharedKey = "\""+ ssidPassword +"\"";
            //conf.preSharedKey = null;
            conf.allowedKeyManagement.set(WifiConfiguration.KeyMgmt.NONE);
            int idNet=wifi.addNetwork(conf);
            if (idNet>=0){
                findWifi=true;
                wifi.disconnect();
                wifi.enableNetwork(idNet, true);
                wifi.reconnect();
            }
            return true;
        } catch (Exception e) {
            e.printStackTrace();
            return false;
        }

    }


    public void ArdConDis (View view) throws  IOException{
        if (!connected) {
            arraylist.clear();
            adapter.notifyDataSetChanged();
            new connectTask().execute("");
            connected=true;
        } else {

            if (mTcpClient != null) {
                mTcpClient.stopClient();
                textState.setText("Disconnected");
                connected=false;
            }
        }
    }

    /*
    public class connectTask extends AsyncTask<String,Void,TCPClient> {
        @Override
        protected TCPClient doInBackground(String... message) {
            mTcpClient = new TCPClient();
            mTcpClient.run();
            return null;
        }
    }
    */

    public class connectTask extends AsyncTask<String,String,TCPClient> {
        @Override
        protected TCPClient doInBackground(String... message) {
            //we create a TCPClient object and
            mTcpClient = new TCPClient(new TCPClient.OnMessageReceived() {
                @Override
                //here the messageReceived method is implemented
                public void messageReceived(String message) {
                    //this method calls the onProgressUpdate
                    publishProgress(message);
                }
            });
            mTcpClient.run();
            return null;
        }


        @Override
        protected void onProgressUpdate(String... values) {
            super.onProgressUpdate(values);
            String strInput;
            strInput=values[0];
            //in the arrayList we add the messaged received from server
            arraylist.add(strInput);
            adapter.notifyDataSetChanged();
            if (strInput.contains("connected")){
                textState.setText("Connected");
            } else {
                updateLine(strInput);
            }
        }
    }

    public void updateLine(String latlon){
        int firsindx=latlon.indexOf(':');
        int lastindx=latlon.indexOf(',');
        float lat = Float.parseFloat(latlon.substring(firsindx+1,lastindx-1));
        float lon = Float.parseFloat(latlon.substring(lastindx+1));
        LatLng actualLatLon = new LatLng(lat, lon);
        points.add(actualLatLon);
        polyline_opt = new PolylineOptions().width(10).color(Color.BLUE);
        if (points.size()>2) {
            polyline_opt.addAll(points);
            polyline = mMap.addPolyline(polyline_opt);
        }
        mMap.moveCamera(CameraUpdateFactory.newLatLngZoom(actualLatLon,17.0f));

    }


    public void sendLforward(View view) throws IOException {
        textCmd.setText("Left forward");
        if (mTcpClient != null) {
            mTcpClient.sendMessage("q");
            Toast.makeText(this, "Send q", Toast.LENGTH_SHORT).show();
        }
    }

    public void sendLback(View view) throws IOException{
        textCmd.setText("Left back");
        if (mTcpClient != null) {
            mTcpClient.sendMessage("z");
            Toast.makeText(this, "Send z", Toast.LENGTH_SHORT).show();
        }
    }

    public void sendRforward(View view) throws IOException{
        textCmd.setText("Rigth forward");
        if (mTcpClient != null) {
            mTcpClient.sendMessage("e");
            Toast.makeText(this, "Send e", Toast.LENGTH_SHORT).show();
        }
    }

    public void sendRback(View view) throws IOException{
        textCmd.setText("Rigth back");
        if (mTcpClient != null) {
            mTcpClient.sendMessage("c");
            Toast.makeText(this, "Send c", Toast.LENGTH_SHORT).show();
        }
    }

    public void sendStop(View view) throws IOException{
        textCmd.setText("Rigth back");
        if (mTcpClient != null) {
            mTcpClient.sendMessage("s");
            Toast.makeText(this, "Send s", Toast.LENGTH_SHORT).show();
        }
    }

    public void sendForw(View view) throws IOException{
        textCmd.setText("Forward");
        if (mTcpClient != null) {
            mTcpClient.sendMessage("w");
            Toast.makeText(this, "Send w", Toast.LENGTH_SHORT).show();
        }
    }
    public void sendBack(View view) throws IOException{
        textCmd.setText("Back");
        if (mTcpClient != null) {
            mTcpClient.sendMessage("x");
            Toast.makeText(this, "Send x", Toast.LENGTH_SHORT).show();
        }
    }
}

"TCPClient" class

Java
package com.example.myapplication;

import android.provider.ContactsContract;
import android.util.Log;

import java.io.BufferedReader;
import java.io.BufferedWriter;
import java.io.DataInputStream;
import java.io.DataOutputStream;
import java.io.IOException;
import java.io.InputStreamReader;
import java.io.OutputStreamWriter;
import java.io.PrintWriter;
import java.net.InetAddress;
import java.net.InetSocketAddress;
import java.net.Socket;
import java.net.SocketImpl;

import static java.io.DataInputStream.readUTF;

public class TCPClient {

    public static final String SERVER_IP = "192.168.4.1"; //"192.168.4.1";
    public static final int SERVER_PORT = 333;
    public Socket socket;
    // while this is true, the client will continue running
    private boolean mRun = false;
    // sends message received notifications
    private OnMessageReceived mMessageListener;
    private DataOutputStream DataOut;
    private DataInputStream DataIn;
    private String msg;
    byte[] buf;
    private String cmd="";
    private boolean sending=false;
    /**
     * Constructor of the class. OnMessagedReceived listens for the messages received from server
     */
    public TCPClient(OnMessageReceived listener) {
        mMessageListener = listener;
    }
    /**
     * Sends the message entered by client to the server
     *
     * @param message text entered by client
     */


    public void sendMessage(String message) throws IOException {
        cmd=message;
    }
    /*
    public void sendMessage(final String message)  {
        Runnable runnable = new Runnable() {
            @Override
            public void run() {
                if (DataOut != null && msg==null) {
                    try {
                        Log.i("Debug", "Send command");
                        DataOut.write(message.getBytes(),0,message.length());
                        DataOut.flush();
                        //DataOut.close();
                    } catch (IOException e) {
                        e.printStackTrace();
                    }
                }
            }
        };
        Thread thread = new Thread(runnable);
        thread.start();
    }
    */
    /**
     * Close the connection and release the members
     */
    public void stopClient() throws IOException {
        Log.i("Debug", "Stop Client");
        // send mesage that we are closing the connection
        //sendMessage(Constants.CLOSED_CONNECTION + "Kazy");
        mRun = false;
        if (DataOut != null && socket!=null) {
            DataOut.flush();
            DataOut.close();
        }
        if (DataIn != null && socket!=null) {
            DataIn.close();
        }
        socket.close();
        mMessageListener = null;
        DataIn=null;
        DataOut = null;
        msg = null;
        cmd="";
        //socket=null;
    }

    public void run() {
        int nb;
        mRun = true;
        try {
            //here you must put your computer's IP address.
            InetAddress serverAddr = InetAddress.getByName(SERVER_IP);
            Log.i("Debug", "Connecting...");
            // use to send the command to server
            //create a socket to make the connection with the server
            //socket = new Socket(serverAddr,SERVER_PORT);
            //if (socket==null) {
            socket = new Socket();
            socket.connect(new InetSocketAddress(SERVER_IP, SERVER_PORT), 1000 * 60);
            //}
            try {
                DataOut = new DataOutputStream(socket.getOutputStream());
                //receives the message which the server sends back
                //mBufferIn = new BufferedReader(new InputStreamReader(socket.getInputStream()));
                DataIn = new DataInputStream(socket.getInputStream());
                Log.i("Debug", "Connecting OK");
                buf= new byte[1024];
                Log.i("Debug", "Inside try catch");
                //in this while the client listens for the messages sent by the server
                while (mRun) {
                    if (DataOut != null && cmd != "") {
                        DataOut.write(cmd.getBytes(), 0, cmd.length());
                        DataOut.flush();
                        sending=true;
                        //cmd = "";
                        Log.i("Debug", "Send cmd: " + cmd);
                    }
                    if (!socket.isClosed()) {
                        nb = DataIn.read(buf);
                        if (nb != -1) {
                            msg = new String(buf, 0, nb);
                            if (sending && msg.contains("ok")){
                                sending=false;
                                cmd="";
                            } else if (!msg.contains("ok")){
                                mMessageListener.messageReceived(msg);
                            }
                            Log.i("Debug", "Recived : " + msg);
                            msg = null;
                        }
                    }
                }
                //Log.i("Debug", "Received Message:");
            } catch (Exception e) {
                Log.e("Debug", "Send Error", e);
            }finally {
                //the socket must be closed. It is not possible to reconnect to this socket
                // after it is closed, which means a new socket instance has to be created.
                /*
                DataOut.flush();
                DataOut.close();
                DataIn.close();
                socket.close();
                */
            }
        } catch (Exception e) {
            Log.e("Debug", "Connecting Error", e);
        }
    }

    //Declare the interface. The method messageReceived(String message) will must be implemented in the MyActivity
    //class at on asynckTask doInBackground

    public interface OnMessageReceived {
        public void messageReceived(String message);
    }

}

ARTe Arduino Sketch

Arduino
#include <Ultrasonic.h>
#include <Motor.h>
#include <WiFiESP8266.h>
#include <Adafruit_GPS.h>
// Attach GPS module to Arduino Due Serial2
#define GPSSerial Serial2
#define PMTK_SET_NMEA_UPDATE_1HZ  "$PMTK220,1000*1F"
#define PMTK_SET_NMEA_UPDATE_5HZ  "$PMTK220,200*2C"
#define PMTK_SET_NMEA_UPDATE_10HZ "$PMTK220,100*2F"

// turn on only the second sentence (GPRMC)
#define PMTK_SET_NMEA_OUTPUT_RMCONLY "$PMTK314,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*29"
// turn on GPRMC and GGA
#define PMTK_SET_NMEA_OUTPUT_RMCGGA "$PMTK314,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*28"
// turn on ALL THE DATA
#define PMTK_SET_NMEA_OUTPUT_ALLDATA "$PMTK314,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0*28"
// turn off output
#define PMTK_SET_NMEA_OUTPUT_OFF "$PMTK314,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*28"
#define PMTK_Q_RELEASE "$PMTK605*31"
//------------------------------------------------
// Pins connected to Ultrasonic
const int ul_trig = 43;
const int ul_eco = 41;
// Pins connected to H-bridge to drive motors
const int mR_en = 3;
const int mL_en = 2;
const int mR_A = 31;
const int mR_B = 30;
const int mL_A = 33;
const int mL_B = 32;
// Pins connected to Encoder
const int enR_sen = 24;
const int enL_sen = 26;
// Wi-Fi module attached to Arduino Due Serial1
// Netwoek parameters
String Network = "INO_ESP8266";
String Password = "12345678";
String KeyType = "2";
String Channel = "1";
//------------------------------------------------
// Duty Cycle Max and Min used to PWM driving of motors :
const int dc_min = 3000;
const int dc_max = 4096;
// Encoder variables
const int CFL = 20;         // Rising for a lap
static unsigned long cL;    // Left rising counter
static unsigned long cR;    // Right rising counter
static unsigned long turnTimeL; //Time used to control right turn of vehicle
static unsigned long turnTimeR; //Time used to control left turn of vehicle
static unsigned long t;     // Arduino time
static float rpsL;          // Estimated Left wheel velocity
static float rpsR;          // Estimated Right wheel velocity
static int dcL;             // Duty cycle used for left wheel when control is active
static int dcR;             // Duty cycle used for right wheel when control is active


// Buffer used to store the data recived from Wi-Fi
String Buffer;
// Buffer used to store the data will send by Wi-Fi
String Data;
// Buffer used to store the command recived from Wi-Fi or from PC
String Command;
// Buffer used to store the data recived from GPS
String BufferGPS;

static bool start;      // set to true after Wi-Fi module initialization
static bool wifiCon;    // set to true after a client is connected to network
static bool okfound;    // set to true during  Wi-Fi module initialization
static bool readyfound; // set to true during  Wi-Fi module initialization
static bool motFon;     // set to true when vheicle go forward
static bool motBon;     // set to true when vheicle go back
static bool motRotR;    // set to true when vheicle turn Left
static bool motRotL;    // set to true when vheicle turn Right
static bool controlStartL;  // set to true when control of left  wheel is enable
static bool controlStartR;  // set to true when control of right  wheel is enable
static bool GPSstart;       // set to true after GPS module initialization
static bool GPSfix;         // set to true after reciving a fix from GPS
//------------------------------------------------
// Function used to select the command to execute
void SendCommand(int dc_L, int dc_R) {
  static Motor L_motor(mL_en, mL_A, mL_B);
  static Motor R_motor(mR_en, mR_A, mR_B);
  //Serial.println(Command);
  // Motors commands
  if (Command == "q") {
    // Veicle turn Left
    turnTimeR = millis();
    controlStartL = false;
    controlStartR = true;
    motRotR = true;
    L_motor.stopMot();
    R_motor.stopMot();
    delay(1);
    //R_motor.forward(dc_R);
    R_motor.forward(dc_max); // turn left the vehicle trought right wheel
    //L_motor.forward(dc_min); // this line can be commented on
    Command = "";
  }
  else if (Command == "z") {
    // Veicle turn Left
    controlStartL = false;
    controlStartR = false;
    L_motor.back(dc_L);     // turn left the vehicle trought left wheel
    Command = "";
  }
  else if (Command == "a") {
    // Stop Left wheel
    controlStartL = false;
    controlStartR = false;
    L_motor.stopMot();
    Command = "";
  }
  else if (Command == "e") {
    // Veicle turn Right
    turnTimeL = millis();
    controlStartR = false;
    controlStartL = true;
    motRotL = true;
    R_motor.stopMot();
    L_motor.stopMot();
    delay(1);
    //L_motor.forward(dc_L);
    L_motor.forward(dc_max);  // turn right the vehicle trought left wheel
    //R_motor.forward(dc_min);  // this line can be commented on
    Command = "";
  }
  else if (Command == "c") {
    // Veicle turn Right
    controlStartL = false;
    controlStartR = false;
    R_motor.back(dc_R);       // turn right the vehicle trought right wheel
    Command = "";
  }
  else if (Command == "d") {
    // Stop Rigth wheel
    controlStartL = false;
    controlStartR = false;
    R_motor.stopMot();
    Command = "";
  }
  else if (Command == "s") {
    // Stop vehicle
    controlStartL = false;
    controlStartR = false;
    motFon = false;
    motBon = false;
    L_motor.stopMot();
    R_motor.stopMot();
    Command = "";
  }
  else if (Command == "w") {
    // Vehicle Forward
    controlStartL = true;
    controlStartR = true;
    motFon = true;
    motBon = false;
    L_motor.forward(dc_L);
    R_motor.forward(dc_R);
    Command = "";
  }
  else if (Command == "x") {
    // Vehicle Back
    controlStartL = true;
    controlStartR = true;
    motBon = true;
    motFon = false;
    L_motor.back(dc_L);
    R_motor.back(dc_R);
    Command = "";
  }
}
//------------------------------------------------
void setup() {
  Serial.begin(115200);
  // Serial for wi-fi communaication
  Serial1.begin (115200);

  // Attach motors pins
  analogWriteResolution(12);
  pinMode(mL_en, OUTPUT);
  pinMode(mR_en, OUTPUT);
  pinMode(mL_A, OUTPUT);
  pinMode(mL_B, OUTPUT);
  pinMode(mR_A, OUTPUT);
  pinMode(mR_B, OUTPUT);
  // Attach Encoder pins and interrupts
  pinMode(enL_sen, INPUT_PULLUP);
  pinMode(enR_sen, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(enL_sen), IncLc, RISING);
  attachInterrupt(digitalPinToInterrupt(enR_sen), IncRc, RISING);

  t = 0;
  cL = 0;
  cR = 0;
  rpsL = 0;
  rpsR = 0;
  dcL = 0;
  dcR = 0;

  Buffer = "";
  Data = "";
  Command = "";
  BufferGPS = "";

  start = false;
  wifiCon = false;
  okfound = false;
  readyfound = false;
  motFon = false;
  motBon = false;
  controlStartL = false;
  controlStartR = false;
  motRotR = false;
  motRotL = false;
  GPSstart = false;
  GPSfix = false;
}
//------------------------------------------------
void loop() {
}
// Function attached to interrupt of left wheel
void IncLc()
{
  cL++;
}
// Function attached to interrupt of right wheel
void IncRc()
{
  cR++;
}
//------------------------------------------------
// TASKS:
// T1: Control left wheel
void loop1(100) {
  static int k = 0;
  const int Ns = 10;     // number of samplings
  const int dcMax = 4096;// Max duty cycle
  const int dcMin = 1000;// Min duty cycle
  const int dcNom = 3600;// Nominal duty cycle
  const double v_ref = 1.5;// Velocity of reference
  const double KpL = 150;// K proportional
  const double KiL = 80;// K integrative
  //static int dcL = 0;
  static unsigned long tk[Ns];  // Time samples
  static unsigned long ckL[Ns]; // Rises detetcted from encoder
  static double vL[Ns];         // Velocity
  static double eL[Ns];         // Error
  static double eiL = 0.0;      // Integral of error
  static double vsL = 0.0;      // Sum of velocity
  static double vmL = 0.0;      // Mean velocity
  static unsigned long cRotL = 0;// Used to control the number of rotation of wheel
  static bool firstStartL = true;
  static bool rotLstart = false;
  static unsigned long AcL;   // Rise variation
  static unsigned long AcT;   // Time variation
  //------------------------------------------------
  tk[k] = millis();
  ckL[k] = cL;
  cL = 0;
  if (firstStartL) {
    vL[0] = 0;
    eL[0] = 0;
    vmL = 0;
    AcT = 0;
    AcL = 0;
    k = 1;
    firstStartL = false;
    //controlStartL = true;
  }
  else {
    if (k == 0) {
      // Compute changes ad time variation
      AcT = tk[k] - tk[Ns - 1];
      AcL = ckL[k];
      vL[k] = vL[Ns - 1];
      vsL = vL[k];
      // Compute Error
      //eL[k] = v_ref - vL[k];
      eL[k] = v_ref - vmL;
      // Compute integral of Error
      eiL = (eL[k] + eL[Ns - 1]) * (AcT / 2);
    }
    else {
      // Compute changes ad time variation
      AcT = tk[k] - tk[k - 1];
      AcL = ckL[k];
      // Compute velocity of wheels
      vL[k] = 50.0 * double(AcL) / double(AcT);
      // Compute mean velocity of wheels
      vsL += vL[k];
      vmL = vsL / double(k + 1);
      // Compute Error
      //eL[k] = v_ref - vL[k];
      eL[k] = v_ref - vmL;
      // Compute integral of Error
      eiL = (eL[k] + eL[k - 1]) * (AcT / 2);
    }
    // compute Duty Cycle
    //Serial.println(String(k)+" : "+String(vL[k]));
    dcL = dcNom + int(KpL * eL[k]) + int(KiL * eiL);
    // Limit DC left wheel
    if (dcL > dcMax) {
      dcL = dcMax;
    }
    if (dcL < dcMin) {
      dcL = dcMin;
    }
    if (controlStartL) {
      // Send control command when control start
      if (motRotL) {
        //cRotL += AcL;
        //Serial.println(cRotL);
        //if (cRotL >= 10) {
        if (tk[k] - turnTimeL >= 500) {
          cRotL = 0;
          motRotL = false;
          if (motFon) {
            Command = "w";
          } else if (motBon) {
            Command = "x";
          } else {
            Command = "s";
          }
        }
        SendCommand(dcL, dcR);
      }
      else if (motFon) {
        Command = "w";
        SendCommand(dcL, dcR);
      }
      else if (motBon) {
        Command = "x";
        SendCommand(dcL, dcR);
      }
    }
    k++;
    if (k == Ns) {
      rpsL = vmL; // store data
      //cL = 0;
      //vmL = 0;
      k = 0;
    }
  }
}

// T2: Control right wheel
void loop2(100) {
  static int k = 0;
  const int Ns = 10;     // number of samplings
  const int dcMax = 4096;// Max duty cycle
  const int dcMin = 1000;// Min duty cycle
  const int dcNom = 3600;// Nominal duty cycle
  const double v_ref = 1.5;// Velocity of reference
  const double KpR = 150;// K proportional
  const double KiR = 80;// K integrative
  //static int dcR = 0;
  static unsigned long tk[Ns];
  static unsigned long ckR[Ns]; // R trans for sampling
  static double vR[Ns];         // Velocity
  static double eR[Ns];         // Error
  static double eiR = 0.0;      // Integral of error
  static double vsR = 0.0;
  static double vmR = 0.0;
  static unsigned long cRotR = 0;// Used to control the number of rotation of wheel
  static bool firstStartR = true;
  static bool rotRstart = false;
  static unsigned long AcR;   // Rise variation
  static unsigned long AcT;   // Time variation
  //------------------------------------------------
  tk[k] = millis();
  ckR[k] = cR;
  cR = 0;
  if (firstStartR) {
    vR[0] = 0;
    eR[0] = 0;
    vmR = 0;
    AcT = 0;
    AcR = 0;
    k = 1;
    firstStartR = false;
  }
  else {
    if (k == 0) {
      // Compute changes ad time variation
      AcT = tk[k] - tk[Ns - 1];
      AcR = ckR[k];
      vR[k] = vR[Ns - 1];
      vsR = vR[k];
      // Compute Error
      //eR[k] = v_ref - vR[k];
      eR[k] = v_ref - vmR;
      // Compute integral ofError
      eiR = (eR[k] + eR[Ns - 1]) * (AcT / 2);
    }
    else {
      // Compute changes ad time variation
      AcT = tk[k] - tk[k - 1];
      AcR = ckR[k];
      // Compute velocity of wheels
      vR[k] = 50.0 * double(AcR) / double(AcT);
      // Compute mean velocity of wheels
      vsR += vR[k];
      vmR = vsR / double(k + 1);
      // Compute Error
      //eR[k] = v_ref - vR[k];
      eR[k] = v_ref - vmR;
      // Compute integral ofError
      eiR = (eR[k] + eR[k - 1]) * (AcT / 2);
    }
    // Compute Duty Cycle
    dcR = dcNom + int(KpR * eR[k]) + int(KiR * eiR);
    // Limit DC right wheel
    if (dcR > dcMax) {
      dcR = dcMax;
    }
    if (dcR < dcMin) {
      dcR = dcMin;
    }
    if (controlStartR) {
      // Send control command
      if (motRotR) {
        //cRotR += AcR;
        //Serial.println(cRotL);
        //if (cRotR >= 10) {
        if (tk[k] - turnTimeR >= 500) {
          cRotR = 0;
          motRotR = false;
          if (motFon) {
            Command = "w";
          } else if (motBon) {
            Command = "x";
          } else {
            Command = "s";
          }
        }
        SendCommand(dcL, dcR);
      }
      else if (motFon) {
        Command = "w";
        SendCommand(dcL, dcR);
      }
      else if (motBon) {
        Command = "x";
        SendCommand(dcL, dcR);
      }
    }
    k++;
    if (k == Ns) {
      //rpsR = float(ckR[Ns - 1]) / CFL;
      rpsR = vmR; // store data
      //cR = 0;
      //vmR=0;
      k = 0;
    }
  }
}
// T3: Test PC serial and W-Fi serial
void loop3(50) {
  static int indx = 0;
  // Test if serial command is available
  if (Serial.available() > 0) {
    Command = Serial.readString();
    SendCommand(dc_max, dc_max);
  }
  // Test if serial WiFi is available
  while (Serial1.available () > 0) {
    Buffer = Serial1.readStringUntil('\r');
    Serial.print(Buffer);
    if ( Buffer.lastIndexOf("+IPD") >= 0 ) {
      indx = Buffer.lastIndexOf(":");
      Command = Buffer.substring(indx + 1);
      //Serial.print("Wi-Fi command: ");
      //Serial.print(Command);
      SendCommand(dc_max, dc_max);
      Data = "ok";
      Buffer = "";
    }
    else if (Buffer.lastIndexOf("Link") >= 0) {
      //Serial.println("");
      //Serial.println("Connected");
      Data = "connected";
      wifiCon = true;
      Buffer = "";
    }
    else if (Buffer.lastIndexOf("Unlink") >= 0) {
      //Serial.println("");
      //Serial.println("Disconnected");
      //Serial1.flush();
      Data = "";
      wifiCon = false;
      Buffer = "";
    }
    else if ( Buffer.lastIndexOf("OK") >= 0 ) {
      okfound = true;
      Buffer = "";
    }
    else if ( Buffer.lastIndexOf("ready") >= 0 ) {
      readyfound = true;
      Buffer = "";
    }
    //Buffer = "";
  }
}
// T4: Obstacle detecting
void loop4(200) {
  float cmM;
  static Ultrasonic ultrasonic(ul_trig, ul_eco);
  cmM = float(ultrasonic.distanceRead(CM));
  //Serial.print("Sensore di distanza [cm]: ");
  //Serial.println(cmMsec);
  if ( cmM <= 5 ) {
    Command = "s";
    SendCommand(0, 0);
  }
}
// T5: WiFi handle
void loop5(500) {
  static WiFiESP8266 wifi(Network, Password, KeyType, Channel);
  if (!start) {
    // WiFi initialize
    wifi.init();
    delay(100);
    wifi.SetSoftAPmode();
    delay(100);
    wifi.CreateServer();
    delay(100);
    Serial.println("START WiFi OK");
    start = true;
  } else {
    if (Data != "") {
      if ( !wifiCon && start) {
        Serial.println(Data);
        //Serial.println(datastr);
        Data = "";
      }
      else if ( wifiCon) { //&& Serial1.available () <= 0) {
        while (Serial1.availableForWrite() <= 0);
        // WiFi send data
        wifi.SendDataRequest(Data);
        //Serial1.flush();
        Data = "";
      }
      //Data = "";
    }
  }
  // WiFi command info
  if (Command == "1") {
    wifi.GetIP_ClientPC();
    Command = "";
  }
}
// T6: Data to send creation and GPS handler
void loop6(500) {
  static String datastr = "";
  static float lat = 0;
  static float lon = 0;
  // Connect to the GPS on the hardware port
  static Adafruit_GPS GPS(&GPSSerial);
  t = millis();
  if (start) {
    if (!GPSstart ) {
      GPS.begin(9600);
      GPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY);
      while (GPSSerial.available())
        GPS.read();
      Serial.print("\nSTARTING LOGGING....");
      if (GPS.LOCUS_StartLogger())
        Serial.println(" STARTED! ");
      else
        Serial.println(" no response :(");
      GPSstart = true;
    } else {
      while (GPSSerial.available () > 0) {
        BufferGPS += GPS.read();
      }
      if (GPS.newNMEAreceived()) {
        //Serial.println(GPS.lastNMEA()); // sets the newNMEAreceived() flag to false
        if (!GPS.parse(GPS.lastNMEA())) // sets the newNMEAreceived() flag to false
          return; // we can fail to parse a sentence in which case we should just wait for another
        lat = GPS.latitudeDegrees;
        lon = GPS.longitudeDegrees;
        GPSfix = GPS.fix;
      }
      BufferGPS = "";
    }
    if (Data == "") {
      if (!wifiCon) {
        datastr = String(t);
        datastr += ",";
        datastr += String(rpsL);
        datastr += ",";
        datastr += String(rpsR);
        Data = datastr;
      }
      else {
        if (GPSfix) {
          datastr = String(t);
          datastr += ":";
          datastr += String(lat, 6);
          datastr += ",";
          datastr += String(lon, 6);
          Data = datastr;
        } else {
          Data = "connected";
        }
      }
    }
  }
}

The "Motor" library

C/C++
/*
 * Motor.cpp
 *
*/
//home/manu/.arduino15/packages/arduino/hardware/sam/1.6.11/cores/arduino
#include <Arduino.h>
#include "Motor.h"

Motor::Motor(int enPin,int APin, int BPin) {
	en=enPin;  	
	A=APin;
	B=BPin;
	analogWrite(en,0);
}

//_________________________________ATTUAZIONE MOTORE_________________________________
void Motor::forward(int D){
  	analogWrite(en,D);
	digitalWrite(A,LOW);	
	digitalWrite(B,HIGH);
}

void Motor::back(int D){
	analogWrite(en,D);
	digitalWrite(A,HIGH);
    	digitalWrite(B,LOW);
}


void Motor::stopMot(){
	analogWrite(en,0);
}

The "wifi" lib

C/C++
/*
 * WiFiESP8266.cpp
 *
*/
#include <Arduino.h>
#include <string>
#include <stdio.h>
//#include <iostream>
#include "WiFiESP8266.h"
using namespace std;

// char*=std::string&
WiFiESP8266::WiFiESP8266(String Network,String Password,String Channel,String KeyType) {
	Net=Network;
	Pas=Password;
	Cha=Channel;
	KeyTy=KeyType;
	buffer="";
	connected=false;
}

void WiFiESP8266::init(){
	//Serial1.begin (115200);
	//restart ESP8266
    	Serial1.print ("AT+RST\r");
	delay(10);
}

void WiFiESP8266::SetSoftAPmode(){
  String Com;
  //Serial.println("Input followin information");
  Serial.print ("Network: ");
  //Network=ReadString();
  Serial.println (Net);
  Serial.print ("Password: ");
  //Password=ReadString();
  Serial.println (Pas);
  Serial.print ("Channel: ");
  //Channel=ReadString();
  Serial.println (Cha);
  Serial.print ("Key Type: ");
  //KeyType=ReadString();
  Serial.println (KeyTy);
  //Wi-Fi mode station-softAP (STA-SAP)
  Serial1.print ("AT+CWMODE=3\r");
  delay(10);
  // Set configuration of SAP mode
  Com ="AT+CWSAP=\"";
  Com +=Net;
  Com +="\",\"";
  Com +=Pas;
  Com +="\",";
  Com +=Cha;
  Com +=",";
  Com +=KeyTy;
  Com +="\r";
  Serial1.print(Com);
  delay(10);
  //Serial1.print("AT+RST\r");
}

void WiFiESP8266::CreateServer(){
  Serial.println("Create server: ");
  // enable multi connection
  Serial1.print("AT+CIPMUX=1\r");
  delay(10);
  //delay(25);
  // CIPSERVER=mode,port
  //    mode=1: create server;
  //    mode=0: delete serever;
  //    port: number of port (333 defoult)
  Serial1.print("AT+CIPSERVER=1,333\r");
  delay(10);
  // set timeout time in a range (0-7200 [second])
  Serial1.print("AT+CIPSTO=120\r");
  delay(10);
}

void WiFiESP8266::GetIP_ClientPC(){
  Serial.print("IP rover: ");
  Serial1.print("AT+CIPSTATUS\r");
  delay(10);
  Serial.println("");
  Serial.print("IP connesso: ");
  Serial1.print("AT+CWLIF\r");
  delay(10);
}
/*
void ESP8266::WiFi_SendData(){
  if (connected){
    Serial.println("Data to send:");
    if(Serial.available()>0){
      data=Serial.readString();
      Serial.println(data);
      SendData(data);
    }
  }
}

void ESP8266::SendData(String dat){
  String Com;
  bool sendper=true;
  Com="AT+CIPSEND=0,";
  Com +=String(dat.length());
  Com += "\r";
  //Serial1.print("AT+CIPSEND=0,");
  //Serial1.print(dat.length());
  //Serial1.print("\r");
  Serial1.print(Com);
  while (sendper){
    if (Serial1.find ('>')){
      Serial1.print(dat);
      Serial1.print("\r");
      //Serial.print (" > ");
      //Serial.print(dat);
      sendper=false;
    }
  }
}*/
void WiFiESP8266::SendDataRequest(String dat){
  String Com;
  Com="AT+CIPSEND=0,";
  Com +=String(dat.length());
  Com += "\r";
  Serial1.print(Com);
  Com=dat;
  Com+="\r";
  if (  Serial1.find('>') ) {
      Serial1.print(Com);
  }
}

void WiFiESP8266::TestSerialWiFi(){
  while (Serial1.available () > 0){
    buffer=Serial1.readStringUntil('\r');
    //Serial.print(buffer);
    if (buffer=="Link"){
      //Serial.println("");
      //Serial.println("Connesso");
      connected=true;
    }    
  }
}

void WiFiESP8266::CloseTCPconnection(){
  Serial.println("Close:");
  Serial1.print("AT+CIPCLOSE\r");
  delay(10);
}

Credits

Emanuele86

Emanuele86

0 projects • 0 followers

Comments