SnakeMic/Arduino_code.ino at master · thathvik/SnakeMic · GitHub

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
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
#include <SoftwareSerial.h>
SoftwareSerial Bluetooth(2, 3); // RX, TX

#define en1 5
#define in1 8
#define in2 9
#define en2 6
#define in3 10
#define in4 11

int xDir = 55; //Specifies location of the joystick on screen along x axis (additionally used for custom controls)
int yDir = 55; //Specifies location of the joystick on screen along y axis (additionally used for custom controls)
//Initialize variables to specify the motor speed of both the wheels
int motorSpeed1 = 0;
int motorSpeed2 = 0;

//For curve function
int count = 0;
bool toggleFlag = 0;
void setup() {
  pinMode(en1, OUTPUT);
  pinMode(en2, OUTPUT);
  pinMode(in1, OUTPUT);
  pinMode(in2, OUTPUT);
  pinMode(in3, OUTPUT);
  pinMode(in4, OUTPUT);
  Serial.begin(9600);
  Bluetooth.begin(9600); // Default communication rate of the Bluetooth module
  delay(500);
}

void loop() {
  // Read the incoming data from the Smartphone Android App
  while (Bluetooth.available() >= 2) {
    xDir = Bluetooth.read();
    delay(10);
    yDir = Bluetooth.read();
    Serial.print(xDir);
    Serial.print(",");
    Serial.println(yDir);
  }
  delay(10);
  // Makes sure we receive correct values
  //Custom Inputs - Straight, Circle, Sine, Stop, Rotate In Place
  if (xDir == 5)
  {
    switch (yDir)
    {
      case 172: // Go straight
        motorSpeed1 = 250;
        motorSpeed2 = 250;
        forward();
        break;
      case 174: // Circle
        motorSpeed1 = 250;
        motorSpeed2 = 200;
        forward();
        break;
      case 176: // Sine or Wave motion
        if (toggleFlag)
        {
          motorSpeed1 = 250;
          motorSpeed2 = 200;
        }
        else
        {
          motorSpeed1 = 200;
          motorSpeed2 = 250;
        }
        curve();
        break;
      case 178: //Rotate in place
        motorSpeed1 = 250;
        motorSpeed2 = 250;
        turnRight();
        break;
      case 180: //Stop
        Stop();
        break;
    }
  }
  else
  {
    //Joystick Control
    if (xDir > 50 && xDir < 120 && yDir > 50 && yDir < 120) {
      Stop();
    }
    if (yDir > 50 && yDir < 120) {
      if (xDir < 50) {
        turnRight();
        motorSpeed1 = map(xDir, 50, 10, 0, 255);
        motorSpeed2 = map(xDir, 50, 10, 0, 255);
      }
      if (xDir > 120) {
        turnLeft();
        motorSpeed1 = map(xDir, 120, 150, 0, 255);
        motorSpeed2 = map(xDir, 120, 150, 0, 255);
      }
    }
    else {
      if (xDir > 50 && xDir < 120) {

        if (yDir < 50) {
          forward();
        }
        if (yDir > 120) {
          backward();
        }
        if (yDir < 50) {
          motorSpeed1 = map(yDir, 50, 10, 0, 255);
          motorSpeed2 = map(yDir, 50, 10, 0, 255);
        }
        if (yDir > 120) {
          motorSpeed1 = map(yDir, 120, 150, 0, 255);
          motorSpeed2 = map(yDir, 120, 150, 0, 255);
        }
      }
      else {
        if (yDir < 50) {
          forward();
        }
        if (yDir > 120) {
          backward();
        }
        if (xDir < 50) {
          motorSpeed1 = map(xDir, 50, 10, 255, 50);
          motorSpeed2 = 255;
        }
        if (xDir > 120) {
          motorSpeed1 = 255;
          motorSpeed2 = map(xDir, 120, 150, 255, 50);
        }
      }
    }
  }
  //Serial.print(motorSpeed1);
  //Serial.print(",");
  //Serial.println(motorSpeed1);

  analogWrite(en1, motorSpeed1); // Send PWM signal to motor A
  analogWrite(en2, motorSpeed2); // Send PWM signal to motor B
}

void forward() {
  Serial.println("forward");
  digitalWrite(in1, HIGH);
  digitalWrite(in2, LOW);
  digitalWrite(in3, HIGH);
  digitalWrite(in4, LOW);
}
void backward() {
  Serial.println("backward");
  digitalWrite(in1, LOW);
  digitalWrite(in2, HIGH);
  digitalWrite(in3, LOW);
  digitalWrite(in4, HIGH);
}
void turnRight() {
  Serial.println("turnRight");
  digitalWrite(in1, HIGH);
  digitalWrite(in2, LOW);
  digitalWrite(in3, LOW);
  digitalWrite(in4, HIGH);
}
void turnLeft() {
  Serial.println("turnLeft");
  digitalWrite(in1, LOW);
  digitalWrite(in2, HIGH);
  digitalWrite(in3, HIGH);
  digitalWrite(in4, LOW);
}
void Stop() {
  digitalWrite(in1, LOW);
  digitalWrite(in2, LOW);
  digitalWrite(in3, LOW);
  digitalWrite(in4, LOW);
  Serial.println("stop");
}
void curve() {
  Serial.println("curve");
  forward();
  count++;
  if (count >= 200)
  {
    toggleFlag = !toggleFlag;
    count = 0;
  }
  Serial.println(count);
}