-
Notifications
You must be signed in to change notification settings - Fork 2
/
controller.ino
89 lines (80 loc) · 1.97 KB
/
controller.ino
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
#include <WiFi.h>
#include "MPU9250.h"
MPU9250 IMU(Wire,0x68);
int status;
const int weight=10;
float temp_X[2][100];
float temp_Y[2][100];
String enc_val;
char ssid[] = "HelloCar";
char pass[] = "99eleven";
IPAddress server(192,168,4,15);
WiFiClient client;
int ledPin = 2;
void setup()
{
Serial.begin(115200);
for(int i=0;i<weight;i++)
{
temp_X[0][i]=0;
temp_X[1][i]=0;
temp_Y[0][i]=0;
temp_Y[1][i]=0;
}
while(!Serial) {}
status = IMU.begin();
if (status < 0) {
Serial.println("IMU initialization unsuccessful");
Serial.println("Check IMU wiring or try cycling power");
Serial.print("Status: ");
Serial.println(status);
while(1) {}
}
WiFi.mode(WIFI_STA);
WiFi.begin(ssid, pass); // connects to the WiFi AP
Serial.println();
Serial.println("Connection to the AP");
while (WiFi.status() != WL_CONNECTED)
{
Serial.print(".");
delay(500);
}
Serial.println();
Serial.println("Connected");
Serial.print("LocalIP:"); Serial.println(WiFi.localIP());
Serial.println("MAC:" + WiFi.macAddress());
Serial.print("Gateway:"); Serial.println(WiFi.gatewayIP());
Serial.print("AP MAC:"); Serial.println(WiFi.BSSIDstr());
pinMode(ledPin, OUTPUT);
}
void loop()
{
int i;
if(!client.connected()) client.connect(server, 80);
float value_x=0;
float value_y=0;
IMU.readSensor();
for(i=weight-2;i>=0;i--)
{
temp_X[0][i+1]=temp_X[0][i];
temp_X[1][i+1]=temp_X[1][i];
temp_Y[0][i+1]=temp_Y[0][i];
temp_Y[1][i+1]=temp_Y[1][i];
}
temp_X[0][0]=IMU.getAccelX_mss();
temp_X[1][0]=IMU.getGyroX_rads();
temp_Y[0][0]=IMU.getAccelY_mss();
temp_Y[1][0]=IMU.getGyroY_rads();
for( i=0;i<weight;i++)
{
value_x+=0.7*temp_X[0][i]+0.3*temp_X[1][i];
value_y+=0.7*temp_Y[0][i]+0.3*temp_Y[1][i];
}
value_x/=weight;
value_y/=weight;
enc_val=(String)value_x+' '+(String)value_y;
Serial.print(value_x);
Serial.println();
client.println(enc_val);
delay(50);
}