Limenitis Reducta
Published © GPL3+

Lotp Robot Dog V2

Teensy, Arduino, Modular, Autonomous, Avoiding Obstacle, Self-Balanced, Pressure Control, Lidar, Gas Sensor, Drone, Gyro, GPS, Wi-Fi, Camera

ExpertFull instructions providedOver 26 days3,779
Lotp Robot Dog V2

Things used in this project

Hardware components

Buzzer 12mm - 3V 30mA - 85dB
×2
5mm Led Green - 400-500 mcd
×50
5mm Led Yellow - 400-500 mcd
×50
5mm Led Blue - 400-500 mcd
×50
5mm Led Red - 400-500 mcd
×50
3.7V 1S 460mAh 1cell Li-Polymer Battery 80C - PX460XT
×1
Logic Level Converter (3.3 V-5 V)
×3
Sharp GP2Y0A21YK Infrared Sensor 10-80 cm
×1
Montage tape - 22 AWG - 15 Meter Red
×2
680uF 4V Capacitor 8.2x7.9mm
×6
1000uF 6.3V Capacitor 12x8mm
×6
Ams1117 3.3v Voltage Regulator Module
×3
100uF 63v Capacitor
×20
Arduino FT232 Module
×2
3x7 Epoxy Double Sided Prototype PCB
×6
SI2302DS, SI2302, SOT-23 Mosfet Transistor
×8
12V 12mm 60 RPM Reductor Micro DC Motor
×1
3.7 V 1S Lipo Battery-Pil 450 mAh 25C - Micro Drone Battery
×1
M3 nut - 10 Pieces
×10
M3 washer - 10 Pieces
×5
M3 8 mm YSB Star Cylindrical Metric Screw - 10 Pieces
×5
M3 6 mm YSB Star Cylindrical Metric Screw - 10 Pieces
×5
12V 40W 3D Printer Heater
×2
CC3D Flight Control Card
×1
Montage tape - 22 AWG - 15 Meter Blue
×2
7.4 V 2S Lipo Battery 1350 mAh 25C
×2
5mm Copper tape 30mt
×1
iMax B3 Compact Lipo (2-3S) Charger
×1
Slotted weight kit
×2
Ceramic Cabled 28dB Active GPS Antenna
×1
3D Printer Fan 12V 40x40 mm
×1
MPU6050 6 Axis Gyro Sensor - GY-521
×4
Wireless NRF24L01 2.4 GHz Transceiver Module - 2.4 GHz
×1
5V Voltage positive booster Regulator
×1
3.7V 590 mAh Li-Polymer Battery
×1
XT60 Plug 60A Li-Po Connector Set
×4
TO126 Silikon Isolator
×6
2.5 V -30V Mini Digital Red Voltmeter
×2
1x40 32mm Male Header
×15
1x40 90C Female Header
×6
2x40 90C Female Header
×6
7.4V 2S1P 2000 mAh (30C) Li-Polymer battery
×2
LM2596 adjustable Voltage dropper power module
×2
DC/DC - AC/DC Converter
×4
Arduino Pro Mini 328 - 3.3 V / 8 MHz (with Header)
×6
Arduino Micro
×1
Radial Ball Bearing 608zz Set Of 4
×4
CP2102 Micro USB 2.0 UART TTL Serial Converter Arduino Module
×1
Passive GPS Antenna uFL 15x15mm 1dBi
×1
Esp8266 3dBİ Wifi Antenna Set
×1
Pro Mini (arduino) 5v/16mhz
×1
2x16 LCD Screen Blue- Qapass
×1
5.8G 48CH 4.3 Inch LCD 480x272 16:9 NTSC/PAL FPV Monitor
×1
800~2200MHz GSM Antenna 2.15-3dBi 48mm Sma Male
×1
NRF24L01+PA+LNA SMA 2.4G Wireless Wi-Fi Module - 1 km
×2
Neo-7m Arduino Shield Mini Gps Module
×2
6x6 12mm Tach Buton (4 legs)
×10
KTS102 On / Off 3 Legs Toggle Key
×10
M6 X 26mm Steel Barrel - Teflon Pipe
×3
Aluminum Heater Block- MK7 MK8
×3
3D Printer Nozzle 0.3 mm 1.75mm
×3
Filament 1.75 mm Torques PLA - ABG
×3
Eachine TX01 NTSC Super Mini AIO 5.8G 40CH 25MW VTX 600TVL 1/4 Cmos FPV Camera
×2
TP4056 3.7 V 1S Lipo Charger 5 V 1 A Lithium Battery Charger
×1
820 Coreless DC Motor (Mini Drone Motor) X 2
×3
1x40 12mm male Header
×3
2.54mm 0.1" 15-pin Female Header
×8
6V 30Rpm 15mm Reductor Dc Motor
×1
Rp-l-400 Thin Film Pressure Sensor
×2
Filament 1.75 mm Black PLA - ABG
×2
MQ Series Gas Sensor Card PL-1479
×1
Methane Cng Gas Sensor - Mq-4
×1
Carbon Monoxide Sensor (mq7)
×1
MG958 X-Large Digital Servo Motor
×13
25T Servo Motor Metal Header 14 mm Aluminum
×8
Pro 3 Axis Joystick
×2
Grove - Time Of Flight Distance Sensor(vl53l0x)
×1
25T Servo Motor Arm (Black)
×6
Single Side Copper Prototype Hole PCB
×3
Teensy 3.5
×1
Rp-l-170 Thin Film Pressure Sensor
×2
Mpu-9250 9-Axis Gyro Magnetometer
×1

Software apps and online services

Fritzing
Ultimaker Cura
Fusion 360
Autodesk Fusion 360

Hand tools and fabrication machines

Anet A8 (3D Printer)

Story

Read more

Custom parts and enclosures

1 Cube Module

File missing, please reupload.

2 Cube Module

File missing, please reupload.

Balance Stand

Battary Module

Computer Module

Controller

Drone Module Case

Drone Module

Front Left Leg

Gas Module

Lidar Module

LOTP Robodog Assembled

Main Body

Regulator Module

Computer Unit Main Program

List of Hardware Components

Project Performance Values

Project Document (in Turkish)

Attachments

Schematics

Drone Case 0

File missing, please reupload.

Drone Case 1

File missing, please reupload.

Lidar

File missing, please reupload.

Gas_Module

File missing, please reupload.

Main Distrubute Board

File missing, please reupload.

Regulator Mobule

File missing, please reupload.

Computer Unit Main Program

Controller Flow Chart

Drone Module

Gas Module Flow Chart

Lidar Module

Movements

Computer Unit Circuit Schema

Main Infrastructure Circuit Schema

Gas Module Circuit Schema

Drone Station Circuit Schema

Lidar Module Circuit Schema

Remote Controller Circuit Schema

Regulator Unit Circuit Schema

Computer Block

File missing, please reupload.

Controller

File missing, please reupload.

Code

LOTP_Drone_Module.ino

Arduino
#include <Wire.h>

int Adress = 3;
bool turn = 0;
bool oldTurn = 3;

void setup() {
  Serial.begin(9600);
  Wire.begin(Adress);

  Wire.onReceive(receiveEvent);

  for(int i = 0; i < 3; i++){
    pinMode(i+11, OUTPUT);
  }
  pinMode(10, INPUT);
}

void loop() {
  if(oldTurn != turn){
    if(turn){
      digitalWrite(13,1);
      digitalWrite(11,1);
      delay(25*1000);
      digitalWrite(11,0);
    }
    else{
      digitalWrite(13,0);
      while(!digitalRead(10)){
        digitalWrite(12,1);
      }
      digitalWrite(12,0);
    }
    oldTurn = turn;
  }
}

void receiveEvent() {
  while(Wire.available()) {
    turn = Wire.read();
  }
  Serial.println("Rec");
}

LOTP_Gas_Senror.ino

Arduino
#include <MQUnifiedsensor.h>
#include <Wire.h>

byte Package[5];
int Adress = 5;
bool turn = 0;
int req = 0;
double lastReq = 0;
double lastBuzz = 0;
int Buzz = 12;
int Red = 10;
int Green = 11;
int danger = 0;

union {
   byte array[4];
   float bigNum;
} H2;

union {
   byte array[4];
   float bigNum;
} LPG;

union {
   byte array[4];
   float bigNum;
} CH4;

union {
   byte array[4];
   float bigNum;
} CO;

union {
   byte array[4];
   float bigNum;
} Alcohol;

MQUnifiedsensor MQ7("Arduino Pro Mini", 5, 10, A0, "MQ-7");

void setup() {
  Serial.begin(9600);
  Wire.begin(Adress);

  Wire.onReceive(receiveEvent);
  Wire.onRequest(requestEvent);

  MQ7.setRegressionMethod(1);
  MQ7.init(); 

  for(int i = 0; i < 3; i++){
    pinMode(i+10, OUTPUT);
  }

  Serial.print("Calibrating please wait.");
  float calcR0 = 0;
  for(int i = 1; i<=10; i ++)
  {
    delay(500)
    MQ7.update(); // Update data, the arduino will be read the voltage on the analog pin
    calcR0 += MQ7.calibrate(27.5);
    Serial.print(".");
  }
  MQ7.setR0(calcR0/10); 
  Serial.println("  done!.");

  if(isinf(calcR0)) {Serial.println("Warning: Conection issue founded, R0 is infite (Open circuit detected) please check your wiring and supply"); while(1);}
  if(calcR0 == 0){Serial.println("Warning: Conection issue founded, R0 is zero (Analog pin with short circuit to ground) please check your wiring and supply"); while(1);}
}

void loop() {
  danger = 0;
  MQ7.update();
  digitalWrite(Green, turn);
  
  if(millis() - lastReq > 100){
    req = 0;
  }
  
  if(turn){    
    MQ7.setA(69.014); MQ7.setB(-1.374);
    H2.bigNum = MQ7.readSensor(); //H2
    //if(H2.bigNum > 100){danger ++;}
    //Serial.print(H2.bigNum);
    //Serial.print(" H2 ");
 
    MQ7.setA(700000000); MQ7.setB(-7.703);
    LPG.bigNum = MQ7.readSensor(); //LPG
    //if(LPG.bigNum > 100000){danger ++;}
    //Serial.print(LPG.bigNum);
    //Serial.print( " LPG ");

    MQ7.setA(60000000000000); MQ7.setB(-10.54);
    CH4.bigNum = MQ7.readSensor(); //CH4
    if(CH4.bigNum > 10000){danger ++;}
    //Serial.print(CH4.bigNum);
    //Serial.print(" CH4 ");

    MQ7.setA(99.042); MQ7.setB(-1.518);
    CO.bigNum = MQ7.readSensor(); //CO
    if(CO.bigNum > 50){danger ++;}
    //Serial.print(CO.bigNum);
    //Serial.print( " CO ");

    MQ7.setA(40000000000000000); MQ7.setB(-12.35);
    Alcohol.bigNum = MQ7.readSensor(); //Alcohol
    //if(Alcohol.bigNum > 50000){danger ++;}
    //Serial.print(Alcohol.bigNum);
    //Serial.print(" Alcohol ");
    //Serial.println(" ");
    
  }
  if(danger > 0){
    if(millis() - lastBuzz < 250){
      digitalWrite(Buzz, 1);
    }
    else{
      digitalWrite(Buzz, 0);
    }
    if(millis() - lastBuzz > 500){
      lastBuzz = millis();
    }
    digitalWrite(Red, 1);
  }
  else{
    digitalWrite(Buzz, 0);
    digitalWrite(Red, 0);
  }
  
}

void receiveEvent() {
  while(Wire.available()) {
    turn = Wire.read();
  }
  Serial.println("Rec");
}
 
void requestEvent() {
  lastReq = millis();
  if(req == 0){
    for(int i = 0; i < 4; i++){
      Wire.write(H2.array[i]);
    }
    req = 1;
  }
  else if(req == 1){
    for(int i = 0; i < 4; i++){
      Wire.write(LPG.array[i]);
    }
    req = 2;
  }
  else if(req == 2){
    for(int i = 0; i < 4; i++){
      Wire.write(CH4.array[i]);
    }
    req = 3;
  }
  else if(req == 3){
    for(int i = 0; i < 4; i++){
      Wire.write(CO.array[i]);
    }
    req = 4;
  }
  else if(req == 4){
    for(int i = 0; i < 4; i++){
      Wire.write(Alcohol.array[i]);
    }
    req = 0;
  }
  //Serial.print(req);
  //Serial.println("Req");
}

LOTP_RoboDog.ino

Arduino
#include <SoftwareSerial.h>
#include <MPU6050_light.h>
#include <ArduinoJson.h>
#include <TinyGPS++.h>
#include <nRF24L01.h>
#include <Servo.h>
#include <RF24.h>
#include <math.h>
#include <Wire.h>
#include <SPI.h>
#include <SD.h>

int Leg1 = 50;
int Leg2 = 125;
int Leg3 = 125;

SoftwareSerial ss(0, 1); //rx, tx
RF24 radio(7, 8); // CE, CSN
MPU6050 mpu(Wire);
TinyGPSPlus gps;
Servo FLLeg[3];
Servo FRLeg[3];
Servo RLLeg[3];
Servo RRLeg[3];

const uint64_t tAddress = 0xE8E8F0F0E0LL;
const uint64_t rAddress = 0xE8E8F0F0E1LL;

int joystick[6] = {512, 512, 512, 512, 512, 512};
int oldJoystick[6] = {512, 512, 512, 512, 512, 512};
float gyro[3] = {0, 0, 0};
int maxForce[4] = {800, 900, 900, 300};
float FLLegPress;
float FRLegPress;
float RLLegPress;
float RRLegPress;
double gyroLast; 
double limitMove[7];
float Ag[3] = {0, 0, 0};
int curser = 6;
int moveSideA = 0; 
double Sz;
double disLimit[4] = {700, 700, 700, 700};
double LidarForce[4];

int Yin;
int Xin;
int Zin;

double Ax;
double Ay;
double Az;
    
char oldStand;

int LegtoOrg[3] = {150,95,0};

int FLLegPins[3] = {6,25,26};
int FRLegPins[3] = {5,24,4};
int RLLegPins[3] = {31,29,32};
int RRLegPins[3] = {30,28,27};

float FLLegPos[3] = {0,100,0};
float FRLegPos[3] = {0,100,0};
float RLLegPos[3] = {0,100,0};
float RRLegPos[3] = {0,100,0};

//float FLLegPos[3] = {125,125,0};
//float FRLegPos[3] = {125,125,0};
//float RLLegPos[3] = {125,125,0};
//float RRLegPos[3] = {125,125,0};

float FLLegAdd[3] = {0,10,0};
float FRLegAdd[3] = {5,0,0};
float RLLegAdd[3] = {5,15,0};
float RRLegAdd[3] = {0,0,10};

union{
  byte array[4];
  float bigNum;  
} H2;

union{
  byte array[4];
  float bigNum;  
} LPG;

union{
  byte array[4];
  float bigNum;  
} CH4;

union{
  byte array[4];
  float bigNum;  
} CO;

union{
  byte array[4];
  float bigNum;  
} Alchol;

union{
  byte array[4];
  float bigNum;  
} xForce;

union{
  byte array[4];
  float bigNum;  
} xnForce;

union{
  byte array[4];
  float bigNum;  
} yForce;

union{
  byte array[4];
  float bigNum;  
} ynForce;

struct Package{
  int16_t joystick[6];
  bool gpsRequest;
  bool gasRequest;

  bool standMode;
  bool walkMode;

  bool Gps;
  bool Lidar;
  bool Drone;
  bool Gas;

  bool restoreConfig;

  byte joystickSmoothness;
  byte path;
  byte stepH;
  bool gyroAssist;
  bool forceAssist;
  bool WalkMode;
  
  byte lidarSwitch;
  byte droneSwitch;
};

struct GPSPackage{
  byte staller;

  byte hour;
  byte minute;

  int16_t year;
  byte month;
  byte day;

  float longti;
  float lati;
};

struct GasPackage{
  float H2;
  float LPG;
  float CH4;
  float CO;
  float Alchol;
};

struct Config {
  byte joystickSmoothness;
  byte path;
  byte stepH;
  byte gyroAssist;
  byte forceAssist;
  byte lidarSwitch;
  byte droneSwitch;
  bool WalkMode;
};


const char *filename = "/config.txt";  // <- SD library uses 8.3 filenames
Config config;

Package package;
Package oldPackage;

GPSPackage gpsPackage;
GasPackage gasPackage;

int DroneAdress = 3;
int GasAdress = 5;
int LidarAdress = 9;
double lidarTime = 0;
double gasTime = 0;

// |h|h|s|p|p|p|
// |p|p|p|h|h|s|

float hoverTime = 2;
float stepDelay = 1;
float pushTime = hoverTime + stepDelay; 

int maxStepX=  150;
int maxStepZ = 100;
int stepOffset = 0;
int tilt = 10;

float sTime[4];

float W2s[4] = {0, pushTime, pushTime, 0};
float W4s[4] = {0, pushTime, 2 * pushTime, 3 * pushTime};

void setup() {
  Serial.begin(115200);
  ss.begin(9600);
  Wire.begin();
  Wire1.begin();

  //while(!Serial){}
  
  while (!SD.begin(BUILTIN_SDCARD)) {
    Serial.println(F("Failed to initialize SD library"));
    delay(1000);
  }

  mpu.begin();
  Serial.println("MPU6050 Found!");
  
  loadConfiguration(filename, config);

  for(int i = 0; i < 4; i++){
    sTime[i] = W2s[i];
  }

  for(int i = 0; i < 4; i++){
    sTime[i] *= config.path;
  }
  
  for(int i = 0; i < 3; i++){
    FLLeg[i].attach(FLLegPins[i]); 
    FLLeg[i].write(90 + FLLegAdd[i]);
  }
  for(int i = 0; i < 3; i++){
    FRLeg[i].attach(FRLegPins[i]);
    FRLeg[i].write(90 + FRLegAdd[i]);
  }
  for(int i = 0; i < 3; i++){
    RLLeg[i].attach(RLLegPins[i]);
    RLLeg[i].write(90 + RLLegAdd[i]);
  }
  for(int i = 0; i < 3; i++){
    RRLeg[i].attach(RRLegPins[i]);
    RRLeg[i].write(90 + RRLegAdd[i]);
  }

  radio.begin();
  //radio.setPackageRate( RF24_250KBPS );
  radio.openWritingPipe(tAddress);
  radio.openReadingPipe(1,rAddress);
  //radio.setPALevel(RF24_PA_MIN);

  moveLegsPositions();

  //restoreConfig();
  delay(2500);
  mpu.calcOffsets();
  
  radio.startListening();
  delay(1);
}

void loop() {
  //delay(25);

  int oldDrone = package.Drone;
  int oldLidar = package.Lidar;
  int oldGas = package.Gas;
  getPackage();

  //Serial.println(package.WalkMode);
  
  getJoystick();
  
  mpu.update();
  getGyro();

  if(oldLidar != package.Lidar){
    if(package.Lidar){
      Wire1.beginTransmission(LidarAdress);
      Wire1.write(1);
      Wire1.endTransmission();
    }
    else{
      Wire1.beginTransmission(LidarAdress);
      Wire1.write(0);
      Wire1.endTransmission();
    }
  }
  if(package.Lidar and millis() - lidarTime > 500){
    lidarTime = millis();
    getLidar();
  }

  if(oldDrone != package.Drone){
    if(package.Drone){
      Wire1.beginTransmission(DroneAdress);
      Wire1.write(1);
      Wire1.endTransmission();
    }
    else{
      Wire1.beginTransmission(DroneAdress);
      Wire1.write(0);
      Wire1.endTransmission();
    }
  }

  if(oldGas != package.Gas){
    if(package.Gas){
      Wire1.beginTransmission(GasAdress);
      Wire1.write(1);
      Wire1.endTransmission();
    }
    else{
      Wire1.beginTransmission(GasAdress);
      Wire1.write(0);
      Wire1.endTransmission();
    }
  }
  if(package.Gas and millis() - gasTime > 500){
    gasTime = millis();
    getGas();
  }
  if(package.gasRequest){
    delay(1);
    radio.stopListening();
    radio.write(&gasPackage, sizeof(gasPackage));
    //Serial.println("Sended");
    radio.startListening();
    delay(1);
  }
  
  if(package.Gps){
    while(ss.available() > 0){
      if(gps.encode(ss.read())){
        if(gps.location.isValid()){
          //Serial.println(gps.location.lat(),6);
          gpsPackage.lati = gps.location.lat();
          gpsPackage.longti = gps.location.lng();
        }

        if(gps.time.isValid()){
          gpsPackage.hour = gps.time.hour();
          gpsPackage.minute = gps.time.minute();
        }

        if(gps.date.isValid()){
          gpsPackage.year = gps.date.year();
          gpsPackage.month = gps.date.month();
          gpsPackage.day = gps.date.day();
        }

        if(gps.satellites.isValid()){
          //Serial.println(gps.satellites.value());
          gpsPackage.staller = gps.satellites.value();
        }
        
        //for(int i = 0; i < 29; i++){
        //  Serial.print(GPSPackage[i]);
        //}
        //Serial.println("GPS");

      if(package.gpsRequest){
        delay(1);
        radio.stopListening();
        radio.write(&gpsPackage, sizeof(gpsPackage));
        //Serial.println("Sended");
        radio.startListening();
        delay(1);
        }
      }
    }
  }
  
  if(package.standMode and !package.walkMode){
    if(oldStand != package.standMode){
      int Bpos[12];
      for(int i = 0; i < 3; i++){
        Bpos[i] = FLLegPos[i];
      }
      for(int i = 0; i < 3; i++){
        Bpos[i + 3] = FRLegPos[i];
      }
      for(int i = 0; i < 3; i++){
        Bpos[i + 6] = RLLegPos[i];
      }
      for(int i = 0; i < 3; i++){
        Bpos[i + 9] = RRLegPos[i];
      }
      
  
      int sitPoint[3] = {0,175,0};
      int line[3];
      for(int i = 0; i < 100; i++){
        delay(5);
        for(int v = 0; v < 3; v++){
        line[v] = Bpos[v] - sitPoint[v];
  
        FLLegPos[v] = Bpos[v] - line[v] * i / 100 ;
        }
  
        for(int v = 0; v < 3; v++){
        line[v] = Bpos[v + 3] - sitPoint[v];
  
        FRLegPos[v] = Bpos[v + 3] - line[v] * i / 100 ;
        }
  
        for(int v = 0; v < 3; v++){
        line[v] = Bpos[v + 6] - sitPoint[v];
  
        RLLegPos[v] = Bpos[v + 6] - line[v] * i / 100 ;
        }
  
        for(int v = 0; v < 3; v++){
        line[v] = Bpos[v + 9] - sitPoint[v];
  
        RRLegPos[v] = Bpos[v + 9] - line[v] * i / 100 ;
        }
        
        moveLegsPositions();
      }
      oldStand = package.standMode;
    }
    Yin = 200 - map(joystick[5], 0, 1023, 50, 0);
    Xin = map(joystick[4], 0, 1023, -50, 50);
    Zin = map(joystick[3], 0, 1023, -45, 45);

    Ax = map(joystick[0], 0, 1023, -25, 25);
    Ay = map(joystick[1], 0, 1023, -15, 15);
    Az = map(joystick[2], 0, 1023, -15, 15);

    gyro[0] += Ax; 
    gyro[1] += Ay; 
    int p;
    if(millis() - gyroLast and millis() - gyroLast < 200){
      p =  (millis() - gyroLast) * 50;
    }
    else{
      p = 100;
    }
    for(int i = 0; i < 2 and config.gyroAssist; i++){
      
      if(abs(gyro[i]) > 2 and abs(Ag[i] + gyro[i] / p) <= 15) {Ag[i] += gyro[i] / p; gyroLast = millis();}
      
      
    }
    
    Ax += Ag[0];
    Ay += Ag[1];
    Zin += sin(radians(Ag[0])) * Yin; 
    Xin += sin(radians(Ag[1])) * Yin;
    
    standingMove();
    
    if(config.forceAssist){
      if(analogRead(A1) > maxForce[0] and -FLLegPress < 50){
        FLLegPress -= analogRead(A1) / maxForce[0];
      }
      else if(abs(FLLegPress) > 1){
        FLLegPress += 1;
      }
      FLLegPos[1] += FLLegPress;
  
      if(analogRead(A0) > maxForce[1] and -FRLegPress < 50){
        FRLegPress -= analogRead(A0) / maxForce[1];
      }
      else if(abs(FRLegPress) > 1){
        FRLegPress += 1;
      }
      FRLegPos[1] += FRLegPress;
  
      if(analogRead(A3) > maxForce[2] and -RLLegPress < 50){
        RLLegPress -= analogRead(A3) / maxForce[2];
      }
      else if(abs(RLLegPress) > 1){
        RLLegPress += 1;
      }
      RLLegPos[1] += RLLegPress;
  
      if(analogRead(A2) > maxForce[3] and -RRLegPress < 50){
        RRLegPress -= analogRead(A2) / maxForce[3];
      }
      else if(abs(RRLegPress) > 1){
        RRLegPress += 1;
      }
      RRLegPos[1] += RRLegPress;
    }

    moveLegsPositions();
    
  }
  
  if(!package.standMode and oldStand != package.standMode){

    int Bpos[12];
    for(int i = 0; i < 3; i++){
      Bpos[i] = FLLegPos[i];
    }
    for(int i = 0; i < 3; i++){
      Bpos[i + 3] = FRLegPos[i];
    }
    for(int i = 0; i < 3; i++){
      Bpos[i + 6] = RLLegPos[i];
    }
    for(int i = 0; i < 3; i++){
      Bpos[i + 9] = RRLegPos[i];
    }
    

    int sitPoint[3] = {0,100,0};
    int line[3];
    for(int i = 0; i < 100; i++){
      delay(5);
      for(int v = 0; v < 3; v++){
      line[v] = Bpos[v] - sitPoint[v];

      FLLegPos[v] = Bpos[v] - line[v] * i / 100 ;
      }

      for(int v = 0; v < 3; v++){
      line[v] = Bpos[v + 3] - sitPoint[v];

      FRLegPos[v] = Bpos[v + 3] - line[v] * i / 100 ;
      }

      for(int v = 0; v < 3; v++){
      line[v] = Bpos[v + 6] - sitPoint[v];

      RLLegPos[v] = Bpos[v + 6] - line[v] * i / 100 ;
      }

      for(int v = 0; v < 3; v++){
      line[v] = Bpos[v + 9] - sitPoint[v];

      RRLegPos[v] = Bpos[v + 9] - line[v] * i / 100 ;
      }
      
      moveLegsPositions();
    }
  }
  double Sy = 200 - map(joystick[5], 0, 1023, 50, 0);
  
  if(package.standMode and package.walkMode){
    
    double Wx = map(joystick[1], 0, 1023, maxStepX, -maxStepX);
    double Wz = map(joystick[0], 0, 1023, -maxStepZ, maxStepZ);

    double Fz = map(joystick[3], 0, 1023, -maxStepZ, maxStepZ) + Wz;
    double Rz = -Fz + 2*Wz;

    if(package.Lidar){
      Serial.println(xForce.bigNum);
      Serial.println(xnForce.bigNum);
      Serial.println(yForce.bigNum);
      Serial.println(ynForce.bigNum);
      Serial.println(" ");

      if(xForce.bigNum < disLimit[0]){
        Wz += 35;
      }
      if(xnForce.bigNum < disLimit[1]){
        Wz -= 35;
      }
      if(yForce.bigNum < disLimit[2]){
        Wx -= 35;
      }
      if(ynForce.bigNum < disLimit[3]){
        Wx += 35;
      }
    }

    

    if(sqrt(sq(Wx) + sq(Wz)) > 10 or abs(Fz) > 10){
      FLLegPos[0] -= stepOffset;
      FRLegPos[0] -= stepOffset;
      RLLegPos[0] -= stepOffset;
      RRLegPos[0] -= stepOffset;

      FLLegPos[1] -= tilt;
      RLLegPos[1] -= tilt;
      if(0 <= sTime[0] / config.path and sTime[0] / config.path < hoverTime / 2){
        FLLegPos[0] = FLLegPos[0] + ((Wx - FLLegPos[0]) * (1 / ((config.path * (hoverTime / 2)) - sTime[0])));
        FLLegPos[1] = tilt + FLLegPos[1] + ((Sy - config.stepH - FLLegPos[1]) * (1 / ((config.path * (hoverTime / 2)) - sTime[0])));
        FLLegPos[2] = FLLegPos[2] + ((Fz - FLLegPos[2]) * (1 / ((config.path * (hoverTime / 2)) - sTime[0]))); 
      }
      if(hoverTime / 2 <= sTime[0] / config.path and sTime[0] / config.path < hoverTime){
        FLLegPos[0] = Wx;
        FLLegPos[1] = tilt + FLLegPos[1] + ((Sy - FLLegPos[1]) * (1 / ((config.path * hoverTime / 2) - (sTime[0] - (hoverTime / 2) * config.path))));
        FLLegPos[2] = Fz; 
      }
      if(hoverTime <= sTime[0] / config.path and sTime[0] / config.path < 2 * pushTime){
        FLLegPos[0] = FLLegPos[0] + ((-Wx - FLLegPos[0]) * (1 / ((config.path * (pushTime + stepDelay)) - (sTime[0] - hoverTime * config.path))));
        FLLegPos[1] = tilt + FLLegPos[1] + ((Sy - FLLegPos[1]) * (1 / ((config.path * (pushTime + stepDelay)) - (sTime[0] - hoverTime * config.path))));
        FLLegPos[2] = FLLegPos[2] + ((-Fz - FLLegPos[2]) * (1 / ((config.path * (pushTime + stepDelay)) - (sTime[0] - hoverTime * config.path)))); 
      }
  
  
      if(0 <= sTime[1] / config.path and sTime[1] / config.path < hoverTime / 2){
        FRLegPos[0] = FRLegPos[0] + ((Wx - FRLegPos[0]) * (1 / ((config.path * (hoverTime / 2)) - sTime[1])));
        FRLegPos[1] = FRLegPos[1] + ((Sy - config.stepH - FRLegPos[1]) * (1 / ((config.path * (hoverTime / 2)) - sTime[1])));
        FRLegPos[2] = FRLegPos[2] + ((-Fz - FRLegPos[2]) * (1 / ((config.path * (hoverTime / 2)) - sTime[1]))); 
      }
      if(hoverTime / 2 <= sTime[1] / config.path and sTime[1] / config.path < hoverTime){
        FRLegPos[0] = Wx;
        FRLegPos[1] = FRLegPos[1] + ((Sy - FRLegPos[1]) * (1 / ((config.path * hoverTime / 2) - (sTime[1] - (hoverTime / 2) * config.path))));
        FRLegPos[2] = -Fz; 
      }
      if(hoverTime <= sTime[1] / config.path and sTime[1] / config.path < 2 * pushTime){
        FRLegPos[0] = FRLegPos[0] + ((-Wx - FRLegPos[0]) * (1 / ((config.path * (pushTime + stepDelay)) - (sTime[1] - hoverTime * config.path))));
        FRLegPos[1] = FRLegPos[1] + ((Sy - FRLegPos[1]) * (1 / ((config.path * (pushTime + stepDelay)) - (sTime[1] - hoverTime * config.path))));
        FRLegPos[2] = FRLegPos[2] + ((Fz - FRLegPos[2]) * (1 / ((config.path * (pushTime + stepDelay)) - (sTime[1] - hoverTime * config.path)))); 
      }

  
      if(0 <= sTime[2] / config.path and sTime[2] / config.path < hoverTime / 2){
        RLLegPos[0] = RLLegPos[0] + ((Wx - RLLegPos[0]) * (1 / ((config.path * (hoverTime / 2)) - sTime[2])));
        RLLegPos[1] = tilt + RLLegPos[1] + ((Sy - config.stepH - RLLegPos[1]) * (1 / ((config.path * (hoverTime / 2)) - sTime[2])));
        RLLegPos[2] = RLLegPos[2] + ((Rz - RLLegPos[2]) * (1 / ((config.path * (hoverTime / 2)) - sTime[2]))); 
      }
      if(hoverTime / 2 <= sTime[2] / config.path and sTime[2] / config.path < hoverTime){
        RLLegPos[0] = Wx;
        RLLegPos[1] = tilt + RLLegPos[1] + ((Sy - RLLegPos[1]) * (1 / ((config.path * hoverTime / 2) - (sTime[2] - (hoverTime / 2) * config.path))));
        RLLegPos[2] = Rz; 
      }
      if(hoverTime <= sTime[2] / config.path and sTime[2] / config.path < 2 * pushTime){
        RLLegPos[0] = RLLegPos[0] + ((-Wx - RLLegPos[0]) * (1 / ((config.path * (pushTime + stepDelay)) - (sTime[2] - hoverTime * config.path))));
        RLLegPos[1] = tilt + RLLegPos[1] + ((Sy - RLLegPos[1]) * (1 / ((config.path * (pushTime + stepDelay)) - (sTime[2] - hoverTime * config.path))));
        RLLegPos[2] = RLLegPos[2] + ((-Rz - RLLegPos[2]) * (1 / ((config.path * (pushTime + stepDelay)) - (sTime[2] - hoverTime * config.path)))); 
      }
  
      
      if(0 <= sTime[3] / config.path and sTime[3] / config.path < hoverTime / 2){
        RRLegPos[0] = RRLegPos[0] + ((Wx - RRLegPos[0]) * (1 / ((config.path * (hoverTime / 2)) - sTime[3])));
        RRLegPos[1] = RRLegPos[1] + ((Sy - config.stepH - RRLegPos[1]) * (1 / ((config.path * (hoverTime / 2)) - sTime[3])));
        RRLegPos[2] = RRLegPos[2] + ((-Wz - RRLegPos[2]) * (1 / ((config.path * (hoverTime / 2)) - sTime[3]))); 
      }
      if(hoverTime / 2 <= sTime[3] / config.path and sTime[3] / config.path < hoverTime){
        RRLegPos[0] = Wx;
        RRLegPos[1] = RRLegPos[1] + ((Sy - RRLegPos[1]) * (1 / ((config.path * hoverTime / 2) - (sTime[3] - (hoverTime / 2) * config.path))));
        RRLegPos[2] = -Wz; 
      }
      if(hoverTime <= sTime[3] / config.path and sTime[3] / config.path < 2 * pushTime){
        RRLegPos[0] = RRLegPos[0] + ((-Wx - RRLegPos[0]) * (1 / ((config.path * (pushTime + stepDelay)) - (sTime[3] - hoverTime * config.path))));
        RRLegPos[1] = RRLegPos[1] + ((Sy - RRLegPos[1]) * (1 / ((config.path * (pushTime + stepDelay)) - (sTime[3] - hoverTime * config.path))));
        RRLegPos[2] = RRLegPos[2] + ((Wz - RRLegPos[2]) * (1 / ((config.path * (pushTime + stepDelay)) - (sTime[3] - hoverTime * config.path)))); 
      }

      FLLegPos[0] += stepOffset;
      FRLegPos[0] += stepOffset;
      RLLegPos[0] += stepOffset;
      RRLegPos[0] += stepOffset;
      
      for(int i = 0; i < 4; i++){
        if((sTime[i] / config.path >= 2 * pushTime and !package.WalkMode) or (sTime[i] / config.path >= 5 * pushTime and package.WalkMode)){
          sTime[i] = 0;
        }
        else{
          sTime[i] ++;
        }
      }
    }
    else{
      FLLegPos[1] = Sy + tilt;
      FRLegPos[1] = Sy;
      RLLegPos[1] = Sy+ tilt;
      RRLegPos[1] = Sy;
      if(package.WalkMode){
        for(int i = 0; i < 4; i++){
          sTime[i] = W4s[i] * config.path;
        }
      }
      else{
        for(int i = 0; i < 4; i++){
          sTime[i] = W2s[i] * config.path;
        }
      }
    }

    moveLegsPositions();
  }

  oldStand = package.standMode;
}

void moveLegsPositions(){
  //Front Left Leg------------
  double Hz = sqrt(sq(FLLegPos[2] + Leg1) + sq(FLLegPos[1]));
  double Hx = sqrt(sq(FLLegPos[0]) + sq(Hz));

  double a3 = acos( (sq(Leg2) + sq(Leg3) - sq(Hx)) / (2 * Leg2 * Leg3) ) * 57296 / 1000;
  double a2 = 90 + acos( (sq(Hx) + sq(Leg2) - sq(Leg3)) / (2 * Hx * Leg2)) * 57296 / 1000 - asin(FLLegPos[0] / Hx) * 57296 / 1000;
  double a1 = atan((FLLegPos[2] + Leg1) / FLLegPos[1])* 57296 / 1000 - atan(Leg1 / FLLegPos[1])* 57296 / 1000;

  //Serial.println(FLLegPos[0]);
  //Serial.println(Hx);
  //Serial.println(a3);
  
  FLLeg[2].write(map(a3, 35, 135, 180, 0) + FLLegAdd[2]);
  FLLeg[1].write(a2 + FLLegAdd[1]);
  FLLeg[0].write(180 - (90 - a1) + FLLegAdd[0]);

  //Front Right Leg-----------
  Hz = sqrt(sq(FRLegPos[2] + Leg1) + sq(FRLegPos[1]));
  Hx = sqrt(sq(FRLegPos[0]) + sq(Hz));

  a3 = acos( (sq(Leg2)  + sq(Leg3) - sq(Hx)) / (2 * Leg2 * Leg3) ) * 57296 / 1000;
  a2 = 90 + acos( (sq(Hx) + sq(Leg2) - sq(Leg3)) / (2 * Hx * Leg2)) * 57296 / 1000 - asin(FRLegPos[0] / Hx) * 57296 / 1000;
  a1 = atan((FRLegPos[2] + Leg1) / FRLegPos[1])* 57296 / 1000 - atan(Leg1 / FRLegPos[1])* 57296 / 1000;
  
  FRLeg[2].write(map(a3, 35, 135, 0, 180) + FRLegAdd[2]);
  FRLeg[1].write(180 - a2  + FRLegAdd[1]);
  FRLeg[0].write(180 - (90 + a1)  + FRLegAdd[0]);

  //Rear Left Leg------------
  Hz = sqrt(sq(RLLegPos[2] + Leg1) + sq(RLLegPos[1]));
  Hx = sqrt(sq(RLLegPos[0]) + sq(Hz));

  a3 = acos( (sq(Leg2) + sq(Leg3) - sq(Hx)) / (2 * Leg2 * Leg3) ) * 57296 / 1000;
  a2 = 90 + acos( (sq(Hx) + sq(Leg2) - sq(Leg3)) / (2 * Hx * Leg2)) * 57296 / 1000 - asin(RLLegPos[0] / Hx) * 57296 / 1000;
  a1 = atan((RLLegPos[2] + Leg1) / RLLegPos[1])* 57296 / 1000 - atan(Leg1 / RLLegPos[1])* 57296 / 1000;
  
  RLLeg[2].write(map(a3, 35, 135, 180, 0) + RLLegAdd[2]);
  RLLeg[1].write(a2 + RLLegAdd[1]);
  RLLeg[0].write((90 - a1) + RLLegAdd[0]);

  //Rear Right Leg-----------
  Hz = sqrt(sq(RRLegPos[2] + Leg1) + sq(RRLegPos[1]));
  Hx = sqrt(sq(RRLegPos[0]) + sq(Hz));

  a3 = acos( (sq(Leg2) + sq(Leg3) - sq(Hx)) / (2 * Leg2 * Leg3) ) * 57296 / 1000;
  a2 = 90 + acos( (sq(Hx) + sq(Leg2) - sq(Leg3)) / (2 * Hx * Leg2)) * 57296 / 1000 - asin(RRLegPos[0] / Hx) * 57296 / 1000;
  a1 = atan((RRLegPos[2] + Leg1) / RRLegPos[1])* 57296 / 1000 - atan(Leg1 / RRLegPos[1])* 57296 / 1000;
  
  RRLeg[2].write(map(a3, 35, 135, 0, 180) + RRLegAdd[2]);
  RRLeg[1].write(180 - a2 + RRLegAdd[1]);
  RRLeg[0].write((90 + a1) + RRLegAdd[0]);
}

void getPackage(){
  for(int i = 0; i < 5; i++){
    if ( radio.available() ) {
      i = 5;
      radio.read(&package, sizeof(package));
      //Serial.println(package.joystick[1]);
    }
  }
  if(package.restoreConfig){
    setConfig();
  }
}

void getJoystick(){
    
    for(int ax = 0; ax < 6; ax++){
      joystick[ax] = package.joystick[ax];
    }
    
    for(int j = 0; j < 6; j++){
      if(joystick[j] - oldJoystick[j] > config.joystickSmoothness){
        delay(config.joystickSmoothness/10);
        joystick[j] = oldJoystick[j] +config.joystickSmoothness;
      }
      else if(joystick[j] - oldJoystick[j] < -config.joystickSmoothness){
        delay(config.joystickSmoothness/10);
        joystick[j] = oldJoystick[j] -config.joystickSmoothness;
      }
      oldJoystick[j] = joystick[j];
    }
 // for(int ax = 0; ax < 6; ax++){
    //Serial.print(joystick[ax]);
 //   Serial.print(" ");
 // }
  //Serial.println("");

}

void moveTwoLeg(){
  FLLeg[0].write(map(joystick[2],0,1023,45,180));
  FLLeg[1].write(map(joystick[0],0,1023,0,180));
  FLLeg[2].write(map(joystick[1],0,1023,0,180));
  FRLeg[0].write(180-map(joystick[5],0,1023,45,180));
  FRLeg[1].write(map(joystick[3],0,1023,0,180));
  FRLeg[2].write(map(joystick[4],0,1023,0,180));
}

void addToServos(){
  FLLeg[0].write(FLLeg[0].read() + 0);
  FLLeg[1].write(FLLeg[1].read() + 0);
  FLLeg[2].write(FLLeg[2].read() + 0);

  FRLeg[0].write(FRLeg[0].read() + 0);
  FRLeg[1].write(FRLeg[1].read() + 0);
  FRLeg[2].write(FRLeg[2].read() + 0);
  
  RLLeg[0].write(RLLeg[0].read() + 0);
  RLLeg[1].write(RLLeg[1].read() + 0);
  RLLeg[2].write(RLLeg[2].read() + 0);

  RRLeg[0].write(RRLeg[0].read() + 0);
  RRLeg[1].write(RRLeg[1].read() + 0);
  RRLeg[2].write(RRLeg[2].read() + 0);
}

void setLegstoFL(){
  FRLegPos[1] = FLLegPos[1];
  FRLegPos[0] = FLLegPos[0];
  FRLegPos[2] = FLLegPos[2];

  RLLegPos[1] = FLLegPos[1];
  RLLegPos[0] = FLLegPos[0];
  RLLegPos[2] = FLLegPos[2];

  RRLegPos[1] = FLLegPos[1];
  RRLegPos[0] = FLLegPos[0];
  RRLegPos[2] = FLLegPos[2];
}

void loadConfiguration(const char *filename, Config &config) {
  File file = SD.open(filename);
  StaticJsonDocument<512> doc;
  DeserializationError error = deserializeJson(doc, file);
  if (error)
    Serial.println(F("Failed to read file, using default configuration"));

  config.joystickSmoothness = doc["joystickSmoothness"];
  config.path = doc["path"];
  config.stepH = doc["stepH"];
  config.lidarSwitch = doc["lidarSwith"];
  config.gyroAssist = doc["gyroAssist"];
  config.droneSwitch = doc["droneSwith"];
  config.forceAssist = doc["forceAssist"];

  file.close();
}

void restoreConfig(){
  while(!radio.write(&config, sizeof(config))){}
}

void setConfig(){
  config.joystickSmoothness = package.joystickSmoothness;

  for(int i = 0; i < 2; i++){
    sTime[i] /= config.path;
  }
  config.path = package.path;
  if(package.WalkMode){
    for(int i = 0; i < 4; i++){
      sTime[i] = W4s[i];
    }
  }
  else{
    for(int i = 0; i < 4; i++){
      sTime[i] = W2s[i];
    }
  }
  for(int i = 0; i < 2; i++){
    sTime[i] *= config.path;
  }
  config.stepH = package.stepH;
  config.lidarSwitch = package.lidarSwitch;
  config.gyroAssist = package.gyroAssist;
  config.forceAssist = package.forceAssist;
  config.droneSwitch = package.droneSwitch;
  config.WalkMode = package.WalkMode;

  saveConfiguration(filename, config);
}

void saveConfiguration(const char *filename, const Config &config) {
  SD.remove(filename);

  File file = SD.open(filename, FILE_WRITE);
  if (!file) {
    Serial.println(F("Failed to create file"));
    return;
  }
  StaticJsonDocument<256> doc;

  doc["joystickSmoothness"] = config.joystickSmoothness;
  doc["path"] = config.path;
  doc["stepH"] = config.stepH;
  doc["lidarSwith"] = config.lidarSwitch;
  doc["gyroAssist"] = config.gyroAssist;
  doc["droneSwith"] = config.droneSwitch;
  doc["forceAssist"] = config.forceAssist;


  if (serializeJson(doc, file) == 0) {
    Serial.println(F("Failed to write to file"));
  }
  file.close();
}


void getGyro(){
  gyro[0] = -mpu.getAngleX();
  gyro[1] = -mpu.getAngleY();
  gyro[2] = -mpu.getAngleZ();
}

void getTemperature(){
  Serial.println(mpu.getTemp());
}

void getLidar(){
  int i = 0;
  int d = 10;
  Wire1.requestFrom(LidarAdress, 4);
  while(Wire1.available()){
    xForce.array[i] = Wire1.read();
    i++;
  }delay(d);
  
  Wire1.requestFrom(LidarAdress, 4);
  i = 0;
  while(Wire1.available()){
    yForce.array[i] = Wire1.read();
    i++;
  }delay(d);

  Wire1.requestFrom(LidarAdress, 4);
  i = 0;
  while(Wire1.available()){
    xnForce.array[i] = Wire1.read();
    i++;
  }delay(d);
  
  Wire1.requestFrom(LidarAdress, 4);
  i = 0;
  while(Wire1.available()){
    ynForce.array[i] = Wire1.read();
    i++;
  }delay(d);
  //Serial.println("Read");
}

void getGas(){
  Wire1.requestFrom(GasAdress, 4);
  
  int i = 0;
  int d = 5;
  while(Wire1.available()){
    H2.array[i] = Wire1.read();
    i++;
  }delay(d);
  Serial.println(H2.bigNum);

  Wire1.requestFrom(GasAdress, 4);
  i = 0;
  while(Wire1.available()){
    LPG.array[i] = Wire1.read();
    i++;
  }delay(d);
  Serial.println(LPG.bigNum);

  Wire1.requestFrom(GasAdress, 4);
  i = 0;
  while(Wire1.available()){
    CH4.array[i] = Wire1.read();
    i++;
  }delay(d);
  Serial.println(CH4.bigNum);

  Wire1.requestFrom(GasAdress, 4);
  i = 0;
  while(Wire1.available()){
    CO.array[i] = Wire1.read();
    i++;
  }delay(d);
  Serial.println(CO.bigNum);

  Wire1.requestFrom(GasAdress, 4);
  i = 0;
  while(Wire1.available()){
    Alchol.array[i] = Wire1.read();
    i++;
  }delay(d);
  Serial.println(Alchol.bigNum);

  gasPackage.H2 = H2.bigNum;
  gasPackage.LPG = LPG.bigNum;
  gasPackage.CH4 = CH4.bigNum;
  gasPackage.CO = CO.bigNum;
  gasPackage.Alchol = Alchol.bigNum;
}

void standingMove(){
...

This file has been truncated, please download it to see its full contents.

LOTP_Controller.ino

Arduino
#include <LiquidCrystal.h>
#include <SPI.h>
#include <nRF24L01.h>
#include <RF24.h>

RF24 radio(6, 7); // CE, CSN
LiquidCrystal lcd(13, 4, 3, 2, 0, 1);

int joystickZero[6] = {10,10,10,10,10,10};

char joypack[24];
int menuCursor = 0;
int BinMode = 5;
int SinMode = 8;
int Switches[4];
int oldSwitches[4];
int switchPins[4] = {12,11,10,9};
bool Bnew[3] = {1,1,1};
bool bEnter = 0;
int menuLoc[3] = {0,0,0};
int buttonPins[3] = {11,10,9};
int Cstat = 2;
int CurserX = 9;
int menuLength = 4;
bool tStatus = 0;
bool rStatus = 0;
bool recive = 0;
bool doOnes[2] = {0,0};

char configPackage[80];
int packageCharSize = 5;

String menu[4] = {"-GPS     ", "-Modules", "-Settings", "-About  "};
String menuGPS[4] = {"< Back", "-Location", "-Time ", "         "};
String menuSettings[8] = {"< Back      ", "Restore^    ", "J Smoth     ", "Path        ", "Step H      ", "gAssist     ", "fAssist     ", "Walk Mode   "};
String menuModules[4] = {"< Back", "Lidar", "Drone ", "DGM  "};
String menuLidar[4] = {"< Back     ", "Turn Off", "Switch None", "        "};
String menuDrone[4] = {"< Back     ", "Turn Off", "Switch None", "        "};
String menuGas[4] = {"< Back       ", "Alert Off", "Gas Variables", "         "};

String menuAbout[6] = {"< Back   ", "LOTP RoboDog  ", "Prototype", "V2            ", "Made By :", "Halid Yildirim"};

const uint64_t tAddress = 0xE8E8F0F0E1LL;
const uint64_t rAddress = 0xE8E8F0F0E0LL;
const uint64_t tDrone = 0xE8E8F0F1E1LL;

unsigned long pTime = millis();
unsigned long cTime = millis();
unsigned long dTime = 1000;

byte conChar[8] = {
  B00000,
  B01110,
  B10001,
  B00100,
  B01010,
  B00000,
  B00100,
};

byte disconChar[8] = {
  B00000,
  B00000,
  B00000,
  B00000,
  B00000,
  B00000,
  B00100,
};

byte blank[8] = {
  B00000,
  B00000,
  B00000,
  B00000,
  B00000,
  B00000,
  B00000,
};

struct Package{
  int16_t joystick[6];
  bool gpsRequest;
  bool gasRequest;
  
  bool standMode;
  bool walkMode;

  bool Gps;
  bool Lidar;
  bool Drone;
  bool Gas;
  
  bool restoreConfig; 

  byte joystickSmoothness;
  byte path;
  byte stepH;
  bool gyroAssist;
  bool forceAssist;
  bool WalkMode;
  
  byte lidarSwitch;
  byte droneSwitch;
};
 

struct GPSPackage{
  byte staller;

  byte hour;
  byte minute;

  int16_t year;
  byte month;
  byte day;

  float longti;
  float lati;
};

struct GasPackage{
  float H2;
  float LPG;
  float CH4;
  float CO;
  float Alchol;
};

struct Config{
  byte joystickSmoothness;
  byte path;
  byte stepH;
  bool gyroAssist;
  bool forceAssist;
  byte lidarSwitch;
  byte droneSwitch;
};

Package package;

GPSPackage gpsPackage;

GasPackage gasPackage;

Config config;

void setup(){
  Serial.begin(9600);

  pinMode(SinMode, OUTPUT);
  pinMode(BinMode, OUTPUT);

  for(int i = 0; i < 4; i++){
    pinMode(switchPins[i], INPUT);
  }
  
  lcd.begin(16, 2);
  lcd.createChar(1,disconChar);
  lcd.createChar(2,conChar);
  lcd.createChar(0,blank);
  
  radio.begin();
  radio.openWritingPipe(tAddress);
  radio.openReadingPipe(1,rAddress);
  radio.setPALevel(RF24_PA_MIN);
  
  lcd.setCursor(0, 0);
  lcd.print("LOTP RoboDog");
  lcd.setCursor(1, 1);
  lcd.print("V2");

  delay(1500);
  lcd.clear();

  lcd.setCursor(0, 0);
  lcd.print("Searching for");
  lcd.setCursor(0, 1);
  lcd.print("config    Skip <");

  delay(5);
  digitalWrite(BinMode, 1);
  radio.startListening();
  while(!radio.available() and !digitalRead(buttonPins[1])){}
  for(int i = 0; i < 5; i++){
    if(radio.available()){
      lcd.clear();
      radio.read(&config, sizeof(config));
      lcd.setCursor(0, 0);
      lcd.print("Config Recived");
      setConfig();
      delay(750);
      i = 5;
    }
    else{
      setConfigDefault();
      Bnew[1] = 0;
    }
  }
  if(package.gyroAssist){
    menuSettings[5] = "gAssist On  "; 
  }
  else{
    menuSettings[5] = "gAssist Off "; 
  }
  if(package.forceAssist){
    menuSettings[6] = "fAssist On  "; 
  }
  else{
    menuSettings[6] = "fAssist Off "; 
  }
  if(package.WalkMode){
    menuSettings[7] = "Walk Mode 4s"; 
  }
  else{
    menuSettings[7] = "Walk Mode 2s"; 
  }
  
  radio.stopListening();
  digitalWrite(BinMode, 0);
  delay(50);

  
  lcd.clear();
}

void loop(){
  cTime = millis();
  Serial.println(package.WalkMode);
  
  package.restoreConfig = 0;
  
  lcd.setCursor(CurserX, menuCursor%2);
  lcd.write(byte(0));
  
  digitalWrite(BinMode, 1);
  //Up Button
  if(digitalRead(buttonPins[0]) and menuCursor > 0 and Bnew[0]){
    menuCursor--;
    Bnew[0] = 0;
  }
  else if(!digitalRead(buttonPins[0])){
    Bnew[0] = 1;
  }

  //Down Button
  if(digitalRead(buttonPins[2]) and menuCursor < menuLength-1 and Bnew[2]){
    menuCursor++;
    Bnew[2] = 0;
  }
  else if(!digitalRead(buttonPins[2])){
    Bnew[2] = 1;
  }

  //Enter Button
  if(digitalRead(buttonPins[1]) and Bnew[1]){
    Bnew[1] = 0;
    bEnter = 1;
  }
  else if(!digitalRead(buttonPins[1]) and !Bnew[1]){
    lcd.clear();
    updateConnectionStat();
    Bnew[1] = 1;
    bEnter = 0;
  }
  digitalWrite(BinMode, 0);

  if(menuLoc[0] == 0){
    CurserX = 9;
    mainMenu();
    menuLength = 4;
  }
  
  if(menuLoc[0] == 1){
    package.Gps = 1;
    recive = 1;
    if(menuLoc[1] == 0){
      CurserX = 9;
      menuLength = 3;
      GPSMenu();
    }
    
    if(menuLoc[1] == 1){
      CurserX = 13;
      LocMenu();
    }
    if(menuLoc[1] == 2){
      CurserX = 13;
      TimeMenu();
    }
    
    if(cTime - pTime > dTime){
      if(cTime - pTime < dTime + 10){
        rStatus = 0 ;
      }
      package.gpsRequest = 1;
      if(cTime - pTime > dTime + 100){
        pTime = cTime;
      }
    }
    else{
      package.gpsRequest = 0;
    }
  }
  else{
    rStatus = 0;
    package.Gps = 0;
  }
  
  if(menuLoc[0] == 2){
    if(menuLoc[1] == 0){
      CurserX = 10;
      menuLength = 4;
      modulesMenu();
    }

    if(menuLoc[1] == 1){
      CurserX = 11;
      menuLength = 3;
      lidarMenu();
    }
    if(menuLoc[1] == 2){
      CurserX = 11;
      menuLength = 3;
      droneMenu();
    }
    if(menuLoc[1] == 3){
      CurserX = 13;
      menuLength = 3;
      if(menuLoc[2] == 0){
        gasMenu();
      }
      if(menuLoc[2] == 1){
        gasVarMenu();
      }
      
    }
    
  }

  if(package.Gas){
    gasAlert();
    if(cTime - pTime > dTime){
      if(cTime - pTime < dTime + 10){
        rStatus = 0 ;
      }
      package.gasRequest = 1;
      if(cTime - pTime > dTime + 100){
        pTime = cTime;
      }
    }
    else{
      package.gasRequest = 0;
    }
  }

  if(menuLoc[0] == 3){
    
    if(menuLoc[1] == 0){
      CurserX = 12;
      menuLength = 8;
      
      settingsMenu();
    }
    if(menuLoc[1] == 1){
      RestoreConfig();
      menuLoc[1] = 0;
      menuCursor = 1;
      bEnter = 0;
    }
    
    if(menuLoc[1] == 2){
      menuLength = 512;
      if(doOnes[0] == 0){
        doOnes[0] = 1;
        menuCursor = -package.joystickSmoothness + 256;
      }
      lcd.setCursor(7, 0);
      lcd.print("  ");
      lcd.setCursor(7, 0);
      lcd.print(-menuCursor + 256);
      if(bEnter == 1){
        lcd.setCursor(7, 0);
        lcd.print("  ");
        package.joystickSmoothness = -menuCursor + 256;
        menuLoc[1] = 0;
        menuCursor = 2;
        bEnter = 0;
        doOnes[0] = 0;
      }
    }
    if(menuLoc[1] == 3){
      menuLength = 512;
      if(doOnes[0] == 0){
        doOnes[0] = 1;
        menuCursor = -package.path + 256;
      }
      lcd.setCursor(7, 0);
      lcd.print("  ");
      lcd.setCursor(7, 0);
      lcd.print(-menuCursor + 256);
      if(bEnter == 1){
        lcd.setCursor(7, 0);
        lcd.print("  ");
        package.path = -menuCursor + 256;
        menuLoc[1] = 0;
        menuCursor = 3;
        bEnter = 0;
        doOnes[0] = 0;
      }
    }
    if(menuLoc[1] == 4){
      menuLength = 512;
      if(doOnes[0] == 0){
        doOnes[0] = 1;
        menuCursor = -package.stepH + 256;
      }
      lcd.setCursor(7, 0);
      lcd.print("  ");
      lcd.setCursor(7, 0);
      lcd.print(-menuCursor + 256);
      if(bEnter == 1){
        lcd.setCursor(7, 0);
        lcd.print("  ");
        package.stepH = -menuCursor + 256;
        menuLoc[1] = 0;
        menuCursor = 4;
        bEnter = 0;
        doOnes[0] = 0;
      }
    }
    if(menuLoc[1] == 5){
      menuLength = 1;
      if(package.gyroAssist){
        package.gyroAssist = 0;
        menuSettings[5] = "gAssist Off "; 
      }
      else{
        package.gyroAssist = 1;
        menuSettings[5] = "gAssist On  "; 
      }
      menuLoc[1] = 0;
      menuCursor = 5;
      bEnter = 0;
      doOnes[0] = 0;
    }
    if(menuLoc[1] == 6){
      menuLength = 1;
      if(package.forceAssist){
        package.forceAssist = 0;
        menuSettings[6] = "fAssist Off "; 
      }
      else{
        package.forceAssist = 1;
        menuSettings[6] = "fAssist On  "; 
      }
      menuLoc[1] = 0;
      menuCursor = 6;
      bEnter = 0;
      doOnes[0] = 0;
    }
    if(menuLoc[1] == 7){
      menuLength = 1;
      if(package.WalkMode){
        package.WalkMode = 0;
        menuSettings[7] = "Walk Mode 2s"; 
      }
      else{
        package.WalkMode = 1;
        menuSettings[7] = "Walk Mode 4s"; 
      }
      menuLoc[1] = 0;
      menuCursor = 7;
      bEnter = 0;
      doOnes[0] = 0;
    }
    
    
  }
  
  if(menuLoc[0] == 4){
    CurserX = 12;
    menuLength = 5;
      
    aboutMenu();
  }
  
  //Get joystick and add in to Package
  zipJoystick();

  //Get Switch mode and add in to Package
  getSwitch();
  
  package.standMode = Switches[0];
  package.walkMode = Switches[1];

  if(package.lidarSwitch == 3 and oldSwitches[2] != Switches[2]){
    package.Lidar = Switches[2];
  }
  if(package.lidarSwitch == 4 and oldSwitches[3] != Switches[3]){
    package.Lidar = Switches[3];
  }

  if(package.droneSwitch == 3 and oldSwitches[2] != Switches[2]){
    package.Drone = Switches[2];
  }
  if(package.droneSwitch == 4 and oldSwitches[3] != Switches[3]){
    package.Drone = Switches[3];
  }

  for(int i = 0; i < 4; i++){
    oldSwitches[i] = Switches[i];
  }

  //Send Package and check for connection
  radio.stopListening();
  tStatus = radio.write(&package, sizeof(package));
  if(package.gpsRequest){
    getGpsPackage();
  }
  if(package.gasRequest){
    getGasPackage();
  }
  updateConnectionStat();
}

void zipJoystick(){
  int joystick[6] = {analogRead(A0),analogRead(A1),analogRead(A2),analogRead(A3),analogRead(A4),analogRead(A5)};

  for(int i = 0; i < 6; i++){
    if(abs(joystick[i] - 512) < joystickZero[i]){
      joystick[i] = 512;
    }
    package.joystick[i] = joystick[i];
  }
}

void getSwitch(){
  digitalWrite(SinMode, 1);
  for(int i = 0; i < 4; i++){
    Switches[i] = digitalRead(switchPins[i]);
  }
  digitalWrite(SinMode, 0);
}

void updateConnectionStat(){
  lcd.setCursor(14,0);
  lcd.write("T");
  lcd.setCursor(15,0);
  if(tStatus){
    lcd.write(byte(2));
  }
  else{
    lcd.write(byte(1));
  }
  lcd.setCursor(14,1);
  lcd.write("R");
  lcd.setCursor(15,1);
  if(rStatus){
    lcd.write(byte(2));
  }
  else{
    lcd.write(byte(1));
  }
}

void mainMenu(){
  lcd.setCursor(CurserX, menuCursor%2);
  lcd.write(char(60));
  lcd.setCursor(0, 0);
  lcd.print(menu[int(menuCursor/2)*2]);
  lcd.setCursor(0, 1);
  lcd.print(menu[int(menuCursor/2)*2+1]);

  if(bEnter == 1){
    menuLoc[0] = menuCursor + 1;
    menuCursor = 0;
    bEnter = 0;
  }
}

void GPSMenu(){
  lcd.setCursor(CurserX, menuCursor%2);
  lcd.write(char(60));
  lcd.setCursor(0, 0);
  lcd.print(menuGPS[int(menuCursor/2)*2]);
  lcd.setCursor(0, 1);
  lcd.print(menuGPS[int(menuCursor/2)*2+1]);

  if(bEnter == 1 and menuCursor == 0){
    menuLoc[0] = 0;
    menuLoc[1] = 0;
    menuLoc[2] = 0;
    menuCursor = 0;
    bEnter = 0;
  }
  else if(bEnter == 1){
    menuLoc[1] = menuCursor;
    menuCursor = 0;
    bEnter = 0;
  }
}

void LocMenu(){
  lcd.setCursor(0, 0);
  lcd.print("Lati");
  for(int i = 0; i < 8; i++){
    lcd.setCursor(i + 5, 0);
    lcd.print(gpsPackage.lati);
  }
  
  lcd.setCursor(0, 1);
  lcd.print("Long");
  for(int i = 0; i < 8; i++){
    lcd.setCursor(i + 5, 1);
    lcd.print(gpsPackage.longti);
  }

  if(bEnter == 1){
    menuLoc[1] = 0;
    menuCursor = 1;
    bEnter = 0;
  }
}

void TimeMenu(){
  lcd.setCursor(0, 0);
  lcd.print("Time");

  lcd.setCursor(7, 0);
  lcd.print(gpsPackage.hour);
    
  lcd.setCursor(9, 0);
  lcd.print(":");
  
  lcd.setCursor(10, 0);
  lcd.print(gpsPackage.minute);
  
  
  lcd.setCursor(2, 1);
  lcd.print(gpsPackage.day);
  
  lcd.setCursor(4, 1);
  lcd.print(".");
  
  lcd.setCursor(5, 1);
  lcd.print(gpsPackage.month);

  lcd.setCursor(7, 1);
  lcd.print(".");
  
  lcd.setCursor(8, 1);
  lcd.print(gpsPackage.year);
  

  if(bEnter == 1){
    menuLoc[1] = 0;
    menuCursor = 2;
    bEnter = 0;
  }
}

void aboutMenu(){
  lcd.setCursor(CurserX, menuCursor%2);
  lcd.write(char(60));
  lcd.setCursor(0, 0);
  lcd.print(menuAbout[int(menuCursor/2)*2]);
  lcd.setCursor(0, 1);
  lcd.print(menuAbout[int(menuCursor/2)*2+1]);

  if(bEnter == 1 and menuCursor == 0){
    menuLoc[0] = 0;
    menuLoc[1] = 0;
    menuLoc[2] = 0;
    menuCursor = 3;
    bEnter = 0;
  }
}

void getGpsPackage(){
    delay(1);
    
    radio.startListening();
    for(int i = 0; i < 5; i++){
      if(radio.available()){
        i = 5;
        rStatus = 1;
        radio.read(&gpsPackage, sizeof(gpsPackage));
      }
    }
    delay(1);
}

void getGasPackage(){
    delay(1);
    
    radio.startListening();
    for(int i = 0; i < 5; i++){
      if(radio.available()){
        i = 5;
        rStatus = 1;
        radio.read(&gasPackage, sizeof(gasPackage));
      }
    }
    delay(1);
}

void settingsMenu(){
  lcd.setCursor(CurserX, menuCursor%2);
  lcd.write(char(60));
  lcd.setCursor(0, 0);
  lcd.print(menuSettings[int(menuCursor/2)*2]);
  lcd.setCursor(0, 1);
  lcd.print(menuSettings[int(menuCursor/2)*2+1]);

  if(bEnter == 1 and menuCursor == 0){
    menuLoc[0] = 0;
    menuLoc[1] = 0;
    menuLoc[2] = 0;
    menuCursor = 2;
    bEnter = 0;
  }
  else if(bEnter == 1){
    menuLoc[1] = menuCursor;
    menuCursor = 0;
    bEnter = 0;
  }
}

void restoreConfig(){
  package.restoreConfig = 1;
}

void modulesMenu(){
  lcd.setCursor(CurserX, menuCursor%2);
  lcd.write(char(60));
  lcd.setCursor(0, 0);
  lcd.print(menuModules[int(menuCursor/2)*2]);
  lcd.setCursor(0, 1);
  lcd.print(menuModules[int(menuCursor/2)*2+1]);

  if(bEnter == 1 and menuCursor == 0){
    menuLoc[0] = 0;
    menuLoc[1] = 0;
    menuLoc[2] = 0;
    menuCursor = 1;
    bEnter = 0;
  }
  else if(bEnter == 1){
    menuLoc[1] = menuCursor;
    menuCursor = 0;
    bEnter = 0;
  }
}

void lidarMenu(){
  lcd.setCursor(CurserX, menuCursor%2);
  lcd.write(char(60));
  lcd.setCursor(0, 0);
  lcd.print(menuLidar[int(menuCursor/2)*2]);
  lcd.setCursor(0, 1);
  lcd.print(menuLidar[int(menuCursor/2)*2+1]);

  if(bEnter == 1 and menuCursor == 0){
    menuLoc[1] = 0;
    menuLoc[2] = 0;
    menuCursor = 1;
    bEnter = 0;
  }
  else if(bEnter and menuCursor == 1 and !doOnes[1]){
    doOnes[1] = 1;
    if(!package.Lidar){
      menuLidar[1] = "Turn On ";
      package.Lidar = 1;
    }
    else{
      menuLidar[1] = "Turn Off";
      package.Lidar = 0;
    }
  }
  
  else if(bEnter and menuCursor == 2 and !doOnes[1]){
    doOnes[1] = 1;
    if(package.lidarSwitch == 0){
      menuLidar[2] = "Switch 3   ";
      package.lidarSwitch = 3;
    }
    else if(package.lidarSwitch == 3){
      menuLidar[2] = "Switch 4   ";
      package.lidarSwitch = 4;
    }
    else{
      menuLidar[2] = "Switch None";
      package.lidarSwitch = 0;
    }
  }
  else if(!bEnter and doOnes[1]){
    doOnes[1] = 0;
  }
}

void droneMenu(){
  lcd.setCursor(CurserX, menuCursor%2);
  lcd.write(char(60));
  lcd.setCursor(0, 0);
  lcd.print(menuDrone[int(menuCursor/2)*2]);
  lcd.setCursor(0, 1);
  lcd.print(menuDrone[int(menuCursor/2)*2+1]);

  if(bEnter == 1 and menuCursor == 0){
    menuLoc[1] = 0;
    menuLoc[2] = 0;
    menuCursor = 2;
    bEnter = 0;
  }
  else if(bEnter and menuCursor == 1 and !doOnes[1]){
    doOnes[1] = 1;
    if(!package.Drone){
      menuDrone[1] = "Turn On ";
      package.Drone = 1;
    }
    else{
      menuDrone[1] = "Turn Off";
      package.Drone = 0;
    }
  }
  
  else if(bEnter and menuCursor == 2 and !doOnes[1]){
    doOnes[1] = 1;
    if(package.droneSwitch == 0){
      menuDrone[2] = "Switch 3   ";
      package.droneSwitch = 3;
    }
    else if(package.droneSwitch == 3){
      menuDrone[2] = "Switch 4   ";
      package.droneSwitch = 4;
    }
    else{
      menuDrone[2] = "Switch None";
      package.droneSwitch = 0;
    }
  }
  else if(!bEnter and doOnes[1]){
    doOnes[1] = 0;
  }
}

void gasMenu(){
  lcd.setCursor(CurserX, menuCursor%2);
  lcd.write(char(60));
  lcd.setCursor(0, 0);
  lcd.print(menuGas[int(menuCursor/2)*2]);
  lcd.setCursor(0, 1);
  lcd.print(menuGas[int(menuCursor/2)*2+1]);

  if(bEnter == 1 and menuCursor == 0){
    menuLoc[1] = 0;
    menuLoc[2] = 0;
    menuCursor = 3;
    bEnter = 0;
  }
  else if(bEnter and menuCursor == 1 and !doOnes[1]){
    doOnes[1] = 1;
    if(!package.Gas){
      menuGas[1] = "Alert On ";
      package.Gas = 1;
    }
    else{
      menuGas[1] = "Alert Off";
      package.Gas = 0;
    }
  }
  else if(bEnter and menuCursor == 2 and !doOnes[1]){
    doOnes[1] = 1;
    menuLoc[2] = 1;
    menuCursor = 0;
    bEnter = 0;
  }
  else if(!bEnter and doOnes[1]){
    doOnes[1] = 0;
  }
}

void gasVarMenu(){
  delay(1);
  if(menuCursor == 0){
    lcd.setCursor(7,0);
    lcd.write("       ");
    lcd.setCursor(0,0);
    lcd.write("H2 ");
    lcd.print(gasPackage.H2);
    lcd.setCursor(7,1);
    lcd.write("       ");
    lcd.setCursor(0,1);
    lcd.write("LPG ");
    lcd.print(gasPackage.LPG);
  }
  if(menuCursor == 1){
    lcd.setCursor(7,0);
    lcd.write("       ");
    lcd.setCursor(0,0);
    lcd.write("CH4 ");
    lcd.print(gasPackage.CH4);
    lcd.setCursor(7,1);
    lcd.write("       ");
    lcd.setCursor(0,1);
    lcd.write("CO ");
    lcd.print(gasPackage.CO);
  }
  if(menuCursor == 2){
    lcd.setCursor(7,0);
    lcd.write("       ");
    lcd.setCursor(0,0);
    lcd.write("Alchol ");
    lcd.setCursor(0,1);
    lcd.write("              ");
    lcd.setCursor(0,1);
    lcd.print(gasPackage.Alchol);
  }
  
  if(bEnter == 1){
    menuLoc[2] = 0;
    menuCursor = 2;
    bEnter = 0;
  }
}

void RestoreConfig(){
  package.restoreConfig = 1;
}

void setConfig(){
  package.joystickSmoothness = config.joystickSmoothness;
  package.path = config.path;
  package.stepH = config.stepH;
  package.gyroAssist = config.gyroAssist;
  package.forceAssist = config.forceAssist;
  package.lidarSwitch = config.lidarSwitch;
  package.droneSwitch = config.droneSwitch;
}

void gasAlert(){
  bool BCH4 = gasPackage.CH4 > 10000;
  bool BCO = gasPackage.CO > 50;
  if(BCO or BCH4){
    lcd.clear();
    delay(1);
    if(BCO){
      lcd.setCursor(0,0);
      lcd.print("CO Danger!!");
    }
    else if(BCH4){
      lcd.setCursor(0,0);
      lcd.print("CH4 Danger!!");
    }
    
  }
}

void setConfigDefault(){
  package.joystickSmoothness = 4;
  package.path = 45;
  package.stepH = 75;
  package.gyroAssist = 1;
  package.forceAssist = 1;
  package.lidarSwitch = 0;
  package.droneSwitch = 0;
}

LOTP_Lidar.ino

Arduino
#include <VL53L0X.h>
#include <Wire.h>

double pastTime = 0;
bool R;
float Map[200];
float Sides[8];
float Force[4];
int req = 0;
double lastReq = 0;
int d = 0;
bool turn = 0;

union {
   byte array[4];
   float bigNum;
} xPackage;

union {
   byte array[4];
   float bigNum;
} xnPackage;

union {
   byte array[4];
   float bigNum;
} yPackage;

union {
   byte array[4];
   float bigNum;
} ynPackage;

int Adress = 9;

VL53L0X sensor;

void setup() {
  //Serial.begin(9600);
  Wire.begin(Adress);

  sensor.init();
  
  pinMode(13, OUTPUT);
  pinMode(12, INPUT);
  Wire.onReceive(receiveEvent);
  Wire.onRequest(requestEvent);

  sensor.startContinuous();
}

void loop() {
  R = digitalRead(12);
  digitalWrite(13, turn);

  if(millis() - lastReq > 200){
    req = 0;
  }

  if(turn){
    Map[d] = sensor.readRangeContinuousMillimeters();
    d++;
    
    if((R and millis() - pastTime > 500) or d == 200){
      //Serial.println(d);
      pastTime = millis();
      
      for(int i = 0; i < 8; i++){
        for(int p = 0; p < d/8; p++){
          Sides[i] += Map[p + (d/8) * i];
        } 
        Sides[i] /= d/8;
        //Serial.print(Sides[i]);
        //Serial.print(" ");
      }
      //Serial.println("");

      for(int i = 0; i < 4; i++){
        Force[i] = 0;
      }

      Force[2] += Sides[3];
      Force[2] += Sides[4];
      Force[3] += Sides[0];
      Force[3] += Sides[7];

      Force[0] += Sides[5];
      Force[0] += Sides[6];
      Force[1] += Sides[1];
      Force[1] += Sides[2];

      for(int i = 0; i < 4; i++){
        Force[i] = Force[i] / 2;
      }

      //Serial.print(Force[0]);
      //Serial.print(" ");
      //Serial.print(Force[1]);
      //Serial.println(" ");

      xPackage.bigNum = Force[0];
      xnPackage.bigNum = Force[1];
      yPackage.bigNum = Force[2];
      ynPackage.bigNum = Force[3];
      
      d = 0;
      
    }
  }
  
}

void receiveEvent() {
  while(Wire.available()) {
    turn = Wire.read();
  }
}
 
void requestEvent() {
  lastReq = millis();
  if(req == 0){
    for(int i = 0; i < 4; i++){
      Wire.write(xPackage.array[i]);
    }
    req = 1;
  }
  else if(req == 1){
    for(int i = 0; i < 4; i++){
      Wire.write(yPackage.array[i]);
    }
    req = 2;
  }
  else if(req == 2){
    for(int i = 0; i < 4; i++){
      Wire.write(xnPackage.array[i]);
    }
    req = 3;
  }
  else if(req == 3){
    for(int i = 0; i < 4; i++){
      Wire.write(ynPackage.array[i]);
    }
    req = 0;
  }
}

Credits

Limenitis Reducta

Limenitis Reducta

3 projects • 8 followers
I am a 20 years old "Computer Engineering" student. I am working on Robotic projects, Microprocessor Design and Software projects.

Comments