1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
/*
  ________    _           __    __
  /_  __/ /_  (_)___ ___  / /_  / /__
  / / / __ \/ / __ `__ \/ __ \/ / _ \
  / / / / / / / / / / / / /_/ / /  __/
  /_/ /_/ /_/_/_/ /_/ /_/_.___/_/\___/
    ____        __          __  _              __ __ _ __     ___
   / __ \____  / /_  ____  / /_(_)_________   / //_/(_) /_   |__ \
  / /_/ / __ \/ __ \/ __ \/ __/ / ___/ ___/  / ,<  / / __/   __/ /
  / _, _/ /_/ / /_/ / /_/ / /_/ / /__(__  )  / /| |/ / /_    / __/
  /_/ |_|\____/_.___/\____/\__/_/\___/____/  /_/ |_/_/\__/   /____/
*/

//All the libraries used
#include <WiFiEsp.h>
#include <WiFiEspUdp.h>
#include "SoftwareSerial.h"
#include "Grove_I2C_Motor_Driver.h"

// default I2C address is 0x0f
#define I2C_ADDRESS 0x0f

//Motor names
#define LEFTMOTOR MOTOR2
#define RIGHTMOTOR MOTOR1

//Joystick Limits
int leftUp  = 253;
int leftDown = 769;
int rightUp = 775;
int rightDown = 247;

//Create Serial communication Object
SoftwareSerial Serial1(8, 9); // RX, TX

WiFiEspUDP Udp;

IPAddress apIP = IPAddress(192, 168, 111, 111);
IPAddress stationIP = IPAddress(192, 168, 111, 112);
IPAddress broadcastIP = IPAddress(0, 0, 0, 0);

//Wifi settings
char ssid[] = "My WiFi Robot";    // the name of your access point
char pass[] = "password";        // the password for your access point
int status = WL_IDLE_STATUS;     // the Wifi radio's status

//Ports to be used
unsigned int listeningPort = 10002;  // local port to listen on
unsigned int sendingPort = 10003;  // local port to sent on


//Message Variables
#define arrayLength 6
char data[arrayLength + 1];
String dataAsString;

// Vaiables
int leftData = 0;
int rightData = 0;

//Motor Variables
int leftSpeed = 0;
int rightSpeed = 0;

void setup()
{
  // Start Serial Communication with computer and WiFi module
  Serial.begin(9600);
  // initialize serial for ESP module
  Serial1.begin(9600);
  // initialize ESP module
  WiFi.init(&Serial1);
  WiFi.configAP(apIP);

  // start motor driver
  Motor.begin(I2C_ADDRESS);
  // correct pwm cycles
  Motor.frequence(F_490Hz);

  // Print out debugging messages
  Serial.print("Attempting to start AP ");
  Serial.println(ssid);

  // start access point
  status =  WiFi.beginAP(ssid, 10, pass, ENC_TYPE_WPA2_PSK);


  // Set up UDP listener on port
  Udp.begin(listeningPort);

  // Print out which port
  Serial.print(F("Listening on port "));
  Serial.println(listeningPort);

}

void loop()
{

  //Check for incoming packets
  int packetSize = Udp.parsePacket();

  // If there are packets
  if (packetSize) {
    //memset(data, 0, sizeof data);
    //Print out update
    //Serial.println("Packets Received");
    // read the packet into packetBufffer
    int len = Udp.read(data, 6);
    if (len > 0) {
      data[len] = 0;
    }
    //Send received data to function
    dataTranslate(data);

    //Map values to speeds
    leftSpeed = map(leftData, leftDown, leftUp, -100, 100);
    rightSpeed = map(rightData, rightDown, rightUp, -100, 100);
  }

  //Check to see if joystick is in deadzone
  if (leftSpeed < 10 && leftSpeed > -10) {
    //Stop motor
    Motor.stop(LEFTMOTOR);
  } else {
    //Set motor to mapped speed
    Motor.speed(LEFTMOTOR, leftSpeed);
  }

  //Check to see if joystick is in deadzone
  if (rightSpeed < 10 && rightSpeed > -10) {
    //Stop motor
    Motor.stop(RIGHTMOTOR);
  } else {
    //Set motor to mapped speed
    Motor.speed(RIGHTMOTOR, rightSpeed);
  }

  //Print out all values
  Serial.println(String(leftSpeed) + "," + String(rightSpeed));
}

//Function that takes raw string and chops it into values
void dataTranslate(char recData[]) {
  // Set passed array into a temp variable
  dataAsString = recData;
  // Write a section of the array to a value based on location
  leftData = dataAsString.substring(0, 3).toInt();
  rightData = dataAsString.substring(3, 6).toInt();
}