├── src
├── main
│ ├── res
│ │ ├── mipmap-hdpi
│ │ │ └── ic_launcher.png
│ │ ├── mipmap-mdpi
│ │ │ └── ic_launcher.png
│ │ ├── mipmap-xhdpi
│ │ │ └── ic_launcher.png
│ │ ├── mipmap-xxhdpi
│ │ │ └── ic_launcher.png
│ │ ├── mipmap-xxxhdpi
│ │ │ └── ic_launcher.png
│ │ ├── values
│ │ │ ├── strings.xml
│ │ │ ├── colors.xml
│ │ │ ├── dimens.xml
│ │ │ └── styles.xml
│ │ ├── values-v21
│ │ │ └── styles.xml
│ │ ├── values-w820dp
│ │ │ └── dimens.xml
│ │ ├── menu
│ │ │ └── menu_device_list.xml
│ │ └── layout
│ │ │ ├── content_device_list.xml
│ │ │ ├── activity_device_list.xml
│ │ │ ├── activity_gps_controll.xml
│ │ │ └── content_gps_controll.xml
│ ├── AndroidManifest.xml
│ └── java
│ │ └── com
│ │ └── example
│ │ └── artur_000
│ │ └── followmerobot
│ │ ├── DeviceList.java
│ │ └── GpsControll.java
├── test
│ └── java
│ │ └── com
│ │ └── example
│ │ └── artur_000
│ │ └── followmerobot
│ │ └── ExampleUnitTest.java
└── androidTest
│ └── java
│ └── com
│ └── example
│ └── artur_000
│ └── followmerobot
│ └── ApplicationTest.java
├── FollowMe
├── Definitions.h
└── FollowMe.ino
└── README.md
/src/main/res/mipmap-hdpi/ic_launcher.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/akuramshin/Follow-Me-Robot/HEAD/src/main/res/mipmap-hdpi/ic_launcher.png
--------------------------------------------------------------------------------
/src/main/res/mipmap-mdpi/ic_launcher.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/akuramshin/Follow-Me-Robot/HEAD/src/main/res/mipmap-mdpi/ic_launcher.png
--------------------------------------------------------------------------------
/src/main/res/mipmap-xhdpi/ic_launcher.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/akuramshin/Follow-Me-Robot/HEAD/src/main/res/mipmap-xhdpi/ic_launcher.png
--------------------------------------------------------------------------------
/src/main/res/mipmap-xxhdpi/ic_launcher.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/akuramshin/Follow-Me-Robot/HEAD/src/main/res/mipmap-xxhdpi/ic_launcher.png
--------------------------------------------------------------------------------
/src/main/res/mipmap-xxxhdpi/ic_launcher.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/akuramshin/Follow-Me-Robot/HEAD/src/main/res/mipmap-xxxhdpi/ic_launcher.png
--------------------------------------------------------------------------------
/src/main/res/values/strings.xml:
--------------------------------------------------------------------------------
1 |
2 | FollowMeRobot
3 | Settings
4 | GpsControll
5 |
6 |
--------------------------------------------------------------------------------
/src/main/res/values/colors.xml:
--------------------------------------------------------------------------------
1 |
2 |
3 | #3F51B5
4 | #303F9F
5 | #FF4081
6 |
7 |
--------------------------------------------------------------------------------
/src/main/res/values/dimens.xml:
--------------------------------------------------------------------------------
1 |
2 |
3 | 16dp
4 | 16dp
5 | 16dp
6 |
7 |
--------------------------------------------------------------------------------
/src/main/res/values-v21/styles.xml:
--------------------------------------------------------------------------------
1 | >
2 |
3 |
9 |
10 |
--------------------------------------------------------------------------------
/src/main/res/values-w820dp/dimens.xml:
--------------------------------------------------------------------------------
1 |
2 |
5 | 64dp
6 |
7 |
--------------------------------------------------------------------------------
/src/test/java/com/example/artur_000/followmerobot/ExampleUnitTest.java:
--------------------------------------------------------------------------------
1 | package com.example.artur_000.followmerobot;
2 |
3 | import org.junit.Test;
4 |
5 | import static org.junit.Assert.*;
6 |
7 | /**
8 | * To work on unit tests, switch the Test Artifact in the Build Variants view.
9 | */
10 | public class ExampleUnitTest {
11 | @Test
12 | public void addition_isCorrect() throws Exception {
13 | assertEquals(4, 2 + 2);
14 | }
15 | }
--------------------------------------------------------------------------------
/src/androidTest/java/com/example/artur_000/followmerobot/ApplicationTest.java:
--------------------------------------------------------------------------------
1 | package com.example.artur_000.followmerobot;
2 |
3 | import android.app.Application;
4 | import android.test.ApplicationTestCase;
5 |
6 | /**
7 | * Testing Fundamentals
8 | */
9 | public class ApplicationTest extends ApplicationTestCase {
10 | public ApplicationTest() {
11 | super(Application.class);
12 | }
13 | }
--------------------------------------------------------------------------------
/src/main/res/menu/menu_device_list.xml:
--------------------------------------------------------------------------------
1 |
11 |
--------------------------------------------------------------------------------
/src/main/res/values/styles.xml:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 |
10 |
11 |
15 |
16 |
17 |
18 |
19 |
20 |
21 |
--------------------------------------------------------------------------------
/src/main/res/layout/content_device_list.xml:
--------------------------------------------------------------------------------
1 |
2 |
14 |
15 |
23 |
24 |
30 |
31 |
--------------------------------------------------------------------------------
/src/main/AndroidManifest.xml:
--------------------------------------------------------------------------------
1 |
2 |
4 |
5 |
6 |
7 |
8 |
9 |
10 |
11 |
17 |
21 |
22 |
23 |
24 |
25 |
26 |
27 |
31 |
32 |
33 |
34 |
--------------------------------------------------------------------------------
/FollowMe/Definitions.h:
--------------------------------------------------------------------------------
1 | // Pin variables
2 | #define GPS_TX_PIN 6
3 | #define GPS_RX_PIN 3
4 |
5 | #define PIN_SPEED_RIGHT 9
6 | #define PIN_SPEED_LEFT 5
7 |
8 | #define BLUETOOTH_TX_PIN 10
9 | #define BLUETOOTH_RX_PIN 11
10 |
11 | // Bluetooth GPS input
12 | #define SOP '<'
13 | #define EOP '>'
14 |
15 | // Motor stuffs
16 | #define RC_NEUTRAL 1500
17 | #define RC_MAX 1600
18 | #define RC_MIN 1400
19 |
20 | // If one motor tends to spin faster than the other, add offset
21 | #define MOTOR_A_OFFSET 0
22 | #define MOTOR_B_OFFSET 0
23 |
24 | // You must then add your 'Declination Angle' to the compass, which is the 'Error' of the magnetic field in your location.
25 | // Find yours here: http://www.magnetic-declination.com/
26 | // Mine is: 13° 24' E (Positive), which is ~13 Degrees, or (which we need) 0.23 radians
27 | #define DECLINATION_ANGLE -0.17f
28 |
29 | // The offset of the mounting position to true north
30 | // It would be best to run the /examples/magsensor sketch and compare to the compass on your smartphone
31 | #define COMPASS_OFFSET 0.35f
32 |
33 | // How often the GPS should update in MS
34 | // Keep this above 1000
35 | #define GPS_UPDATE_INTERVAL 1000
36 |
37 | // Number of changes in movement to timeout for GPS waypoints
38 | // Keeps the robot from driving away if there is a problem
39 | #define GPS_WAYPOINT_TIMEOUT 25
40 |
41 | // Struct to combine our coordinates into one struct for ease of use
42 | struct GeoLoc {
43 | float lat;
44 | float lon;
45 | };
46 |
--------------------------------------------------------------------------------
/src/main/res/layout/activity_device_list.xml:
--------------------------------------------------------------------------------
1 |
2 |
9 |
10 |
14 |
15 |
21 |
22 |
23 |
24 |
25 |
26 |
33 |
34 |
35 |
--------------------------------------------------------------------------------
/src/main/res/layout/activity_gps_controll.xml:
--------------------------------------------------------------------------------
1 |
2 |
9 |
10 |
14 |
15 |
21 |
22 |
23 |
24 |
25 |
26 |
33 |
34 |
35 |
--------------------------------------------------------------------------------
/src/main/res/layout/content_gps_controll.xml:
--------------------------------------------------------------------------------
1 |
2 |
14 |
15 |
23 |
24 |
32 |
33 |
41 |
42 |
43 |
--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
1 | # Follow-Me-Robot
2 |
3 | This is a senior highschool project for an autonomous robot that will follow you (more your position) upon the click of a button. The main idea is that the user will have a smart-phone that will send its GPS coordinates over bluetooth to the robot, who will, with the use of a compas and a GPS module, drive to the received coordinates.
4 |
5 | Room for improvement would be to implement a way to make a constant stream of GPS coordinates to the robot (currently we send coordinates only when the button is pressed). Additionally, the robot has no knowledge of its surrounding so it will not avoid obstacles.
6 |
7 | Usefull links I used to put together this project:
8 | - https://www.hackster.io/hackerhouse/make-an-autonomous-follow-me-cooler-7ca8bc
9 | - https://developer.android.com/guide/topics/location/strategies.html
10 | - http://www.instructables.com/id/Android-Bluetooth-Control-LED-Part-2/
11 | - http://forum.arduino.cc/index.php?topic=91707.0
12 |
13 | For this project I used:
14 | - AndyMark Tilerunner robot. Really any sort of robot will do.
15 | - Arduino Uno to controll the robot. The Arduino Uno is very cheap (especially the clones) and has all the capabilities required for the project.
16 | - 2 vex pro victor 888 motor controllers. I only used these controllers because I had them lying around and they only require 1 PWN pin to operate a motor (compared to 1 PWN and 2 other pins for an L298 controller). I do not recommend these controllers for this particular project as they are really bulky.
17 | - The GPS reciever I used for the robot is the EM-506 (48 Channel). This reciever is quite pricey and the connector is too small for jumper wires to fit, so I had to cut off the end and solder jumper wires to each connection individually. Nevertheless, the reciever proved to lock on fast even indoors and is quite accurate.
18 | - I used the Wireless 4 Pin Bluetooth RF Transceiver Module RS232 With Backplane as the bluetooth module for my robot.
19 | - For the compass I bought the Triple-axis Accelerometer+Magnetometer (Compass) Board - LSM303. This module includes both and accelerometer and magnetometer, but I only used the magnetometer pins from the board.
20 | - As the power source for my motors I used the AJC® Brand 12 Volt 7Ah Sealed Lead Acid Battery. This battery is quite large due to its large capacity and is a bit overkill, but I had it available and it fit on my robot chassis.
21 | - To power the arduino I used a portable battery bank, like the one you can use to charge your phone.
22 | - I used an Android phone to run my app since I coded the app in Android Studios. The newer the phone, the better the GPS coordinates provided by the phone will be.
23 |
24 | The Defenitions.h file is where I defined all of my pins so change the values accordingly. FollowMe.ino is the source code for the Arduino for the movement. The "src" folder contains the java code and manifest files for the Android Studio app code.
25 |
--------------------------------------------------------------------------------
/src/main/java/com/example/artur_000/followmerobot/DeviceList.java:
--------------------------------------------------------------------------------
1 | package com.example.artur_000.followmerobot;
2 |
3 | import android.os.Bundle;
4 | import android.support.v7.app.AppCompatActivity;
5 | import android.support.v7.widget.Toolbar;
6 | import android.view.View;
7 | import android.view.Menu;
8 | import android.view.MenuItem;
9 | import android.widget.Button;
10 | import android.widget.ListView;
11 | import java.util.Set;
12 | import java.util.ArrayList;
13 | import android.widget.Toast;
14 | import android.widget.ArrayAdapter;
15 | import android.widget.AdapterView;
16 | import android.widget.TextView;
17 | import android.content.Intent;
18 | import android.bluetooth.BluetoothAdapter;
19 | import android.bluetooth.BluetoothDevice;
20 |
21 | public class DeviceList extends AppCompatActivity {
22 |
23 | Button btnPaired;
24 | ListView deviceList;
25 | private BluetoothAdapter myBluetooth = null;
26 | private Set pairedDevices;
27 | public static String EXTRA_ADDRESS = "device_address";
28 |
29 | @Override
30 | protected void onCreate(Bundle savedInstanceState) {
31 | super.onCreate(savedInstanceState);
32 | setContentView(R.layout.activity_device_list);
33 | Toolbar toolbar = (Toolbar) findViewById(R.id.toolbar);
34 | setSupportActionBar(toolbar);
35 | deviceList = (ListView) findViewById(R.id.listView);
36 |
37 | btnPaired = (Button) findViewById(R.id.button);
38 | btnPaired.setOnClickListener(new View.OnClickListener() {
39 | @Override
40 | public void onClick(View v) {
41 | pairedDevicesList();
42 | }
43 | });
44 |
45 | myBluetooth = BluetoothAdapter.getDefaultAdapter();
46 | if (myBluetooth == null){
47 |
48 | // Show a message that the device has no bluetooth adapter
49 | Toast.makeText(getApplicationContext(), "Bluetooth Device Not Available", Toast.LENGTH_LONG).show();
50 |
51 | // Finish apk
52 | finish();
53 | }else if (!myBluetooth.isEnabled()){
54 | // Ask to the user turn the bluetooth on
55 | Intent turnBTon = new Intent(BluetoothAdapter.ACTION_REQUEST_ENABLE);
56 | startActivityForResult(turnBTon,1);
57 | }
58 | }
59 |
60 | @Override
61 | public boolean onCreateOptionsMenu(Menu menu) {
62 | // Inflate the menu; this adds items to the action bar if it is present.
63 | getMenuInflater().inflate(R.menu.menu_device_list, menu);
64 | return true;
65 | }
66 |
67 | @Override
68 | public boolean onOptionsItemSelected(MenuItem item) {
69 | // Handle action bar item clicks here. The action bar will
70 | // automatically handle clicks on the Home/Up button, so long
71 | // as you specify a parent activity in AndroidManifest.xml.
72 | int id = item.getItemId();
73 |
74 | //noinspection SimplifiableIfStatement
75 | if (id == R.id.action_settings) {
76 | return true;
77 | }
78 |
79 | return super.onOptionsItemSelected(item);
80 | }
81 |
82 | private void pairedDevicesList(){
83 | pairedDevices = myBluetooth.getBondedDevices();
84 | ArrayList list = new ArrayList();
85 |
86 | if (pairedDevices.size() > 0){
87 | for (BluetoothDevice bt : pairedDevices){
88 | list.add(bt.getName() + "\n" + bt.getAddress()); //Get the device's name and the address
89 | }
90 | }else{
91 | // We didn't find any paired bluetooth devices.
92 | Toast.makeText(getApplicationContext(), "No Paired Bluetooth Devices Found.", Toast.LENGTH_LONG).show();
93 | }
94 |
95 | final ArrayAdapter adapter = new ArrayAdapter(this,android.R.layout.simple_list_item_1, list);
96 | deviceList.setAdapter(adapter);
97 | deviceList.setOnItemClickListener(myListClickListener); //Method called when the device from the list is clicked
98 | }
99 |
100 | private AdapterView.OnItemClickListener myListClickListener = new AdapterView.OnItemClickListener()
101 | {
102 | public void onItemClick (AdapterView av, View v, int arg2, long arg3)
103 | {
104 | // Get the device MAC address, the last 17 chars in the View
105 | String info = ((TextView) v).getText().toString();
106 | String address = info.substring(info.length() - 17);
107 |
108 | // Make an intent to start next activity.
109 | Intent i = new Intent(DeviceList.this, GpsControll.class);
110 |
111 | //Change the activity.
112 | i.putExtra(EXTRA_ADDRESS, address); //this will be received at GpsControll (class) Activity
113 | startActivity(i);
114 | }
115 | };
116 | }
117 |
--------------------------------------------------------------------------------
/FollowMe/FollowMe.ino:
--------------------------------------------------------------------------------
1 | // Imports
2 | #include
3 | #include
4 | #include
5 | #include
6 | #include
7 | #include
8 | #include "./Definitions.h"
9 |
10 | // GPS
11 | TinyGPSPlus gps;
12 | #define GPS_BAUD 4800
13 |
14 | // Motors
15 | Servo servoThrottleRight;
16 | Servo servoThrottleLeft;
17 |
18 | // Master Enable
19 | bool enabled = true;
20 |
21 | // Bluetooth input
22 | bool started = false;
23 | bool ended = false;
24 | char inData[80]; // creates an 80 character array called "inData"
25 | byte index; //creates a variable type=byte called "index"
26 | double Long; //variable for longitude coordinate
27 | double Lat; //variable for latitude coordinate
28 |
29 | // Serial components
30 | SoftwareSerial bluetoothSerial(BLUETOOTH_TX_PIN, BLUETOOTH_RX_PIN);
31 | SoftwareSerial gpsSerial(GPS_RX_PIN, GPS_TX_PIN); // TXD to digital pin 6
32 |
33 | /* Compass */
34 | Adafruit_HMC5883_Unified mag = Adafruit_HMC5883_Unified(12345);
35 |
36 | GeoLoc checkGPS() {
37 | Serial.println("Reading onboard GPS: ");
38 | unsigned long start = millis();
39 | while (millis() - start < GPS_UPDATE_INTERVAL) {
40 | // If we recieved new location then take the coordinates and pack them into a struct
41 | if (feedgps())
42 | return gpsdump();
43 | }
44 |
45 | GeoLoc robotLoc;
46 | robotLoc.lat = 0.0;
47 | robotLoc.lon = 0.0;
48 |
49 | return robotLoc;
50 | }
51 |
52 | // Get and process GPS data
53 | GeoLoc gpsdump() {
54 | GeoLoc robotLoc;
55 | robotLoc.lat = gps.location.lat();
56 | robotLoc.lon = gps.location.lng();
57 |
58 | Serial.print(robotLoc.lat, 7); Serial.print(", "); Serial.println(robotLoc.lon, 7);
59 |
60 | return robotLoc;
61 | }
62 |
63 | // Feed data as it becomes available
64 | bool feedgps() {
65 | while (gpsSerial.available() > 0) {
66 | if (gps.encode(gpsSerial.read()))
67 | return true;
68 | }
69 | return false;
70 | }
71 |
72 | void displayCompassDetails(void)
73 | {
74 | sensor_t sensor;
75 | mag.getSensor(&sensor);
76 | Serial.println("------------------------------------");
77 | Serial.print ("Sensor: "); Serial.println(sensor.name);
78 | Serial.print ("Driver Ver: "); Serial.println(sensor.version);
79 | Serial.print ("Unique ID: "); Serial.println(sensor.sensor_id);
80 | Serial.print ("Max Value: "); Serial.print(sensor.max_value); Serial.println(" uT");
81 | Serial.print ("Min Value: "); Serial.print(sensor.min_value); Serial.println(" uT");
82 | Serial.print ("Resolution: "); Serial.print(sensor.resolution); Serial.println(" uT");
83 | Serial.println("------------------------------------");
84 | Serial.println("");
85 | delay(500);
86 | }
87 |
88 | #ifndef DEGTORAD
89 | #define DEGTORAD 0.0174532925199432957f
90 | #define RADTODEG 57.295779513082320876f
91 | #endif
92 |
93 | float geoBearing(struct GeoLoc &a, struct GeoLoc &b) {
94 | float y = sin(b.lon-a.lon) * cos(b.lat);
95 | float x = cos(a.lat)*sin(b.lat) - sin(a.lat)*cos(b.lat)*cos(b.lon-a.lon);
96 | return atan2(y, x) * RADTODEG;
97 | }
98 |
99 | float geoDistance(struct GeoLoc &a, struct GeoLoc &b) {
100 | const float R = 6371000; // radius of earth in metres
101 | float p1 = a.lat * DEGTORAD;
102 | float p2 = b.lat * DEGTORAD;
103 | float dp = (b.lat-a.lat) * DEGTORAD;
104 | float dl = (b.lon-a.lon) * DEGTORAD;
105 |
106 | float x = sin(dp/2) * sin(dp/2) + cos(p1) * cos(p2) * sin(dl/2) * sin(dl/2);
107 | float y = 2 * atan2(sqrt(x), sqrt(1-x));
108 |
109 | // returns distance in meters
110 | return R * y;
111 | }
112 |
113 | float geoHeading() {
114 | /* Get a new sensor event */
115 | sensors_event_t event;
116 | mag.getEvent(&event);
117 |
118 | // Hold the module so that Z is pointing 'up' and you can measure the heading with x&y
119 | // Calculate heading when the magnetometer is level, then correct for signs of axis.
120 | float heading = atan2(event.magnetic.y, event.magnetic.x);
121 |
122 | // Offset
123 | heading -= DECLINATION_ANGLE;
124 | heading -= COMPASS_OFFSET;
125 |
126 | // Correct for when signs are reversed.
127 | if(heading < 0)
128 | heading += 2*PI;
129 |
130 | // Check for wrap due to addition of declination.
131 | if(heading > 2*PI)
132 | heading -= 2*PI;
133 |
134 | // Convert radians to degrees for readability.
135 | float headingDegrees = heading * 180/M_PI;
136 |
137 | // Map to -180 - 180
138 | while (headingDegrees < -180) headingDegrees += 360;
139 | while (headingDegrees > 180) headingDegrees -= 360;
140 |
141 | return headingDegrees;
142 | }
143 |
144 | void setSpeedMotorA(int speed) {
145 | servoThrottleRight.writeMicroseconds(speed + MOTOR_A_OFFSET);
146 | }
147 |
148 | void setSpeedMotorB(int speed) {
149 | servoThrottleLeft.writeMicroseconds(speed + MOTOR_B_OFFSET);
150 | }
151 |
152 | void stop() {
153 | // stop the motors
154 | servoThrottleRight.writeMicroseconds(RC_NEUTRAL);
155 | servoThrottleLeft.writeMicroseconds(RC_NEUTRAL);
156 | }
157 |
158 | void drive(int distance, float turn) {
159 | int fullSpeed = 230;
160 | int stopSpeed = 0;
161 |
162 | // drive to location
163 | int s = fullSpeed;
164 | // Slowing down??? Needed???
165 | if ( distance < 8 ) {
166 | int wouldBeSpeed = s - stopSpeed;
167 | wouldBeSpeed *= distance / 8.0f;
168 | s = stopSpeed + wouldBeSpeed;
169 | }
170 |
171 | int autoThrottle = constrain(s, stopSpeed, fullSpeed);
172 | autoThrottle = 230;
173 |
174 | float t = turn;
175 | while (t < -180) t += 360;
176 | while (t > 180) t -= 360;
177 |
178 | Serial.print("turn: ");
179 | Serial.println(t);
180 | Serial.print("original: ");
181 | Serial.println(turn);
182 |
183 | float t_modifier = (180.0 - abs(t)) / 180.0;
184 | float autoSteerA = 1;
185 | float autoSteerB = 1;
186 |
187 | if (t < 0) {
188 | autoSteerB = t_modifier;
189 | } else if (t > 0){
190 | autoSteerA = t_modifier;
191 | }
192 |
193 | Serial.print("steerA: "); Serial.println(autoSteerA);
194 | Serial.print("steerB: "); Serial.println(autoSteerB);
195 |
196 | // int speedA = (int) (((float) map(autoThrottle, stopSpeed, fullSpeed, RC_NEUTRAL, RC_MAX)) * autoSteerA);
197 | // int speedB = (int) (((float) map(autoThrottle, stopSpeed, fullSpeed, RC_NEUTRAL, RC_MIN)) * autoSteerB);
198 | int speedA = (int) (((float) map(autoThrottle, stopSpeed, fullSpeed, RC_NEUTRAL, RC_MIN)) * autoSteerA);
199 | int speedB = (int) (((float) map(autoThrottle, stopSpeed, fullSpeed, RC_NEUTRAL, RC_MAX)) * autoSteerB);
200 |
201 | setSpeedMotorA(speedA);
202 | setSpeedMotorB(speedB);
203 | }
204 |
205 | void driveTo(struct GeoLoc &loc, int timeout) {
206 | gpsSerial.listen();
207 | GeoLoc robotLoc = checkGPS();
208 | bluetoothSerial.listen();
209 |
210 | if (robotLoc.lat != 0 && robotLoc.lon != 0 && enabled) {
211 | float distance = 0;
212 | //Start move loop here
213 | do {
214 | gpsSerial.listen();
215 | robotLoc = checkGPS();
216 | bluetoothSerial.listen();
217 |
218 | distance = geoDistance(robotLoc, loc);
219 | float bearing = geoBearing(robotLoc, loc) - geoHeading();
220 |
221 | Serial.print("Distance: ");
222 | Serial.println(distance);
223 |
224 | Serial.print("Bearing: ");
225 | Serial.println(geoBearing(robotLoc, loc));
226 |
227 | Serial.print("Heading: ");
228 | Serial.println(geoHeading());
229 |
230 | drive(distance, bearing);
231 | timeout -= 1;
232 | } while (distance > 1.0 && enabled && timeout>0);
233 |
234 | stop();
235 | }
236 | }
237 |
238 | void setupCompass() {
239 | /* Initialise the compass */
240 | if(!mag.begin())
241 | {
242 | /* There was a problem detecting the HMC5883 ... check your connections */
243 | Serial.println("Ooops, no HMC5883 detected ... Check your wiring!");
244 | while(1);
245 | }
246 |
247 | /* Display some basic information on this sensor */
248 | displayCompassDetails();
249 | }
250 |
251 | void setup()
252 | {
253 | // Attaching motors
254 | servoThrottleRight.attach(PIN_SPEED_RIGHT);
255 | servoThrottleLeft.attach(PIN_SPEED_LEFT);
256 |
257 | //Debugging via serial
258 | Serial.begin(115200);
259 |
260 | // Compass
261 | setupCompass();
262 |
263 | //GPS
264 | gpsSerial.begin(GPS_BAUD);
265 |
266 | //Bluetooth
267 | bluetoothSerial.begin(9600);
268 | //Blynk.begin(bluetoothSerial, auth);
269 | }
270 |
271 | // Testing
272 | void testDriveNorth() {
273 | float heading = geoHeading();
274 | int testDist = 5;
275 | Serial.println(heading);
276 |
277 | while(!(heading < 5 && heading > -5)) {
278 | drive(testDist, heading);
279 | heading = geoHeading();
280 | Serial.println(heading);
281 | delay(500);
282 | }
283 |
284 | stop();
285 | }
286 |
287 | void loop()
288 | {
289 | //Blynk.run();
290 | // Read all serial data available, as fast as possible
291 | while (bluetoothSerial.available() > 0)
292 | {
293 | char inChar = ((byte)bluetoothSerial.read());
294 | if (inChar == SOP)
295 | {
296 | index = 0;
297 | inData[index] = '\0';
298 | started = true;
299 | ended = false;
300 | }
301 | else if (inChar == EOP)
302 | {
303 | ended = true;
304 | break;
305 | }
306 | else
307 | {
308 | if (index < 79)
309 | {
310 | inData[index] = inChar;
311 | index++;
312 | inData[index] = '\0';
313 | }
314 | }
315 | }
316 |
317 | // We are here either because all pending serial
318 | // data has been read OR because an end of
319 | // packet marker arrived. Which is it?
320 | if (started && ended)
321 | {
322 | char *token = strtok(inData, ",");
323 | boolean first = true;
324 | while (token)
325 | {
326 | double val = atof(token);
327 | token = strtok(NULL, ",");
328 | if (first){
329 | Lat = val;
330 | }else{
331 | Long = val;
332 | }
333 | first = false;
334 | }
335 |
336 | // Reset for the next packet
337 | started = false;
338 | ended = false;
339 | index = 0;
340 | inData[index] = '\0';
341 |
342 | GeoLoc phoneLoc;
343 | phoneLoc.lat = Lat;
344 | phoneLoc.lon = Long;
345 |
346 | Serial.print(phoneLoc.lat, 7); Serial.print(", "); Serial.println(phoneLoc.lon, 7);
347 | driveTo(phoneLoc, GPS_WAYPOINT_TIMEOUT);
348 | }
349 | }
350 |
--------------------------------------------------------------------------------
/src/main/java/com/example/artur_000/followmerobot/GpsControll.java:
--------------------------------------------------------------------------------
1 | package com.example.artur_000.followmerobot;
2 |
3 | import android.content.Context;
4 | import android.content.pm.PackageManager;
5 | import android.location.Location;
6 | import android.location.LocationListener;
7 | import android.location.LocationManager;
8 | import android.os.Bundle;
9 | import android.support.v4.app.ActivityCompat;
10 | import android.support.v4.content.ContextCompat;
11 | import android.support.v7.app.AppCompatActivity;
12 | import android.view.View;
13 | import android.bluetooth.BluetoothSocket;
14 | import android.content.Intent;
15 | import android.widget.Button;
16 | import android.widget.Toast;
17 | import android.app.ProgressDialog;
18 | import android.bluetooth.BluetoothAdapter;
19 | import android.bluetooth.BluetoothDevice;
20 | import android.os.AsyncTask;
21 | import java.io.IOException;
22 | import java.util.UUID;
23 |
24 | public class GpsControll extends AppCompatActivity {
25 |
26 | Button btnForw, btnDisc;
27 | String address = null;
28 | private ProgressDialog progress;
29 | BluetoothAdapter myBluetooth = null;
30 | BluetoothSocket btSocket = null;
31 | private boolean isBtConnect = false;
32 | static final UUID myUUID = UUID.fromString("00001101-0000-1000-8000-00805F9B34FB");
33 |
34 | LocationManager locationManager;
35 | Location currentBestLocation;
36 | LocationListener locationListener;
37 | private static final int ONE_MINUTE = 1000 * 60 * 1;
38 | private static final int MY_PERMISSIONS_REQUEST_READ_CONTACTS = 3;
39 |
40 | @Override
41 | protected void onCreate(Bundle savedInstanceState) {
42 | super.onCreate(savedInstanceState);
43 | setContentView(R.layout.activity_gps_controll);
44 | locationManager = (LocationManager) this.getSystemService(Context.LOCATION_SERVICE);
45 |
46 | locationListener = new LocationListener() {
47 | @Override
48 | public void onLocationChanged(Location location) {
49 | // New Location found by the network location provider
50 | if (isBetterLocation(location, currentBestLocation)){
51 | currentBestLocation = location;
52 | msg("New Location");
53 | }
54 | }
55 |
56 | @Override
57 | public void onStatusChanged(String provider, int status, Bundle extras) {}
58 |
59 | @Override
60 | public void onProviderEnabled(String provider) {}
61 |
62 | @Override
63 | public void onProviderDisabled(String provider) {}
64 | };
65 |
66 | // Receive the address of the bluetooth device
67 | Intent newint = getIntent();
68 | address = newint.getStringExtra(DeviceList.EXTRA_ADDRESS);
69 | new ConnectBT().execute();
70 |
71 | // Register the listener with location manager to start requesting location data
72 | if (ContextCompat.checkSelfPermission(this, android.Manifest.permission.ACCESS_FINE_LOCATION) != PackageManager.PERMISSION_GRANTED) {
73 | ActivityCompat.requestPermissions(this,
74 | new String[]{android.Manifest.permission.ACCESS_FINE_LOCATION},
75 | MY_PERMISSIONS_REQUEST_READ_CONTACTS);
76 | }else {
77 | initLocationRequest(locationListener);
78 | }
79 |
80 | // View of the layout
81 | setContentView(R.layout.activity_gps_controll);
82 |
83 | // Call the widgtes
84 | btnForw = (Button)findViewById(R.id.btnForward);
85 | btnForw.setOnClickListener(new View.OnClickListener() {
86 | @Override
87 | public void onClick(View v) {
88 | // Pass that location to sending method
89 | if (currentBestLocation != null) {
90 | //msg("Latitude: " + currentBestLocation.getLatitude() + "\nLongitude: " + currentBestLocation.getLongitude());
91 | send(currentBestLocation);
92 | }else{
93 | initLocationRequest(locationListener);
94 | }
95 | }
96 | });
97 |
98 | btnDisc = (Button)findViewById(R.id.btn_disconnect);
99 | btnDisc.setOnClickListener(new View.OnClickListener(){
100 | @Override
101 | public void onClick(View v){
102 | if (ContextCompat.checkSelfPermission(GpsControll.this, android.Manifest.permission.ACCESS_FINE_LOCATION) == PackageManager.PERMISSION_GRANTED) {
103 | locationManager.removeUpdates(locationListener);
104 | }
105 | Disconnect();
106 | }
107 | });
108 | }
109 |
110 | private void initLocationRequest(LocationListener locationListener){
111 | if (ContextCompat.checkSelfPermission(GpsControll.this, android.Manifest.permission.ACCESS_FINE_LOCATION) == PackageManager.PERMISSION_GRANTED) {
112 | locationManager.requestLocationUpdates(LocationManager.GPS_PROVIDER, 0, 0, locationListener);
113 | locationManager.requestLocationUpdates(LocationManager.NETWORK_PROVIDER, 0, 0, locationListener);
114 | currentBestLocation = locationManager.getLastKnownLocation(LocationManager.GPS_PROVIDER);
115 | }
116 | }
117 |
118 | @Override
119 | public void onRequestPermissionsResult(int requestCode, String[] permissions, int[] grantResults) {
120 | if (requestCode == MY_PERMISSIONS_REQUEST_READ_CONTACTS){
121 | if (grantResults.length > 0 && grantResults[0] == PackageManager.PERMISSION_GRANTED){
122 | // Permission granted Phheewww
123 | }else{
124 | // Permission denied! Booo!!
125 | finish();
126 | }
127 | }
128 | }
129 |
130 | private boolean isBetterLocation(Location location, Location currBest){
131 | if (currBest == null){
132 | // If we don't have a current best location, then this is it!
133 | return true;
134 | }
135 |
136 | // Check time of location recieved
137 | long timeDela = location.getTime() - currBest.getTime();
138 | boolean isSignificantlyNewer = timeDela > ONE_MINUTE;
139 | boolean isSignificantlyOlder = timeDela < -ONE_MINUTE;
140 | boolean isNewer = timeDela > 0;
141 |
142 | if (isSignificantlyNewer){
143 | // Location is newer by 1 minute! User probs moved to use it!!
144 | return true;
145 | }else if (isSignificantlyOlder){
146 | // Eww the new location is stale don't use it !!
147 | return false;
148 | }
149 |
150 | // Check for the given accuracy if location
151 | int accuracyDelta = (int) (location.getAccuracy() - currBest.getAccuracy());
152 | boolean isMoreAccurate = accuracyDelta < 0;
153 | boolean isLessAccurate = accuracyDelta > 0;
154 | boolean isSignificantlyLessAccurate = accuracyDelta > 10;
155 |
156 | // Check if the old and new location are from the same provider
157 | boolean isFromSameProvider = isSameProvider(location.getProvider(), currentBestLocation.getProvider());
158 |
159 | // Determine location quality using a combination of timeliness and accuracy
160 | if (isMoreAccurate) {
161 | return true;
162 | } else if (isNewer && !isLessAccurate) {
163 | return true;
164 | } else if (isNewer && !isSignificantlyLessAccurate && isFromSameProvider) {
165 | return true;
166 | }
167 | return false;
168 | }
169 |
170 | /** Checks whether two providers are the same */
171 | private boolean isSameProvider(String provider1, String provider2) {
172 | if (provider1 == null) {
173 | return provider2 == null;
174 | }
175 | return provider1.equals(provider2);
176 | }
177 |
178 | private void send(Location location)
179 | {
180 | if (btSocket!=null)
181 | {
182 | try
183 | {
184 | msg(("<" + Double.toString(location.getLatitude()) + "," + Double.toString(location.getLongitude()) + ">"));
185 | btSocket.getOutputStream().write(("<" + Double.toString(location.getLatitude()) + "," + Double.toString(location.getLongitude()) + ">").getBytes());
186 | }
187 | catch (IOException e)
188 | {
189 | msg("Error");
190 | }
191 | }
192 | }
193 |
194 | private void Disconnect()
195 | {
196 | if (btSocket!=null) //If the btSocket is busy
197 | {
198 | try
199 | {
200 | btSocket.close(); //close connection
201 | }
202 | catch (IOException e)
203 | { msg("Error");}
204 | }
205 | finish(); //return to the first layout
206 | }
207 |
208 | private void msg(String s)
209 | {
210 | Toast.makeText(getApplicationContext(),s,Toast.LENGTH_LONG).show();
211 | }
212 |
213 | private class ConnectBT extends AsyncTask {
214 |
215 | private boolean connectSuccs = true;
216 |
217 | @Override
218 | protected void onPreExecute() {
219 | progress = ProgressDialog.show(GpsControll.this, "Connecting...", "Please wait!!!"); //show a progress dialog
220 | }
221 |
222 | @Override
223 | protected Void doInBackground(Void... params) { // Need to perform connection while the dialog is shown in the background
224 | try
225 | {
226 | if (btSocket == null || !isBtConnect)
227 | {
228 | myBluetooth = BluetoothAdapter.getDefaultAdapter();//get the mobile bluetooth device
229 | BluetoothDevice dispositivo = myBluetooth.getRemoteDevice(address);//connects to the device's address and checks if it's available
230 | btSocket = dispositivo.createInsecureRfcommSocketToServiceRecord(myUUID);//create a RFCOMM (SPP) connection
231 | BluetoothAdapter.getDefaultAdapter().cancelDiscovery();
232 | btSocket.connect();//start connection
233 | }
234 | }
235 | catch (IOException e)
236 | {
237 | connectSuccs = false;//if the try failed, you can check the exception here
238 | }
239 | return null;
240 | }
241 |
242 | @Override
243 | protected void onPostExecute(Void result) {
244 | super.onPostExecute(result);
245 | if (!connectSuccs)
246 | {
247 | msg("Connection Failed. Is it on? Try again.");
248 | finish();
249 | }
250 | else
251 | {
252 | msg("Connected.");
253 | isBtConnect = true;
254 | }
255 | progress.dismiss();
256 | }
257 | }
258 | }
259 |
260 |
261 |
--------------------------------------------------------------------------------