void setup() {
Wire.begin();
Wire.beginTransmission(MPU_addr);
Wire.write(0x6B);
Wire.write(0);
Wire.endTransmission(true);
Serial.begin(115200);
//Set device in STA mode to begin with
WiFi.mode(WIFI_STA);
Serial.println("ESPNow/Basic/Master Example");
// This is the mac address of the Master in Station Mode
Serial.print("STA MAC: "); Serial.println(WiFi.macAddress());
// Init ESPNow with a fallback logic
InitESPNow();
// Once ESPNow is successfully Init, we will register for Send CB to
// get the status of Transmitted packet
esp_now_register_send_cb(OnDataSent);
}
void loop() {
Wire.beginTransmission(MPU_addr);
Wire.write(0x3B);
Wire.endTransmission(false);
Wire.requestFrom(MPU_addr, 14, true);
AcX = Wire.read() << 8 | Wire.read();
AcY = Wire.read() << 8 | Wire.read();
AcZ = Wire.read() << 8 | Wire.read();
int xAng = map(AcX, minVal, maxVal, -180, 180);
int yAng = map(AcY, minVal, maxVal, -180, 180);
int zAng = map(AcZ, minVal, maxVal, -180, 180);
ElevationAngle = RAD_TO_DEG * (atan2(xAng, zAng));
// Using example from: https://www.arduino.cc/en/tutorial/calibration
// Calibrate during the first five seconds
while (millis() < 5000) {
TopLeft = analogRead(PinTopLeft);
BottomLeft = analogRead(PinBottomLeft);
TopRight = analogRead(PinTopRight);
BottomRight = analogRead(PinBottomRight);
// Record the maximum sensor value
if (TopLeft > MaxTopLeft) {
MaxTopLeft = TopLeft;
}
if (BottomLeft > MaxBottomLeft) {
MaxBottomLeft = BottomLeft;
}
if (TopRight > MaxTopRight) {
MaxTopRight = TopRight;
}
if (BottomRight > MaxBottomRight) {
MaxBottomRight = BottomRight;
}
// Record the minimum sensor value
if (TopLeft < MinTopLeft) {
MinTopLeft = TopLeft;
}
if (BottomLeft < MinBottomLeft) {
MinBottomLeft = BottomLeft;
}
if (TopRight < MinTopRight) {
MinTopRight = TopRight;
}
if (BottomRight < MinBottomRight) {
MinBottomRight = BottomRight;
}
}
// Read the sensor
TopLeft = analogRead(PinTopLeft); // Top left sensor
BottomLeft = analogRead(PinBottomLeft);
TopRight = analogRead(PinTopRight);
BottomRight = analogRead(PinBottomRight);
// Apply the calibration to the sensor reading
TopLeft = map(TopLeft, MinTopLeft, MaxTopLeft, 0, 1023);
BottomLeft = map(BottomLeft, MinBottomLeft, MaxBottomLeft, 0, 1023);
TopRight = map(TopRight, MinTopRight, MaxTopRight, 0, 1023);
BottomRight = map(BottomRight, MinBottomRight, MaxBottomRight, 0, 1023);
// In case the sensor value is outside the range seen during calibration
TopLeft = constrain(TopLeft, 0, 1023);
BottomLeft = constrain(BottomLeft, 0, 1023);
TopRight = constrain(TopRight, 0, 1023);
BottomRight = constrain(BottomRight, 0, 1023);
// In the loop we scan for slave
ScanForSlave();
// If Slave is found, it would be populate in `slave` variable
// We will check if `slave` is defined and then we proceed further
if (slave.channel == CHANNEL) { // check if slave channel is defined
// `slave` is defined
// Add slave as peer if it has not been added already
bool isPaired = manageSlave();
if (isPaired) {
// pair success or already paired
// Send data to device
sendData();
} else {
// slave pair failed
Serial.println("Slave pair failed!");
}
}
else {
// No slave found to process
}
delay(5000);
}